From 1fd690b3f59962b99a76265955b45e8c0f5e8bfb Mon Sep 17 00:00:00 2001 From: philsson Date: Sun, 2 Sep 2018 19:12:46 +0200 Subject: [PATCH] IMU changes. Calibration options Disable standby mode at init Can now calculate offset on acc and gyro Fifo reset function --- src/drivers/MPU6000.cpp | 94 +++++++++++++++++++++++++++++++++++++++-- src/drivers/MPU6000.h | 28 ++++++++++++ 2 files changed, 119 insertions(+), 3 deletions(-) diff --git a/src/drivers/MPU6000.cpp b/src/drivers/MPU6000.cpp index 8c510e6..beb8e47 100644 --- a/src/drivers/MPU6000.cpp +++ b/src/drivers/MPU6000.cpp @@ -4,8 +4,20 @@ www.xene.it #include #include "MPU6000.h" +#include +#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(); +} diff --git a/src/drivers/MPU6000.h b/src/drivers/MPU6000.h index bf200da..f425e9e 100644 --- a/src/drivers/MPU6000.h +++ b/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 \ No newline at end of file