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();