Revision 8:609a2ad4c30e, committed 2015-11-19

Comitter:

maetugr

Date:

Thu Nov 19 18:47:27 2015 +0000

Parent:

7:90f876d47862

Commit message:

made I2C-Sensors working in parallel, added rolling buffer for PID derivative, played with the PID and frequency parameters in main

Changed in this revision

--- a/IMU/IMU_10DOF.cpp	Mon Sep 14 12:49:08 2015 +0000
+++ b/IMU/IMU_10DOF.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -1,6 +1,6 @@
 #include "IMU_10DOF.h"
 
-IMU_10DOF::IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS) : mpu(MOSI, MISO, SCLK, CS)
+IMU_10DOF::IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName SDA, PinName SCL) : mpu(MOSI, MISO, SCLK, CS), mpu2(SDA, SCL)
 {
     dt = 0;
     dt_sensors = 0;
@@ -15,6 +15,7 @@
     SensorTimer.start(); // start time for measuring sensors
     mpu.readGyro(); // reading sensor data
     mpu.readAcc();
+    mpu2.read(); // reading sensor data
     SensorTimer.stop(); // stop time for measuring sensors
     dt_sensors = SensorTimer.read();
     SensorTimer.reset();
@@ -24,4 +25,5 @@
     LoopTimer.reset();
     
     Filter.compute(dt, mpu.Gyro, mpu.Acc, mpu.Acc);
+    //Filter.compute(dt, mpu2.data_gyro, mpu2.data_acc, mpu2.data_acc);
 }
\ No newline at end of file

--- a/IMU/IMU_10DOF.h	Mon Sep 14 12:49:08 2015 +0000
+++ b/IMU/IMU_10DOF.h	Thu Nov 19 18:47:27 2015 +0000
@@ -5,12 +5,13 @@
 
 #include "mbed.h"
 #include "MPU9250.h"    // Combined Gyroscope & Accelerometer & Magnetometer over SPI
+#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)
 
 class IMU_10DOF
 {           
     public:
-        IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS);
+        IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName SDA, PinName SCL);
         void readAngles();              // read all sensors and calculate angles
         
         float * angle;                  // where the measured and calculated data is saved
@@ -22,6 +23,7 @@
         float dt_sensors;               // time only to read sensors
         
         MPU9250     mpu;                // The sensor Hardware Driver
