philip/balancing-robot
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
balancing-robot/src/drivers/MPU6000.cpp
philsson d1bceb2117 IMU changes. Calibration options 
Disable standby mode at init
Can now calculate offset on acc and gyro
Fifo reset function
2020-06-13 15:45:42 +02:00

373 lines
8.8 KiB
C++
Raw Blame History

/*CODED by Bruno Alfano on 07/03/2014
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)
{
_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){
unsigned int response;
spi.format(8,0);
spi.frequency(1000000);
//FIRST OF ALL DISABLE I2C
select();
response=spi.write(MPUREG_USER_CTRL);
response=spi.write(BIT_I2C_IF_DIS);
deselect();
//RESET CHIP
select();
response=spi.write(MPUREG_PWR_MGMT_1);
response=spi.write(BIT_H_RESET);
deselect();
wait(0.15);
//WAKE UP AND SET GYROZ CLOCK
select();
response=spi.write(MPUREG_PWR_MGMT_1);
response=spi.write(MPU_CLK_SEL_PLLGYROZ);
deselect();
//DISABLE I2C
select();
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);
response=spi.write(0x00);
deselect();
if(response<100){return 0;}//COULDN'T RECEIVE WHOAMI
//SET SAMPLE RATE
select();
response=spi.write(MPUREG_SMPLRT_DIV);
response=spi.write(sample_rate_div);
deselect();
// FS & DLPF
select();
response=spi.write(MPUREG_CONFIG);
response=spi.write(low_pass_filter);
deselect();
//DISABLE INTERRUPTS
select();
response=spi.write(MPUREG_INT_ENABLE);
response=spi.write(0x00);
deselect();
return true; // TODO
}
int mpu6000_spi::enableInterrupt()
{
unsigned int response;
//ENABLE INTERRUPTS
select();
response=spi.write(MPUREG_INT_ENABLE);
response=spi.write(0x01);
deselect();
return 0;
}
unsigned int mpu6000_spi::set_acc_scale(int scale){
unsigned int temp_scale;
select();
spi.write(MPUREG_ACCEL_CONFIG);
spi.write(scale);
deselect();
switch (scale){
case BITS_FS_2G:
acc_divider=16384;
break;
case BITS_FS_4G:
acc_divider=8192;
break;
case BITS_FS_8G:
acc_divider=4096;
break;
case BITS_FS_16G:
acc_divider=2048;
break;
}
wait(0.01);
select();
temp_scale=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
temp_scale=spi.write(0x00);
deselect();
switch (temp_scale){
case BITS_FS_2G:
temp_scale=2;
break;
case BITS_FS_4G:
temp_scale=4;
break;
case BITS_FS_8G:
temp_scale=8;
break;
case BITS_FS_16G:
temp_scale=16;
break;
}
return temp_scale;
}
unsigned int mpu6000_spi::set_gyro_scale(int scale){
unsigned int temp_scale;
select();
spi.write(MPUREG_GYRO_CONFIG);
spi.write(scale);
deselect();
switch (scale){
case BITS_FS_250DPS:
gyro_divider=131;
break;
case BITS_FS_500DPS:
gyro_divider=65.5;
break;
case BITS_FS_1000DPS:
gyro_divider=32.8;
break;
case BITS_FS_2000DPS:
gyro_divider=16.4;
break;
}
wait(0.01);
select();
temp_scale=spi.write(MPUREG_GYRO_CONFIG|READ_FLAG);
temp_scale=spi.write(0x00);
deselect();
switch (temp_scale){
case BITS_FS_250DPS:
temp_scale=250;
break;
case BITS_FS_500DPS:
temp_scale=500;
break;
case BITS_FS_1000DPS:
temp_scale=1000;
break;
case BITS_FS_2000DPS:
temp_scale=2000;
break;
}
return temp_scale;
}
unsigned int mpu6000_spi::whoami(){
unsigned int response;
select();
response=spi.write(MPUREG_WHOAMI|READ_FLAG);
response=spi.write(0x00);
deselect();
return response;
}
float mpu6000_spi::read_acc(int axis){
uint8_t responseH,responseL;
int16_t bit_data;
float data;
select();
switch (axis){
case 0:
responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
break;
case 1:
responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
break;
case 2:
responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
break;
}
responseH=spi.write(0x00);
responseL=spi.write(0x00);
bit_data=((int16_t)responseH<<8)|responseL;
data=(float)bit_data;
data=data/acc_divider;
deselect();
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){
uint8_t responseH,responseL;
int16_t bit_data;
float data;
select();
switch (axis){
case 0:
responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
break;
case 1:
responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
break;
case 2:
responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
break;
}
responseH=spi.write(0x00);
responseL=spi.write(0x00);
bit_data=((int16_t)responseH<<8)|responseL;
data=(float)bit_data;
data=data/gyro_divider;
deselect();
return data - _gyro_offset[axis];
}
float mpu6000_spi::read_temp(){
uint8_t responseH,responseL;
int16_t bit_data;
float data;
select();
responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
responseH=spi.write(0x00);
responseL=spi.write(0x00);
bit_data=((int16_t)responseH<<8)|responseL;
data=(float)bit_data;
data=(data/340)+36.53;
deselect();
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;
//READ CURRENT ACC SCALE
select();
responseH=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
temp_scale=spi.write(0x00);
deselect();
wait(0.01);
set_acc_scale(BITS_FS_8G);
wait(0.01);
//ENABLE SELF TEST
select();
responseH=spi.write(MPUREG_ACCEL_CONFIG);
temp_scale=spi.write(0x80>>axis);
deselect();
wait(0.01);
select();
responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
switch(axis){
case 0:
responseH=spi.write(0x00);
responseL=spi.write(0x00);
responseL=spi.write(0x00);
responseL=spi.write(0x00);
calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
break;
case 1:
responseH=spi.write(0x00);
responseH=spi.write(0x00);
responseL=spi.write(0x00);
responseL=spi.write(0x00);
calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
break;
case 2:
responseH=spi.write(0x00);
responseH=spi.write(0x00);
responseH=spi.write(0x00);
responseL=spi.write(0x00);
calib_data=((responseH&11100000)>>3)|((responseL&00000011));
break;
}
deselect();
wait(0.01);
set_acc_scale(temp_scale);
return calib_data;
}
void mpu6000_spi::select() {
//Set CS low to start transmission (interrupts conversion)
cs = 0;
}
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();
}
Reference in New Issue View Git Blame Copy Permalink