Revision 10:14390c90c3f5, committed 2015-08-31

Comitter:

maetugr

Date:

Mon Aug 31 20:20:50 2015 +0000

Parent:

8:e79c7939d6de

Commit message:

before changing to MPU9250

Changed in this revision

--- a/IMU/IMU_10DOF.cpp	Mon Jul 14 09:06:43 2014 +0000
+++ b/IMU/IMU_10DOF.cpp	Mon Aug 31 20:20:50 2015 +0000
@@ -17,6 +17,8 @@
 void IMU_10DOF::readAngles()
 {
     time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors
+    //mpu.readGyro();
+    //mpu.readAcc();
     Sensor.read();
     //Gyro.read(); // reading sensor data
     //Acc.read();
@@ -27,6 +29,7 @@
     dt = LocalTimer.read() - time_for_dt; // time in s since last loop
     time_for_dt = LocalTimer.read();      // set new time for next measurement
     
+    //Filter.compute(dt, mpu.Gyro, mpu.Acc, Comp.data);
     Filter.compute(dt, Sensor.data_gyro, Sensor.data_acc, Comp.data);
     //Filter.compute(dt, Gyro.data, Acc.data, Comp.data);
 }

--- a/IMU/IMU_10DOF.h	Mon Jul 14 09:06:43 2014 +0000
+++ b/IMU/IMU_10DOF.h	Mon Aug 31 20:20:50 2015 +0000
@@ -10,6 +10,7 @@
 #include "BMP085.h"     // Alt (Altitude sensor or Barometer)
 #include "MPU6050.h"    // Combined Gyroscope & Accelerometer
 #include "IMU_Filter.h" // Class to calculate position angles  (algorithm from S.O.H. Madgwick, see header file for info)
