Revision 40:2ca410923691, committed 2014-02-14
- Comitter:
- maetugr
- Date:
- Fri Feb 14 14:17:32 2014 +0000
- Parent:
- 39:9fd3f4439978
- Commit message:
- now with MPU6050 before taking it too FlyBed2
Changed in this revision
--- a/Sensors/Acc/ADXL345.h Wed Aug 28 00:30:38 2013 +0000
+++ b/Sensors/Acc/ADXL345.h Fri Feb 14 14:17:32 2014 +0000
@@ -53,13 +53,15 @@
{
public:
ADXL345(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
- virtual void read(); // read all axis to array
+ float data[3]; // where the measured data is saved
+ void read(); // read all axis to array
float offset[3]; // offset that's subtracted from every measurement
void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
private:
- virtual void readraw();
+ int raw[3];
+ void readraw();
};
#endif
--- a/Sensors/Alt/BMP085.h Wed Aug 28 00:30:38 2013 +0000
+++ b/Sensors/Alt/BMP085.h Fri Feb 14 14:17:32 2014 +0000
@@ -12,8 +12,8 @@
{
public:
BMP085(PinName sda, PinName scl);
-
- //virtual void read();
+ float data; // where the measured data is saved
+ //void read();
void calibrate(int s);
@@ -21,7 +21,7 @@
private:
// raw data and function to measure it
- int raw[3];
+ int raw;
//void readraw();
// calibration parameters and their saving
--- a/Sensors/Comp/HMC5883.h Wed Aug 28 00:30:38 2013 +0000
+++ b/Sensors/Comp/HMC5883.h Fri Feb 14 14:17:32 2014 +0000
@@ -17,13 +17,14 @@
{
public:
HMC5883(PinName sda, PinName scl);
-
- virtual void read(); // read all axis from register to array data
+ float data[3]; // where the measured data is saved
+ void read(); // read all axis from register to array data
void calibrate(int s);
float get_angle();
private:
- virtual void readraw(); // function to get raw data
+ int raw[3];
+ void readraw(); // function to get raw data
float scale[3]; // calibration parameters
float offset[3];
--- a/Sensors/Gyro/L3G4200D.h Wed Aug 28 00:30:38 2013 +0000
+++ b/Sensors/Gyro/L3G4200D.h Fri Feb 14 14:17:32 2014 +0000
@@ -44,13 +44,15 @@
{
public:
L3G4200D(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
- virtual void read(); // read all axis from register to array data
+ float data[3]; // where the measured data is saved
+ void read(); // read all axis from register to array data
float offset[3]; // offset that's subtracted from every measurement
void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
int readTemp(); // read temperature from sensor
private:
- virtual void readraw();
+ int raw[3];
+ void readraw();
};
#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyro_Acc/MPU6050.cpp Fri Feb 14 14:17:32 2014 +0000
@@ -0,0 +1,82 @@
+#include "MPU6050.h"
+
+MPU6050::MPU6050(PinName sda, PinName scl) : I2C_Sensor(sda, scl, MPU6050_I2C_ADDRESS)
+{
+ // Turns on the MPU6050's gyro and initializes it
+ // register datasheet: http://www.invensense.com/mems/gyro/documents/RM-MPU-6000A.pdf
+ writeRegister(MPU6050_RA_PWR_MGMT_1, 0x01); // wake up from sleep and chooses Gyro X-Axis as Clock source (stadard sleeping and with inacurate clock is 0x40)
+ writeRegister(MPU6050_RA_GYRO_CONFIG, 0x18); // scales gyros range to +-2000dps
+ writeRegister(MPU6050_RA_ACCEL_CONFIG, 0x00); // scales accelerometers range to +-2g
+}
+
+void MPU6050::read()
+{
+ readraw_gyro(); // read raw measurement data
+ readraw_acc();
+
+ for (int i = 0; i < 3; i++)
+ data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10)
+
+ for (int i = 0; i < 3; i++)
+ data_acc[i] = raw_acc[i] - offset_acc[i]; // TODO: didn't care about units because IMU-algorithm just uses vector direction
+}
+
+int MPU6050::readTemp()
+{
+ char buffer[2]; // 8-Bit pieces of temperature data
+
+ readMultiRegister(MPU6050_RA_TEMP_OUT_H, buffer, 2); // read the sensors register for the temperature
+ return (short) (buffer[0] << 8 | buffer[1]);
+}
+
+void MPU6050::readraw_gyro()
+{
+ char buffer[6]; // 8-Bit pieces of axis data
+
+ readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
+
+ raw_gyro[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
+ raw_gyro[1] = (short) (buffer[2] << 8 | buffer[3]);
+ raw_gyro[2] = (short) (buffer[4] << 8 | buffer[5]);
+}
+
+void MPU6050::readraw_acc()
+{
+ char buffer[6]; // 8-Bit pieces of axis data
+
+ readMultiRegister(MPU6050_RA_ACCEL_XOUT_H | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
+
+ raw_acc[0] = (short) (buffer[0] << 8 | buffer[1]); // join 8-Bit pieces to 16-bit short integers
+ raw_acc[1] = (short) (buffer[2] << 8 | buffer[3]);
+ raw_acc[2] = (short) (buffer[4] << 8 | buffer[5]);
+}
+
+void MPU6050::calibrate(int times, float separation_time)
+{
+ // calibrate sensor with an average of count samples (result of calibration stored in offset[])
+ // Calibrate Gyroscope ----------------------------------
+ float calib_gyro[3] = {0,0,0}; // temporary array for the sum of calibration measurement
+
+ for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
+ readraw_gyro();
+ for (int j = 0; j < 3; j++)
+ calib_gyro[j] += raw_gyro[j];
+ wait(separation_time);
+ }
+
+ for (int i = 0; i < 3; i++)
+ offset_gyro[i] = calib_gyro[i]/times; // take the average of the calibration measurements
+
+ // Calibrate Accelerometer -------------------------------
+ float calib_acc[3] = {0,0,0}; // temporary array for the sum of calibration measurement
+
+ for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
+ readraw_acc();
+ for (int j = 0; j < 3; j++)
+ calib_acc[j] += raw_acc[j];
+ wait(separation_time);
+ }
+
+ for (int i = 0; i < 2; i++)
+ offset_acc[i] = calib_acc[i]/times; // take the average of the calibration measurements
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyro_Acc/MPU6050.h Fri Feb 14 14:17:32 2014 +0000
@@ -0,0 +1,144 @@
+// based on http://mbed.org/users/garfieldsg/code/MPU6050/
+
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
+#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
+#define MPU6050_I2C_ADDRESS 0xD0 // adresses above multiplied by 2
+
+// register addresses
+#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
+#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
+#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
+#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
+#define MPU6050_RA_XA_OFFS_L_TC 0x07
+#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
+#define MPU6050_RA_YA_OFFS_L_TC 0x09
+#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
+#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
+#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
+#define MPU6050_RA_XG_OFFS_USRL 0x14
+#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
+#define MPU6050_RA_YG_OFFS_USRL 0x16
+#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
+#define MPU6050_RA_ZG_OFFS_USRL 0x18
+#define MPU6050_RA_SMPLRT_DIV 0x19
+#define MPU6050_RA_CONFIG 0x1A
+#define MPU6050_RA_GYRO_CONFIG 0x1B
+#define MPU6050_RA_ACCEL_CONFIG 0x1C
+#define MPU6050_RA_FF_THR 0x1D
+#define MPU6050_RA_FF_DUR 0x1E
+#define MPU6050_RA_MOT_THR 0x1F
+#define MPU6050_RA_MOT_DUR 0x20
+#define MPU6050_RA_ZRMOT_THR 0x21
+#define MPU6050_RA_ZRMOT_DUR 0x22
+#define MPU6050_RA_FIFO_EN 0x23
+#define MPU6050_RA_I2C_MST_CTRL 0x24
+#define MPU6050_RA_I2C_SLV0_ADDR 0x25
+#define MPU6050_RA_I2C_SLV0_REG 0x26
+#define MPU6050_RA_I2C_SLV0_CTRL 0x27
+#define MPU6050_RA_I2C_SLV1_ADDR 0x28
+#define MPU6050_RA_I2C_SLV1_REG 0x29
+#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
+#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
+#define MPU6050_RA_I2C_SLV2_REG 0x2C
+#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
+#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
+#define MPU6050_RA_I2C_SLV3_REG 0x2F
+#define MPU6050_RA_I2C_SLV3_CTRL 0x30
+#define MPU6050_RA_I2C_SLV4_ADDR 0x31
+#define MPU6050_RA_I2C_SLV4_REG 0x32
+#define MPU6050_RA_I2C_SLV4_DO 0x33
+#define MPU6050_RA_I2C_SLV4_CTRL 0x34
+#define MPU6050_RA_I2C_SLV4_DI 0x35
+#define MPU6050_RA_I2C_MST_STATUS 0x36
+#define MPU6050_RA_INT_PIN_CFG 0x37
+#define MPU6050_RA_INT_ENABLE 0x38
+#define MPU6050_RA_DMP_INT_STATUS 0x39
+#define MPU6050_RA_INT_STATUS 0x3A
+#define MPU6050_RA_ACCEL_XOUT_H 0x3B
+#define MPU6050_RA_ACCEL_XOUT_L 0x3C
+#define MPU6050_RA_ACCEL_YOUT_H 0x3D
+#define MPU6050_RA_ACCEL_YOUT_L 0x3E
+#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
+#define MPU6050_RA_ACCEL_ZOUT_L 0x40
+#define MPU6050_RA_TEMP_OUT_H 0x41
+#define MPU6050_RA_TEMP_OUT_L 0x42
+#define MPU6050_RA_GYRO_XOUT_H 0x43
+#define MPU6050_RA_GYRO_XOUT_L 0x44
+#define MPU6050_RA_GYRO_YOUT_H 0x45
+#define MPU6050_RA_GYRO_YOUT_L 0x46
+#define MPU6050_RA_GYRO_ZOUT_H 0x47
+#define MPU6050_RA_GYRO_ZOUT_L 0x48
+#define MPU6050_RA_EXT_SENS_DATA_00 0x49
+#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
+#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
+#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
+#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
+#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
+#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
+#define MPU6050_RA_EXT_SENS_DATA_07 0x50
+#define MPU6050_RA_EXT_SENS_DATA_08 0x51
+#define MPU6050_RA_EXT_SENS_DATA_09 0x52
+#define MPU6050_RA_EXT_SENS_DATA_10 0x53
+#define MPU6050_RA_EXT_SENS_DATA_11 0x54
+#define MPU6050_RA_EXT_SENS_DATA_12 0x55
+#define MPU6050_RA_EXT_SENS_DATA_13 0x56
+#define MPU6050_RA_EXT_SENS_DATA_14 0x57
+#define MPU6050_RA_EXT_SENS_DATA_15 0x58
+#define MPU6050_RA_EXT_SENS_DATA_16 0x59
+#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
+#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
+#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
+#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
+#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
+#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
+#define MPU6050_RA_EXT_SENS_DATA_23 0x60
+#define MPU6050_RA_MOT_DETECT_STATUS 0x61
+#define MPU6050_RA_I2C_SLV0_DO 0x63
+#define MPU6050_RA_I2C_SLV1_DO 0x64
+#define MPU6050_RA_I2C_SLV2_DO 0x65
+#define MPU6050_RA_I2C_SLV3_DO 0x66
+#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
+#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
+#define MPU6050_RA_MOT_DETECT_CTRL 0x69
+#define MPU6050_RA_USER_CTRL 0x6A
+#define MPU6050_RA_PWR_MGMT_1 0x6B
+#define MPU6050_RA_PWR_MGMT_2 0x6C
+#define MPU6050_RA_BANK_SEL 0x6D
+#define MPU6050_RA_MEM_START_ADDR 0x6E
+#define MPU6050_RA_MEM_R_W 0x6F
+#define MPU6050_RA_DMP_CFG_1 0x70
+#define MPU6050_RA_DMP_CFG_2 0x71
+#define MPU6050_RA_FIFO_COUNTH 0x72
+#define MPU6050_RA_FIFO_COUNTL 0x73
+#define MPU6050_RA_FIFO_R_W 0x74
+#define MPU6050_RA_WHO_AM_I 0x75
+
+class MPU6050 : public I2C_Sensor
+{
+ public:
+ MPU6050(PinName sda, PinName scl); // constructor, uses I2C_Sensor class
+ float data_gyro[3]; // where the measured data is saved
+ float data_acc[3]; // where the measured data is saved
+ virtual void read(); // read all axis from register to array data
+ float offset_gyro[3]; // offset that's subtracted from every Gyroscope measurement
+ float offset_acc[3]; // offset that's subtracted from every Accelerometer measurement
+ void calibrate(int times, float separation_time); // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
+ int readTemp(); // read temperature from sensor
+
+ private:
+ int raw_gyro[3];
+ int raw_acc[3];
+ void readraw_gyro();
+ void readraw_acc();
+};
+
+#endif
\ No newline at end of file
--- a/Sensors/I2C_Sensor.h Wed Aug 28 00:30:38 2013 +0000
+++ b/Sensors/I2C_Sensor.h Fri Feb 14 14:17:32 2014 +0000
@@ -11,10 +11,6 @@
public:
I2C_Sensor(PinName sda, PinName scl, char address);
- float data[3]; // where the measured data is saved
- virtual void read() = 0; // read all axis from register to array data
- //TODO: virtual void calibrate() = 0; // calibrate the sensor and if desired write calibration values to a file
-
protected:
// Calibration
void saveCalibrationValues(float values[], int size, char * filename);
@@ -25,10 +21,6 @@
void writeRegister(char reg, char data);
void readMultiRegister(char reg, char* output, int size);
- // raw data and function to measure it
- int raw[3];
- virtual void readraw() = 0;
-
private:
I2C i2c_init; // original mbed I2C-library just to initialise the control registers
MODI2C i2c; // I2C-Bus
--- a/main.cpp Wed Aug 28 00:30:38 2013 +0000
+++ b/main.cpp Fri Feb 14 14:17:32 2014 +0000
@@ -1,10 +1,11 @@
#include "mbed.h" // Standard Library
#include "LED.h" // LEDs framework for blinking ;)
#include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe)
-#include "L3G4200D.h" // Gyro (Gyroscope)
-#include "ADXL345.h" // Acc (Accelerometer)
-#include "HMC5883.h" // Comp (Compass)
-#include "BMP085_old.h" // Alt (Altitude sensor)
+#include "MPU6050.h" // Combined Gyro and Acc
+//#include "L3G4200D.h" // Gyro (Gyroscope)
+//#include "ADXL345.h" // Acc (Accelerometer)
+//#include "HMC5883.h" // Comp (Compass)
+//#include "BMP085_old.h" // Alt (Altitude sensor)
#include "RC_Channel.h" // RemoteControl Channels with PPM
#include "Servo_PWM.h" // Motor PPM using PwmOut
#include "PID.h" // PID Library (slim, self written)
@@ -27,7 +28,7 @@
#define PITCH 1
#define YAW 2
-#define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC
+#define PC_CONNECTED // decomment if you want to debug per USB/Bluetooth and your PC
// Global variables
bool armed = false; // this variable is for security (when false no motor rotates any more)
@@ -54,13 +55,14 @@
// Initialisation of hardware (see includes for more info)
LED LEDs;
#ifdef PC_CONNECTED
- //PC pc(USBTX, USBRX, 115200); // USB
- PC pc(p9, p10, 115200); // Bluetooth
+ PC pc(USBTX, USBRX, 115200); // USB
+ //PC pc(p9, p10, 115200); // Bluetooth
#endif
-L3G4200D Gyro(p28, p27);
-ADXL345 Acc(p28, p27);
-HMC5883 Comp(p28, p27);
-BMP085_old Alt(p28, p27);
+MPU6050 Sensor(p28, p27);
+//L3G4200D Gyro(p28, p27);
+//ADXL345 Acc(p28, p27);
+//HMC5883 Comp(p28, p27);
+//BMP085_old Alt(p28, p27);
RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 !
Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed!
IMU_Filter IMU; // (don't write () after constructor for no arguments!)
@@ -71,8 +73,9 @@
time_read_sensors = GlobalTimer.read(); // start time measure for sensors
// read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
- Gyro.read();
- Acc.read();
+ Sensor.read();
+ //Gyro.read();
+ //Acc.read();
//Comp.read(); // TODO: not every loop every sensor? altitude not that important
//Alt.Update(); // TODO needs very long to read because of waits
@@ -84,7 +87,8 @@
dt = GlobalTimer.read() - time_for_dt; // time in us since last loop
time_for_dt = GlobalTimer.read(); // set new time for next measurement
- IMU.compute(dt, Gyro.data, Acc.data);
+ IMU.compute(dt, Sensor.data_gyro, Sensor.data_acc);
+ //IMU.compute(dt, Gyro.data, Acc.data);
//pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors);
if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100)
@@ -147,7 +151,7 @@
for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
ESC[i] = 0;
}
- pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n",
+ /*pc.printf("%d,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n",
armed,
dt,
dt_read_sensors,
@@ -163,6 +167,24 @@
MIX.Motor_speed[0],
MIX.Motor_speed[1],
MIX.Motor_speed[2],
+ MIX.Motor_speed[3]);*/
+
+ pc.printf("%f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f, %f,%f,%f,%f\r\n",
+ IMU.angle[ROLL],
+ IMU.angle[PITCH],
+ IMU.angle[YAW],
+ Sensor.data_gyro[0],
+ Sensor.data_gyro[1],
+ Sensor.data_gyro[2],
+ Sensor.data_acc[0],
+ Sensor.data_acc[1],
+ Sensor.data_acc[2],
+ controller_value[ROLL],
+ controller_value[PITCH],
+ controller_value[YAW],
+ MIX.Motor_speed[0],
+ MIX.Motor_speed[1],
+ MIX.Motor_speed[2],
MIX.Motor_speed[3]);
dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time for loop
@@ -200,8 +222,9 @@
#endif
LEDs.roll(2);
- Gyro.calibrate(50, 0.02);
- Acc.calibrate(50, 0.02);
+ Sensor.calibrate(50, 0.02);
+ //Gyro.calibrate(50, 0.02);
+ //Acc.calibrate(50, 0.02);
// Start!
GlobalTimer.start();
Matthias Grob