+        MPU6050     mpu2;
             
     private:                            
         Timer LoopTimer;               // local time to calculate processing speed for entire loop

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/MPU6050/I2C_Sensor.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -0,0 +1,54 @@
+#include "I2C_Sensor.h"
+
+// calculate the 8-Bit write/read I2C-Address from the 7-Bit adress of the device
+#define GET_I2C_WRITE_ADDRESS(ADR)  (ADR << 1&0xFE) // ADR & 1111 1110
+#define GET_I2C_READ_ADDRESS(ADR)   (ADR << 1|0x01) // ADR | 0000 0001
+
+I2C_Sensor::I2C_Sensor(PinName sda, PinName scl, char i2c_address) : i2c(sda, scl), local("local")
+{
+    I2C_Sensor::i2c_address = i2c_address;
+    i2c.frequency(400000); // standard speed
+    //i2c.frequency(1000000); // ultrafast!
+}
+
+void I2C_Sensor::saveCalibrationValues(float values[], int size, char * filename)
+{
+    FILE *fp = fopen(strcat("/local/", filename), "w");
+    for(int i = 0; i < size; i++)
+        fprintf(fp, "%f\r\n", values[i]);
+    fclose(fp);
+}
+
+void I2C_Sensor::loadCalibrationValues(float values[], int size, char * filename)
+{
+    FILE *fp = fopen(strcat("/local/", filename), "r");
+    for(int i = 0; i < size; i++)
+        fscanf(fp, "%f", &values[i]);
+    fclose(fp);
+}
+
+// I2C functions --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+
+
+char I2C_Sensor::readRegister(char reg)
+{
+    char value = 0;
+    
+    i2c.write(i2c_address, &reg, 1);
+    i2c.read(i2c_address, &value, 1);
+
+    return value;
+}
+
+void I2C_Sensor::writeRegister(char reg, char data)
+{
+    char buffer[2] = {reg, data};
+    i2c.write(i2c_address, buffer, 2);
+}
+
+int I2C_Sensor::readMultiRegister(char reg, char* output, int size)
+{
+    if (0 != i2c.write (i2c_address, &reg, 1)) return 1; // tell register address of the MSB get the sensor to do slave-transmit subaddress updating.
+    if (0 != i2c.read  (i2c_address, output, size)) return 1; // tell it where to store the data read
+    return 0;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/MPU6050/I2C_Sensor.h	Thu Nov 19 18:47:27 2015 +0000
@@ -0,0 +1,36 @@
+// by MaEtUgR
+
+#ifndef I2C_Sensor_H
+#define I2C_Sensor_H
+
+#include "mbed.h"
+
+class I2C_Sensor
+{           
+    public:
+        I2C_Sensor(PinName sda, PinName scl, char address);
+        
+        float data[3];                  // where the measured data is saved
+        //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);
+        void loadCalibrationValues(float values[], int size, char * filename);
+    
+        // I2C functions
+        char readRegister(char reg);
+        void writeRegister(char reg, char data);
+        int readMultiRegister(char reg, char* output, int size);
+        
+        // raw data and function to measure it
+        short raw[3];
+        
+    private:
+        I2C i2c;            // original mbed I2C-library just to initialise the control registers
+        char i2c_address;   // address
+        
+        LocalFileSystem local; // file access to save calibration values
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU/MPU6050/MPU6050.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -0,0 +1,110 @@
+#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)
+    /*
+    last 3 Bits of   |Accelerometer(Fs=1kHz) |Gyroscope 
+    MPU6050_RA_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 
+    */
+    writeRegister(MPU6050_RA_CONFIG, 0x03);
+    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();
+    
+    offset_gyro[0] = -35;                // TODO: make better calibration
+    offset_gyro[1] = 3;
+    offset_gyro[2] = 2;
+    
+    for (int i = 0; i < 3; i++)
+        data_gyro[i] = (raw_gyro[i] - offset_gyro[i]) * 0.07 * 0.87; // 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
+
+    // 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;
+    tmp = data_gyro[0];
+    data_gyro[0] = -data_gyro[0];
+    data_gyro[1] = -data_gyro[1];
+    data_gyro[2] = data_gyro[2];
+    tmp = data_acc[0];
+    data_acc[0] = -data_acc[0];
+    data_acc[1] = -data_acc[1];
+    data_acc[2] = data_acc[2];
+}
+
+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
+    
+    if(readMultiRegister(MPU6050_RA_GYRO_XOUT_H | (1 << 7), buffer, 6) != 0) return; // 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/IMU/MPU6050/MPU6050.h	Thu Nov 19 18:47:27 2015 +0000
@@ -0,0 +1,145 @@
+// 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         0xD2 // 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
+        
+        int raw_gyro[3];
+    private:
+        
+        int raw_acc[3];
+        void readraw_gyro();
+        void readraw_acc();
+};
+
+#endif
\ No newline at end of file

--- a/IMU/MPU9250/MPU9250.cpp	Mon Sep 14 12:49:08 2015 +0000
+++ b/IMU/MPU9250/MPU9250.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -17,7 +17,7 @@
     5             |10           |13.8     |10           |13.4     |1 
     6             |5            |19.0     |5            |18.6     |1 
     */
-    writeRegister8(MPU9250_CONFIG, 0x03);
+    writeRegister8(MPU9250_CONFIG, 0x00);
     writeRegister8(MPU9250_GYRO_CONFIG, 0x18);              // scales gyros range to +-2000dps
     writeRegister8(MPU9250_ACCEL_CONFIG, 0x08);             // scales accelerometers range to +-4g
 }
@@ -35,7 +35,7 @@
     int16_t rawGyro[3];
     readRegister48(MPU9250_GYRO_XOUT_H, rawGyro);
     