+//#include "MPU9250.h"    // Combined Gyroscope & Accelerometer & Magnetometer over SPI
 
 class IMU_10DOF
 {           
@@ -26,7 +27,8 @@
         float dt;                       // time for entire loop
         float dt_sensors;               // time only to read sensors
         
-        MPU6050     Sensor;             // All sensors Hardwaredrivers
+        //MPU9250     mpu;                // All sensors Hardwaredrivers
+        MPU6050     Sensor;
         L3G4200D    Gyro;
         ADXL345     Acc;
         HMC5883     Comp;

--- a/IMU/Sensors/Gyro_Acc/MPU6050.cpp	Mon Jul 14 09:06:43 2014 +0000
+++ b/IMU/Sensors/Gyro_Acc/MPU6050.cpp	Mon Aug 31 20:20:50 2015 +0000
@@ -28,10 +28,10 @@
     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 to get degree per second (datasheet s.10)
+        data_gyro[i] = (raw_gyro[i] - offset_gyro[i])*0.07; // subtract offset from calibration and multiply unit factor to get degree per second (datasheet p.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
+        data_acc[i] = raw_acc[i] - offset_acc[i];           // TODO: didn't care about units because IMU-algorithm just uses vector direction
 
     // I have to swich coordinates on my board to match the ones of the other sensors (clear this part if you use the raw coordinates of the sensor)
     float tmp = 0;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/Sensors/MPU9250/MPU9250.cpp	Mon Aug 31 20:20:50 2015 +0000
@@ -0,0 +1,99 @@
+#include "MPU9250.h"
+
+MPU9250::MPU9250(PinName MOSI, PinName MISO, PinName SCLK, PinName CS) : spi(MOSI, MISO, SCLK), cs(CS) {
+    deselect();                         // chip must be deselected first
+    spi.format(8,0);                    // setup the spi for standard 8 bit data and SPI-Mode 0
+    spi.frequency(5e6);                 // with a 5MHz clock rate
+    
+    /*
+    last 3 Bits of|Accelerometer(Fs=1kHz) |Gyroscope 
+    MPU9250_CONFIG|Bandwidth(Hz)|Delay(ms)|Bandwidth(Hz)|Delay(ms)|Fs(kHz)
+    ------------------------------------------------------------------------- 
+    0             |260          |0        |256          |0.98     |8 
+    1             |184          |2.0      |188          |1.9      |1 
+    2             |94           |3.0      |98           |2.8      |1 
+    3             |44           |4.9      |42           |4.8      |1 
+    4             |21           |8.5      |20           |8.3      |1 
+    5             |10           |13.8     |10           |13.4     |1 
+    6             |5            |19.0     |5            |18.6     |1 
+    */
+    writeRegister8(MPU9250_CONFIG, 0x06);
+    writeRegister8(MPU9250_GYRO_CONFIG, 0x18);              // scales gyros range to +-2000dps
+    writeRegister8(MPU9250_ACCEL_CONFIG, 0x08);             // scales accelerometers range to +-4g
+}
+
+uint8_t MPU9250::getWhoami() {
+    return readRegister8(MPU9250_WHO_AM_I);
+}
+
+float MPU9250::getTemperature() {
+    int16_t data = readRegister16(MPU9250_TEMP_OUT_H);
+    return ((data - 21) / 333.87) + 21;                     // formula from register map p.33
+}
+
+void MPU9250::readGyro() {
+    int16_t rawGyro[3];
+    readRegister48(MPU9250_GYRO_XOUT_H, rawGyro);
+    
+    int16_t offsetGyro[3] = {-31, -15, -11};                // TODO: make better calibration
+    
+    for (int i = 0; i < 3; i++)
+        Gyro[i] = (rawGyro[i] - offsetGyro[i])*0.07;        // subtract offset from calibration and multiply unit factor to get degree per second (datasheet p.10)
+}
+
+void MPU9250::readAcc() {
+    int16_t rawAcc[3];
+    readRegister48(MPU9250_ACCEL_XOUT_H, rawAcc);
+    
+    int16_t offsetAcc[3] = {-120, -48, -438};                       // TODO: make better calibration
+    
+    for (int i = 0; i < 3; i++)
+        Acc[i] = (rawAcc[i] - offsetAcc[i])/8192.0;                // TODO: didn't care about units because IMU-algorithm just uses vector direction
+}
+
+// PRIVATE Methods ------------------------------------------------------------------------------------
+
+
+// SPI Interface --------------------------------------------------------------------------------------
+uint8_t MPU9250::readRegister8(uint8_t reg) {
+    uint8_t result;
+    readRegister(reg, &result, 1);
+    return result;
+}
+
+uint16_t MPU9250::readRegister16(uint8_t reg) {
+    uint8_t result[2];
+    readRegister(reg, result, 2);
+    return result[0]<<8 | result[1];                        // join 8-Bit pieces to 16-bit short integer
+}
+
+void MPU9250::readRegister48(uint8_t reg, int16_t *buffer) {
+    uint8_t result[6];
+    readRegister(reg, result, 6);
+    buffer[0] = (int16_t) (result[0] << 8 | result[1]);     // join 8-Bit pieces to 16-bit short integers
+    buffer[1] = (int16_t) (result[2] << 8 | result[3]);
+    buffer[2] = (int16_t) (result[4] << 8 | result[5]);
+}
+
+void MPU9250::writeRegister8(uint8_t reg, uint8_t buffer) {
+    writeRegister(reg, &buffer, 1);
+}
+
+void MPU9250::readRegister(uint8_t reg, uint8_t *buffer, int length) {
+    select();
+    spi.write(reg | 0x80);                                  // send the register address we want to read and the read flag
+    for(int i=0; i<length; i++)                             // get data
+        buffer[i] = spi.write(0x00);
+    deselect();
+}
+
+void MPU9250::writeRegister(uint8_t reg, uint8_t *buffer, int length) {
+    select();
+    spi.write(reg & ~0x80);                                 // send the register address we want to write and the write flag
+    for(int i=0; i<length; i++)                             // put data
+        spi.write(buffer[i]);
+    deselect();
+}
+
+void MPU9250::select() { cs = 0; }                          // set Cable Select pin low to start SPI transaction
+void MPU9250::deselect() { cs = 1; }                        // set Cable Select pin high to stop SPI transaction
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/Sensors/MPU9250/MPU9250.h	Mon Aug 31 20:20:50 2015 +0000
@@ -0,0 +1,146 @@
+// by MaEtUgR (Matthias Grob) 2015
+
+#ifndef MPU9250_H
+#define MPU9250_H
+
+#include "mbed.h"
+
+class MPU9250 {
+    public:            
+        MPU9250(PinName MOSI, PinName MISO, PinName SCLK, PinName CS);          // constructor, uses SPI class
+
+        // Device API
+        uint8_t getWhoami();                                                    // get the Who am I Register (should be 0x71 = 113)
+        float getTemperature();                                                 // get a temperature measurement in °C
+        
+        void readGyro();                                                        // read measurement data from gyrsocope
+        float Gyro[3];                                                          // where gyrsocope measurement data is stored
+        void readAcc();                                                         // read measurement data from accelerometer
+        float Acc[3];                                                           // where accelerometer measurement data is stored
+        
+    private:
+        
+        // SPI Inteface
+        SPI spi;                                                                // SPI Bus
+        DigitalOut cs;                                                          // Slave selector for SPI-Bus (needed to start and end SPI transactions)
+        
+        uint8_t readRegister8(uint8_t reg);                                     // expressive methods to read or write the number of bits written in the name
+        uint16_t readRegister16(uint8_t reg);
+        void readRegister48(uint8_t reg, int16_t *buffer);
+        void writeRegister8(uint8_t reg, uint8_t buffer);
+        
+        void readRegister(uint8_t reg, uint8_t *buffer, int length);            // reads length bytes of the slave registers into buffer memory
+        void writeRegister(uint8_t reg, uint8_t *buffer, int length);           // writes length bytes to the slave registers from buffer memory
+        void select();                                                          // selects the slave for a transaction
+        void deselect();                                                        // deselects the slave after transaction
+};
+
+// --------------------- Register Addresses ----------------------------------------------
+//      Mnemonic                    Address Description
+#define MPU9250_SELF_TEST_X_GYRO    0x00 // Gyroscope Self-Test Registers
+#define MPU9250_SELF_TEST_Y_GYRO    0x01 // 
+#define MPU9250_SELF_TEST_Z_GYRO    0x02 // 
+#define MPU9250_SELF_TEST_X_ACCEL   0x0D // Accelerometer Self-Test Registers
+#define MPU9250_SELF_TEST_Y_ACCEL   0x0E // 
+#define MPU9250_SELF_TEST_Z_ACCEL   0x0F // 
+#define MPU9250_XG_OFFSET_H         0x13 // Gyro Offset Registers
+#define MPU9250_XG_OFFSET_L         0x14 // 
+#define MPU9250_YG_OFFSET_H         0x15 // 
+#define MPU9250_YG_OFFSET_L         0x16 // 
+#define MPU9250_ZG_OFFSET_H         0x17 // 
+#define MPU9250_ZG_OFFSET_L         0x18 // 
+#define MPU9250_SMPLRT_DIV          0x19 // Sample Rate Divider
+#define MPU9250_CONFIG              0x1A // Configuration
+#define MPU9250_GYRO_CONFIG         0x1B // Gyroscope Configuration
+#define MPU9250_ACCEL_CONFIG        0x1C // Accelerometer Configuration
+#define MPU9250_ACCEL_CONFIG_2      0x1D // Accelerometer Configuration 2
+#define MPU9250_LP_ACCEL_ODR        0x1E // Low Power Accelerometer ODR Control
+#define MPU9250_WOM_THR             0x1F // Wake-on Motion Threshold
+#define MPU9250_FIFO_EN             0x23 // FIFO Enable
+
+#define MPU9250_I2C_MST_CTRL        0x24 // I2C Master Control
+#define MPU9250_I2C_SLV0_ADDR       0x25 // I2C Slave 0 Control
+#define MPU9250_I2C_SLV0_REG        0x26 // 
+#define MPU9250_I2C_SLV0_CTRL       0x27 // 
+#define MPU9250_I2C_SLV1_ADDR       0x28 // I2C Slave 1 Control
+#define MPU9250_I2C_SLV1_REG        0x29 // 
+#define MPU9250_I2C_SLV1_CTRL       0x2A // 
+#define MPU9250_I2C_SLV2_ADDR       0x2B // I2C Slave 2 Control
+#define MPU9250_I2C_SLV2_REG        0x2C // 
+#define MPU9250_I2C_SLV2_CTRL       0x2D // 
+#define MPU9250_I2C_SLV3_ADDR       0x2E // I2C Slave 3 Control
+#define MPU9250_I2C_SLV3_REG        0x2F // 
+#define MPU9250_I2C_SLV3_CTRL       0x30 // 
+#define MPU9250_I2C_SLV4_ADDR       0x31 // I2C Slave 4 Control
+#define MPU9250_I2C_SLV4_REG        0x32 // 
+#define MPU9250_I2C_SLV4_DO         0x33 // 
+#define MPU9250_I2C_SLV4_CTRL       0x34 // 
+#define MPU9250_I2C_SLV4_DI         0x35 // 
+#define MPU9250_I2C_MST_STATUS      0x36 // I2C Master Status
+
+#define MPU9250_INT_PIN_CFG         0x37 // INT Pin / Bypass Enable Configuration
+#define MPU9250_INT_ENABLE          0x38 // Interrupt Enable
+#define MPU9250_INT_STATUS          0x3A // Interrupt Status
+
+#define MPU9250_ACCEL_XOUT_H        0x3B // Accelerometer Measurements
+#define MPU9250_ACCEL_XOUT_L        0x3C // 
+#define MPU9250_ACCEL_YOUT_H        0x3D // 
+#define MPU9250_ACCEL_YOUT_L        0x3E // 
+#define MPU9250_ACCEL_ZOUT_H        0x3F // 
+#define MPU9250_ACCEL_ZOUT_L        0x40 // 
+#define MPU9250_TEMP_OUT_H          0x41 // Temperature Measurement
+#define MPU9250_TEMP_OUT_L          0x42 // 
+#define MPU9250_GYRO_XOUT_H         0x43 // Gyroscope Measurements
+#define MPU9250_GYRO_XOUT_L         0x44 // 
+#define MPU9250_GYRO_YOUT_H         0x45 // 
+#define MPU9250_GYRO_YOUT_L         0x46 // 
+#define MPU9250_GYRO_ZOUT_H         0x47 // 
+#define MPU9250_GYRO_ZOUT_L         0x48 // 
+
+#define MPU9250_EXT_SENS_DATA_00    0x49 // External Sensor Data
+#define MPU9250_EXT_SENS_DATA_01    0x4A // 
+#define MPU9250_EXT_SENS_DATA_02    0x4B // 
+#define MPU9250_EXT_SENS_DATA_03    0x4C // 
+#define MPU9250_EXT_SENS_DATA_04    0x4D // 
+#define MPU9250_EXT_SENS_DATA_05    0x4E // 
+#define MPU9250_EXT_SENS_DATA_06    0x4F // 
+#define MPU9250_EXT_SENS_DATA_07    0x50 // 
+#define MPU9250_EXT_SENS_DATA_08    0x51 // 
+#define MPU9250_EXT_SENS_DATA_09    0x52 // 
+#define MPU9250_EXT_SENS_DATA_10    0x53 // 
+#define MPU9250_EXT_SENS_DATA_11    0x54 // 
+#define MPU9250_EXT_SENS_DATA_12    0x55 // 
+#define MPU9250_EXT_SENS_DATA_13    0x56 // 
+#define MPU9250_EXT_SENS_DATA_14    0x57 // 
+#define MPU9250_EXT_SENS_DATA_15    0x58 // 
+#define MPU9250_EXT_SENS_DATA_16    0x59 // 
+#define MPU9250_EXT_SENS_DATA_17    0x5A // 
+#define MPU9250_EXT_SENS_DATA_18    0x5B // 
+#define MPU9250_EXT_SENS_DATA_19    0x5C // 
+#define MPU9250_EXT_SENS_DATA_20    0x5D // 
+#define MPU9250_EXT_SENS_DATA_21    0x5E // 
+#define MPU9250_EXT_SENS_DATA_22    0x5F // 
+#define MPU9250_EXT_SENS_DATA_23    0x60 // 
+
+#define MPU9250_I2C_SLV0_DO         0x63 // I2C Slave 0 Data Out
+#define MPU9250_I2C_SLV1_DO         0x64 // I2C Slave 1 Data Out
+#define MPU9250_I2C_SLV2_DO         0x65 // I2C Slave 2 Data Out
+#define MPU9250_I2C_SLV3_DO         0x66 // I2C Slave 3 Data Out
+#define MPU9250_I2C_MST_DELAY_CTRL  0x67 // I2C Master Delay Control
+#define MPU9250_SIGNAL_PATH_RESET   0x68 // Signal Path Reset
+#define MPU9250_MOT_DETECT_CTRL     0x69 // Accelerometer Interrupt Control
+#define MPU9250_USER_CTRL           0x6A // User Control
+#define MPU9250_PWR_MGMT_1          0x6B // Power Management 1
+#define MPU9250_PWR_MGMT_2          0x6C // Power Management 2
+#define MPU9250_FIFO_COUNTH         0x72 // FIFO Count Registers
+#define MPU9250_FIFO_COUNTL         0x73 // 
+#define MPU9250_FIFO_R_W            0x74 // FIFO Read Write
+#define MPU9250_WHO_AM_I            0x75 // Who Am I
+#define MPU9250_XA_OFFSET_H         0x77 // Accelerometer Offset Registers
+#define MPU9250_XA_OFFSET_L         0x78 // 
+#define MPU9250_YA_OFFSET_H         0x7A // 
+#define MPU9250_YA_OFFSET_L         0x7B // 
+#define MPU9250_ZA_OFFSET_H         0x7D // 
+#define MPU9250_ZA_OFFSET_L         0x7E // 
+
+#endif
\ No newline at end of file

--- a/main.cpp	Mon Jul 14 09:06:43 2014 +0000
+++ b/main.cpp	Mon Aug 31 20:20:50 2015 +0000
@@ -35,7 +35,7 @@
 bool  armed = false;                    // is for security (when false no motor rotates any more)
 bool  debug = true;                    // shows if we want output for the computer
 bool  RC_present = false;               // shows if an RC is present
-float P_R = 0, I_R = 0, D_R = 0;
+float P_R = 2.5, I_R = 3.7, D_R = 0;
 float P_A = 1.865, I_A = 1.765, D_A = 0;
 //float P = 13.16, I = 8, D = 2.73;          // PID values
 float PY = 3.2, IY = 0, DY = 0;
@@ -98,8 +98,8 @@
         }
         
         // Setting PID Values from auxiliary RC channels
-        if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000)
-            P_R = 0 + (((float)RC[CHANNEL8].read()) * 3  / 1000);
+        //if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000)
+        //    P_R = 0 + (((float)RC[CHANNEL8].read()) * 3  / 1000);
         /*if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000)
             I_R = 0 + (((float)RC[CHANNEL7].read()) * 12  / 1000);*/
         for(int i=0;i<3;i++)