Revision 16:74a6531350b5, committed 2012-10-31
- Comitter:
- maetugr
- Date:
- Wed Oct 31 14:44:07 2012 +0000
- Parent:
- 15:753c5d6a63b3
- Child:
- 17:e096e85f6c36
- Commit message:
- nach Kompassumstellung auf I2C_Sensor (nicht getestet)
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Alt/BMP085.cpp Wed Oct 31 14:44:07 2012 +0000
@@ -0,0 +1,9 @@
+#include "mbed.h"
+#include "BMP085.h"
+
+BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS)
+{
+ // initialize BMP085 with settings
+ //writeRegister(0xf4, 0x2e); // TODO: was macht das + register in header!
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Alt/BMP085.h Wed Oct 31 14:44:07 2012 +0000
@@ -0,0 +1,35 @@
+#ifndef BMP085_H
+#define BMP085_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+// I2C address
+#define BMP085_I2C_ADDRESS 0xEE
+
+class BMP085 : public I2C_Sensor
+{
+ public:
+ BMP085(PinName sda, PinName scl);
+
+ float data[3];
+
+ void read();
+
+ void calibrate(int s);
+
+ float get_angle();
+
+ private:
+ // raw data and function to measure it
+ int raw[3];
+ void readraw();
+
+ // calibration parameters and their saving
+ int Min[3];
+ int Max[3];
+ float scale[3];
+ float offset[3];
+};
+
+#endif
--- a/Sensors/BMP085/BMP085.cpp Mon Oct 29 16:43:10 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,9 +0,0 @@
-#include "mbed.h"
-#include "BMP085.h"
-
-BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS)
-{
- // initialize BMP085 with settings
- //writeRegister(0xf4, 0x2e); // TODO: was macht das + register in header!
-
-}
\ No newline at end of file
--- a/Sensors/BMP085/BMP085.h Mon Oct 29 16:43:10 2012 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,34 +0,0 @@
-#ifndef BMP085_H
-#define BMP085_H
-
-#include "I2C_Sensor.h"
-
-// I2C address
-#define BMP085_I2C_ADDRESS 0xEE
-
-class BMP085 : public I2C_Sensor
-{
- public:
- BMP085(PinName sda, PinName scl);
-
- float data[3];
-
- void read();
-
- void calibrate(int s);
-
- float get_angle();
-
- private:
- // raw data and function to measure it
- int raw[3];
- void readraw();
-
- // calibration parameters and their saving
- int Min[3];
- int Max[3];
- float scale[3];
- float offset[3];
-};
-
-#endif
--- a/Sensors/Gyro/L3G4200D.cpp Mon Oct 29 16:43:10 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp Wed Oct 31 14:44:07 2012 +0000
@@ -2,96 +2,69 @@
#include "L3G4200D.h"
#include <math.h>
-#define L3G4200D_I2C_ADDRESS 0xD0
-
+#define L3G4200D_I2C_ADDRESS 0xD0 // TODO: Adressen???
-L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl)
+L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS)
{
- i2c.frequency(400000);
// Turns on the L3G4200D's gyro and places it in normal mode.
- // Normal power mode, all axes enabled
+ // Normal power mode, all axes enabled (for detailed info see datasheet)
- //writeReg(L3G4200D_CTRL_REG2, 0x05); // control filter
- writeReg(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled
- writeReg(L3G4200D_CTRL_REG3, 0x00);
- writeReg(L3G4200D_CTRL_REG4, 0x20); // acuracy 2000 dps
+ //writeRegister(L3G4200D_CTRL_REG2, 0x05); // control filter
+ writeRegister(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled
+ writeRegister(L3G4200D_CTRL_REG3, 0x00);
+ writeRegister(L3G4200D_CTRL_REG4, 0x20); // sets acuracy to 2000 dps (degree per second)
+
+ writeRegister(L3G4200D_REFERENCE, 0x00);
+ //writeRegister(L3G4200D_STATUS_REG, 0x0F);
- writeReg(L3G4200D_REFERENCE, 0x00);
- //writeReg(L3G4200D_STATUS_REG, 0x0F);
- writeReg(L3G4200D_INT1_THS_XH, 0x2C);
- writeReg(L3G4200D_INT1_THS_XL, 0xA4);
- writeReg(L3G4200D_INT1_THS_YH, 0x2C);
- writeReg(L3G4200D_INT1_THS_YL, 0xA4);
- writeReg(L3G4200D_INT1_THS_ZH, 0x2C);
- writeReg(L3G4200D_INT1_THS_ZL, 0xA4);
- //writeReg(L3G4200D_INT1_DURATION, 0x00);
- //writeReg(L3G4200D_CTRL_REG5, 0x12); // Filter einschalten
- //writeReg(L3G4200D_CTRL_REG5, 0x01); // hochpass Filter einschalten
- writeReg(L3G4200D_CTRL_REG5, 0x00); // Filter ausschalten
+ writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF??
+ writeRegister(L3G4200D_INT1_THS_XL, 0xA4);
+ writeRegister(L3G4200D_INT1_THS_YH, 0x2C);
+ writeRegister(L3G4200D_INT1_THS_YL, 0xA4);
+ writeRegister(L3G4200D_INT1_THS_ZH, 0x2C);
+ writeRegister(L3G4200D_INT1_THS_ZL, 0xA4);
+ //writeRegister(L3G4200D_INT1_DURATION, 0x00);
- writeReg(L3G4200D_CTRL_REG1, 0x0F); // Gogo
+ writeRegister(L3G4200D_CTRL_REG5, 0x00); // deactivates the filters (only use one of these options)
+ //writeRegister(L3G4200D_CTRL_REG5, 0x12); // activates both high and low pass filters
+ //writeRegister(L3G4200D_CTRL_REG5, 0x01); // activates high pass filter
- // calibrate gyro with an average of count samples (result to offset)
- #define count 50
+ writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement
+
+ // calibrate gyro with an average of count samples (result of calibration stored in offset[])
for (int j = 0; j < 3; j++)
offset[j] = 0;
- float Gyro_calib[3] = {0,0,0}; // temporary to sum up
+ float Gyro_calib[3] = {0,0,0}; // temporary var for the sum of calibration measurement
- for (int i = 0; i < count; i++) {
+ for (int i = 0; i < 50; i++) { // read 50 times the data in a very short time
read();
for (int j = 0; j < 3; j++)
Gyro_calib[j] += data[j];
- wait(0.001); // maybe less or no wait !!
+ wait(0.001); // TODO: maybe less or no wait !!
}
for (int j = 0; j < 3; j++)
- offset[j] = Gyro_calib[j]/count;
-}
-
-// Writes a gyro register
-void L3G4200D::writeReg(byte reg, byte value)
-{
- byte buffer[2];
- buffer[0] = reg;
- buffer[1] = value;
-
- i2c.write(L3G4200D_I2C_ADDRESS, buffer, 2);
+ offset[j] = Gyro_calib[j]/count; // take the average of the calibration measurements
}
-// Reads a gyro register
-byte L3G4200D::readReg(byte reg)
-{
- byte value = 0;
-
- i2c.write(L3G4200D_I2C_ADDRESS, ®, 1);
- i2c.read(L3G4200D_I2C_ADDRESS, &value, 1);
-
- return value;
-}
-
-// Reads the 3 gyro channels and stores them in vector g
void L3G4200D::read()
{
- byte buffer[6]; // 8-Bit pieces of axis data
- // assert the MSB of the address to get the gyro
- // to do slave-transmit subaddress updating.
+ char buffer[6]; // 8-Bit pieces of axis data
+
buffer[0] = L3G4200D_OUT_X_L | (1 << 7);
- i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1, true);
- i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6, true);
+ readMultiRegister(L3G4200D_OUT_X_L, buffer, 6); // parameter:
- data[0] = (short) (buffer[1] << 8 | buffer[0]);
+ data[0] = (short) (buffer[1] << 8 | buffer[0]); // compose 8-Bit pieces to 16-bit short integers
data[1] = (short) (buffer[3] << 8 | buffer[2]);
data[2] = (short) (buffer[5] << 8 | buffer[4]);
- //with offset of calibration
for (int j = 0; j < 3; j++)
- data[j] -= offset[j];
+ data[j] -= offset[j]; // add offset from calibration
}
-// Reads the gyros Temperature
int L3G4200D::readTemp()
{
- return (short) readReg(L3G4200D_OUT_TEMP);
+ return (short) readRegister(L3G4200D_OUT_TEMP); // read the sensors register for the temperature
}
\ No newline at end of file
--- a/Sensors/Gyro/L3G4200D.h Mon Oct 29 16:43:10 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.h Wed Oct 31 14:44:07 2012 +0000
@@ -4,6 +4,7 @@
#define L3G4200D_H
#include "mbed.h"
+#include "I2C_Sensor.h"
// register addresses
#define L3G4200D_WHO_AM_I 0x0F
@@ -37,21 +38,16 @@
#define L3G4200D_INT1_THS_ZL 0x37
#define L3G4200D_INT1_DURATION 0x38
-typedef char byte;
-
-class L3G4200D
+class L3G4200D : public I2C_Sensor
{
public:
- L3G4200D(PinName sda, PinName scl); // constructor, uses i2c
- void read(); // read all axis to array
- int readTemp(); // read temperature from sensor
- float data[3]; // where the measured data is saved
+ L3G4200D(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
+ void read(); // read all axis to array
+ float data[3]; // where the measured data is saved
+ int readTemp(); // read temperature from sensor
private:
float offset[3]; // offset that's subtracted from every measurement
- I2C i2c; // i2c object to communicate
- void writeReg(byte reg, byte value); // write one single register to sensor
- byte readReg(byte reg); // read one single register from sensor
};
#endif
\ No newline at end of file
--- a/Sensors/I2C_Sensor.cpp Mon Oct 29 16:43:10 2012 +0000
+++ b/Sensors/I2C_Sensor.cpp Wed Oct 31 14:44:07 2012 +0000
@@ -28,26 +28,28 @@
fclose(fp);
}
+//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// ATTENTION!!! the I2C option "repeated" = true is important because otherwise interrupts while bus communications cause crashes (see http://www.i2c-bus.org/repeated-start-condition/)
+//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+
char I2C_Sensor::readRegister(char reg)
{
char value = 0;
- i2c.write(L3G4200D_I2C_ADDRESS, ®, 1, true);
- i2c.read(L3G4200D_I2C_ADDRESS, &value, 1, true);
+ i2c.write(GET_I2C_WRITE_ADDRESS(i2c_address), ®, 1, true);
+ i2c.read(GET_I2C_READ_ADDRESS(i2c_address), &value, 1, true);
return value;
}
void I2C_Sensor::writeRegister(char reg, char data)
{
- char buffer[2]; // TODO: Arraykonstruktion in eine Zeile
- buffer[0] = reg;
- buffer[1] = data;
- i2c.write(GET_I2C_WRITE_ADDRESS(i2c_address), tx, 2, true);
+ char buffer[2] = {reg, data};
+ i2c.write(GET_I2C_WRITE_ADDRESS(i2c_address), buffer, 2, true);
}
void I2C_Sensor::readMultiRegister(char reg, char* output, int size)
-{ // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes (see http://www.i2c-bus.org/repeated-start-condition/)
- i2c.write (GET_I2C_WRITE_ADDRESS(i2c_address), ®, 1, true); //tell it from which register to read
- i2c.read (GET_I2C_READ_ADDRESS(i2c_address), output, size, true); //tell it where to store the data read
+{
+ i2c.write (GET_I2C_WRITE_ADDRESS(i2c_address), ®, 1, true); // tell register address of the MSB get the sensor to do slave-transmit subaddress updating.
+ i2c.read (GET_I2C_READ_ADDRESS(i2c_address), output, size, true); // tell it where to store the data read
}
\ No newline at end of file
--- a/Sensors/I2C_Sensor.h Mon Oct 29 16:43:10 2012 +0000
+++ b/Sensors/I2C_Sensor.h Wed Oct 31 14:44:07 2012 +0000
@@ -11,6 +11,10 @@
void calibrate();
protected:
+ // Calibration value saving
+ void saveCalibrationValues(float values[], int size, char * filename);
+ void loadCalibrationValues(float values[], int size, char * filename);
+
// I2C functions
char readRegister(char reg);
void writeRegister(char reg, char data);
--- a/main.cpp Mon Oct 29 16:43:10 2012 +0000
+++ b/main.cpp Wed Oct 31 14:44:07 2012 +0000
@@ -74,7 +74,7 @@
// calculate new motorspeeds
co = Controller.compute() - 1000;
- if (RC[2].read() > 1100) // zu SICHERHEIT! zum testen (ersetzt armed unarmed)
+ if (RC[2].read() > 1100) // zu SICHERHEIT! zum testen (später ersetzen armed unarmed)
{
/*Motor[0] = RC[2].read()+((RC[0].read() - 1500)/10.0)+40;
Motor[2] = RC[2].read()-((RC[0].read() - 1500)/10.0)-40;*/
@@ -86,14 +86,14 @@
Motor[2] = 1000;
Motor[3] = 1000;
}
- /*Motor[0] = 1000 + (100 - (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000;
+ /*Motor[0] = 1000 + (100 - (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000; // test für erste reaktion der motoren entgegen der Auslenkung
Motor[1] = 1000 + (100 - (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;
Motor[2] = 1000 + (100 + (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000;
Motor[3] = 1000 + (100 + (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;*/
}
int main() { // main programm only used for initialisation and debug output
- NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
+ NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)
//for(int i=0;i<3;i++)
Controller.setInputLimits(-90.0, 90.0);
Matthias Grob
