IMU changes. Calibration options

Disable standby mode at init
Can now calculate offset on acc and gyro
Fifo reset function

src/drivers/MPU6000.cpp

@@ -4,8 +4,20 @@ www.xene.it
  
 #include <mbed.h>
 #include "MPU6000.h"
+#include <math.h>
+#include "src/math/Utilities.h"
  
-mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
+mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs)
+: spi(_spi)
+, cs(_cs) 
+{
+    _gyro_offset[0] = 0;
+    _gyro_offset[1] = 0;
+    _gyro_offset[2] = 0;
+    _acc_offset[0] = 0;
+    _acc_offset[1] = 0;
+    _acc_offset[2] = 0;
+}
  
 
 bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
@@ -33,6 +45,11 @@ bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
     response=spi.write(MPUREG_USER_CTRL);
     response=spi.write(BIT_I2C_IF_DIS);
     deselect();
+    // DISABLE STANDBY MODE
+    select();
+    response=spi.write(MPUREG_PWR_MGMT_2);
+    response=spi.write(0x00);
+    deselect();
     //WHO AM I?
     select();
     response=spi.write(MPUREG_WHOAMI|READ_FLAG);
@@ -54,6 +71,8 @@ bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
     response=spi.write(MPUREG_INT_ENABLE);
     response=spi.write(0x00);
     deselect();
+
+    return true; // TODO
 }
 
 int mpu6000_spi::enableInterrupt()
@@ -183,7 +202,12 @@ float mpu6000_spi::read_acc(int axis){
     data=(float)bit_data;
     data=data/acc_divider;
     deselect();
-    return data;
+    return data - _acc_offset[axis];;
+}
+
+float mpu6000_spi::read_acc_deg(int axis)
+{
+    return asin(read_acc(axis))/PI*180;
 }
  
 float mpu6000_spi::read_rot(int axis){
@@ -208,7 +232,7 @@ float mpu6000_spi::read_rot(int axis){
     data=(float)bit_data;
     data=data/gyro_divider;
     deselect();
-    return data;
+    return data - _gyro_offset[axis];
 }
  
 float mpu6000_spi::read_temp(){
@@ -226,6 +250,60 @@ float mpu6000_spi::read_temp(){
     return data;
 }
  
+bool mpu6000_spi::resetOffset_gyro(){
+    wait_ms(20);
+
+    bool result(true);
+
+    float validThreshold(5.0);
+    
+    for (int axisIt = 0; axisIt <= 2 ; axisIt++)
+    {
+        for (int samples = 0; samples <= 5; samples++)
+        {
+            Thread::wait(20);
+            float abc = read_rot(axisIt);
+            _gyro_offset[axisIt] += abc;
+        }
+        _gyro_offset[axisIt] /= 6;
+
+        if (abs(_gyro_offset[axisIt]) > validThreshold)
+        {
+            // Calibration failed
+            result = false;
+            _gyro_offset[axisIt] = 0.0;
+        }
+    }
+    return result;
+}
+
+bool mpu6000_spi::resetOffset_acc(){
+    wait_ms(20);
+
+    bool result(true);
+
+    float validThreshold(5.0);
+    
+    for (int axisIt = 0; axisIt <= 2 ; axisIt++)
+    {
+        for (int samples = 0; samples <= 5; samples++)
+        {
+            Thread::wait(20);
+            float abc = read_acc(axisIt);
+            _acc_offset[axisIt] += abc;
+        }
+        _acc_offset[axisIt] /= 6;
+
+        if (abs(_acc_offset[axisIt]) > validThreshold)
+        {
+            // Calibration failed
+            result = false;
+            _acc_offset[axisIt] = 0.0;
+        }
+    }
+    return result;
+}
+
 int mpu6000_spi::calib_acc(int axis){
     uint8_t responseH,responseL,calib_data;
     int temp_scale;
@@ -282,3 +360,13 @@ void mpu6000_spi::deselect() {
     //Set CS high to stop transmission (restarts conversion)
     cs = 1;
 }
+
+void mpu6000_spi::fifo_reset()
+{
+    unsigned int response;
+    //FIFO RESET
+    select();
+    response=spi.write(MPUREG_USER_CTRL);
+    response=spi.write(BIT_FIFO_RESET);
+    deselect();
+}

src/drivers/MPU6000.h

@@ -83,6 +83,8 @@ class mpu6000_spi
     returns the value in Gs
     -----------------------------------------------------------------------------------------------*/
     float read_acc(int axis);
+
+    float read_acc_deg(int axis);
     
     /*-----------------------------------------------------------------------------------------------
                                     READ GYROSCOPE
@@ -118,6 +120,24 @@ class mpu6000_spi
     -----------------------------------------------------------------------------------------------*/
     unsigned int set_acc_scale(int scale);
 
+
+
+    /*-----------------------------------------------------------------------------------------------
+                                    CALIBRATE GYRO OFFSET
+    usage: call this function to reset what the gyro sees as still.
+    returns true if successfull. The board cannot be in movement
+    during this process
+    -----------------------------------------------------------------------------------------------*/
+    bool resetOffset_gyro();
+
+    /*-----------------------------------------------------------------------------------------------
+                                    CALIBRATE ACC OFFSET
+    usage: call this function to reset what the acc sees as 0 deg.
+    returns true if successfull. The board cannot be in movement
+    during this process. Should be straight (in desired 0 position)
+    -----------------------------------------------------------------------------------------------*/
+    bool resetOffset_acc();
+
     /*-----------------------------------------------------------------------------------------------
                                     READ ACCELEROMETER CALIBRATION
     usage: call this function to read accelerometer data. Axis represents selected axis:
@@ -154,6 +174,8 @@ class mpu6000_spi
     returns the I2C address (104)
     -----------------------------------------------------------------------------------------------*/
     unsigned int whoami();
+
+    void fifo_reset();
     
     float acc_divider;
     float gyro_divider;
@@ -163,6 +185,8 @@ class mpu6000_spi
     PinName _SO_pin;
     PinName _SCK_pin;
     float _error;
+    float _gyro_offset[3];
+    float _acc_offset[3];
 };
  
 #endif
@@ -254,6 +278,10 @@ class mpu6000_spi
 #define BIT_INT_ANYRD_2CLEAR        0x10
 #define BIT_RAW_RDY_EN              0x01
 #define BIT_I2C_IF_DIS              0x10
+
+#define BIT_FIFO_EN                 0x40
+#define BIT_FIFO_RESET              0x04
+
  
 #define READ_FLAG                   0x80