-    int16_t offsetGyro[3] = {-31, -15, -11};                // TODO: make better calibration
+    int16_t offsetGyro[3] = {-31, -16, -12};                // TODO: make better calibration
     
     for (int i = 0; i < 3; i++)
         Gyro[i] = (rawGyro[i] - offsetGyro[i]) * 0.07 * 0.87;        // subtract offset from calibration and multiply unit factor to get degree per second (datasheet p.10)

--- a/IMU/MPU9250/MPU9250.h	Mon Sep 14 12:49:08 2015 +0000
+++ b/IMU/MPU9250/MPU9250.h	Thu Nov 19 18:47:27 2015 +0000
@@ -18,7 +18,7 @@
         void readAcc();                                                         // read measurement data from accelerometer
         float Acc[3];                                                           // where accelerometer measurement data is stored
         
-    private:
+    //private:
         
         // SPI Inteface
         SPI spi;                                                                // SPI Bus

--- a/PID/PID.cpp	Mon Sep 14 12:49:08 2015 +0000
+++ b/PID/PID.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -31,6 +31,9 @@
         
     // Derivative
     RollBuffer[RollBufferIndex] = (Error - PreviousError) / dt;
+    RollBufferIndex++;
+    if (RollBufferIndex == BUFFERSIZE)
+        RollBufferIndex = 0;
     float Derivative = 0;
     for(int i=0; i<BUFFERSIZE ;i++)
         Derivative += RollBuffer[i];

--- a/main.cpp	Mon Sep 14 12:49:08 2015 +0000
+++ b/main.cpp	Thu Nov 19 18:47:27 2015 +0000
@@ -15,7 +15,7 @@
 
 #define PPM_FREQU       495     // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
 #define INTEGRAL_MAX    300     // maximal output offset that can result from integrating errors
-#define RC_SENSITIVITY  15      // maximal angle from horizontal that the PID is aming for
+#define RC_SENSITIVITY  30      // maximal angle from horizontal that the PID is aming for
 #define YAWSPEED        1.0     // maximal speed of yaw rotation in degree per Rate
 #define AILERON         0       // RC
 #define ELEVATOR        1
@@ -34,20 +34,22 @@
 bool  debug = true;                     // shows if we want output for the computer
 bool  level = false;                     // switches between self leveling and acro mode
 bool  RC_present = false;               // shows if an RC is present
-float P_R = 2.5, I_R = 0.3, D_R = 0;      // PID values for the rate controller
-float P_A = 2.1, I_A = 0.3, D_A = 0;        // PID values for the angle controller      P_A = 1.865, I_A = 1.765, D_A = 0
+float P_R = 2.6, I_R = 0.3, D_R = 0;      // PID values for the rate controller
+float P_A = 1.9, I_A = 0.2, D_A = 0;        // PID values for the angle controller      P_A = 1.865, I_A = 1.765, D_A = 0
 float PY = 2.3, IY = 0, DY = 0;         // PID values for Yaw
 float RC_angle[] = {0,0,0};             // Angle of the RC Sticks, to steer the QC
 float Motor_speed[4] = {0,0,0,0};       // Mixed Motorspeeds, ready to send
 
 Timer LoopTimer;
 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
-float control_frequency = PPM_FREQU;         // frequency for the main loop in Hz
+float control_frequency = 800;//PPM_FREQU;         // frequency for the main loop in Hz
+int counter = 0;
+int divider = 20;
 
 LED         LEDs;
-PC          pc(USBTX, USBRX, 115200);   // USB
-//PC          pc(p9, p10, 115200);       // Bluetooth PIN: 1234
-IMU_10DOF   IMU(p5, p6, p7, p19);
+//PC          pc(USBTX, USBRX, 115200);   // USB
+PC          pc(p9, p10, 115200);       // Bluetooth PIN: 1234
+IMU_10DOF   IMU(p5, p6, p7, p19, p28, p27);
 RC_Channel  RC[] = {RC_Channel(p8,1), RC_Channel(p15,2), RC_Channel(p17,4), RC_Channel(p16,3), RC_Channel(p25,2), RC_Channel(p26,4), RC_Channel(p29,3)}; // no p19/p20 !
 PID         Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
 PID         Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)};
