philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

IMU changes. Calibration options

Browse Source 
Disable standby mode at init
Can now calculate offset on acc and gyro
Fifo reset function
This commit is contained in:
philsson 2018-09-02 19:12:46 +02:00
parent 15dd8e6bc7
commit 1fd690b3f5
2 changed files with 119 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
src/drivers/MPU6000.cpp
View File

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

28
src/drivers/MPU6000.h
View File

@ -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