@@ -103,14 +105,17 @@
     if (level) {
         for(int i=0;i<2;i++) { // LEVEL
             Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
-            Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct
+            if (counter % 16 == 0)
+                Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angles and get his advice to correct
             Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
-            Controller_Rate[i].compute(-Controller_Angle[i].Value, IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            Controller_Rate[i].compute(-Controller_Angle[i].Value, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            //Controller_Rate[i].compute(-Controller_Angle[i].Value, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 );
         }
     } else {
         for(int i=0;i<2;i++) { // ACRO
             Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
-            Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
+            //Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 );
         }
     }
     
@@ -130,22 +135,22 @@
     Motor_speed[3] = throttle   +SQRT2*Controller_Rate[ROLL].Value  +SQRT2*Controller_Rate[PITCH].Value;  // 
     
     Motor_speed[0] -= Controller_Rate[YAW].Value;
+    Motor_speed[1] += Controller_Rate[YAW].Value;
     Motor_speed[2] -= Controller_Rate[YAW].Value;
     Motor_speed[3] += Controller_Rate[YAW].Value;
-    Motor_speed[1] += Controller_Rate[YAW].Value;
     Times[5] = LoopTimer.read(); // 17us
     
     if (armed) // for SECURITY!
     {       
             debug = false;
             // PITCH
-            ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
-            ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
+            //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
+            //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
             // ROLL
             //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50;
             //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50;
-            /*for(int i=0;i<4;i++)   // Set new motorspeeds
-                ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100;*/
+            for(int i=0;i<4;i++)   // Set new motorspeeds
+                ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100;
             
     } else {
         for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
@@ -156,9 +161,13 @@
     
     LEDs.rollnext();
     
-    Times[7] = LoopTimer.read(); // 7us TOTAL 297us
+    /*if(counter % divider == 0) {
+        pc.printf("%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+    }*/
+    counter++;
     
-    while(LoopTimer.read() < 1/control_frequency);
+    Times[7] = LoopTimer.read(); // 7us TOTAL 297us
+    while(LoopTimer.read() < 1/control_frequency); // Kill the rest of the time TODO: make a better solution so we can do misc things with these cycles
     Times[8] = LoopTimer.read();
     LoopTimer.stop();
     LoopTimer.reset();
@@ -167,17 +176,18 @@
     if (debug) {
         pc.printf("$STATE,%d,%d,%.f,%.3f,%.3f\r\n", armed, level, control_frequency, IMU.dt*1e3, IMU.dt_sensors*1e6);
         //pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read());
-        //pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+        pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
+        pc.printf("$GYRO2,%.3f,%.3f,%.3f\r\n", IMU.mpu2.data_gyro[ROLL], IMU.mpu2.data_gyro[PITCH], IMU.mpu2.data_gyro[YAW]);
         //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]);
         pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
         //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]);
         pc.printf("$CONTR,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R, PY);
         pc.printf("$CONTA,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Angle[ROLL].Value, Controller_Angle[PITCH].Value, Controller_Angle[YAW].Value, P_A, I_A, D_A);
         pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]);
-        pc.printf("$TIMES");
+        /*pc.printf("$TIMES");
         for(int i = 1; i < 10; i++)
             pc.printf(",%.3f", (Times[i]-Times[i-1])*1e6);
-        pc.printf("\r\n");
+        pc.printf("\r\n");*/
         wait(0.1);
     }
 }
@@ -209,6 +219,11 @@
     if (command == 'd')
         I_R -= 0.1;
         
+    if (command == 'x')
+        D_R += 0.001;
+    if (command == 'c')
+        D_R -= 0.001;
+        
     if (command == 'r')
         P_A += 0.1;
     if (command == 'f')
@@ -224,12 +239,15 @@
     if (command == 'h')
         PY -= 0.1;
         
-    /*if (command == 'o') {
+    if (command == 'o') {
         control_frequency += 100;
+        
     }
     if (command == 'l') {
         control_frequency -= 100;
-    }*/
+        
+    }
+        
         
     pc.putc(command);
     LEDs.tilt(2);