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

MPU drivers

Browse Source 
Drivers for accelerometer and gyro
This commit is contained in:
philsson 2018-08-25 11:35:35 +02:00
parent 0c5806dc2e
commit bd22dd6ab3
2 changed files with 509 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

348
src/drivers/MPU6000.cpp Normal file
View File

@ -0,0 +1,348 @@
/*CODED by Bruno Alfano on 07/03/2014
www.xene.it
*/
#include <mbed.h>
#include "MPU6000.h"
mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
/*-----------------------------------------------------------------------------------------------
INITIALIZATION
usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
low pass filter value; suitable values are:
BITS_DLPF_CFG_256HZ_NOLPF2
BITS_DLPF_CFG_188HZ
BITS_DLPF_CFG_98HZ
BITS_DLPF_CFG_42HZ
BITS_DLPF_CFG_20HZ
BITS_DLPF_CFG_10HZ
BITS_DLPF_CFG_5HZ
BITS_DLPF_CFG_2100HZ_NOLPF
returns 1 if an error occurred
-----------------------------------------------------------------------------------------------*/
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();
//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 0;
}
/*-----------------------------------------------------------------------------------------------
ACCELEROMETER SCALE
usage: call this function at startup, after initialization, to set the right range for the
accelerometers. Suitable ranges are:
BITS_FS_2G
BITS_FS_4G
BITS_FS_8G
BITS_FS_16G
returns the range set (2,4,8 or 16)
-----------------------------------------------------------------------------------------------*/
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;
}
/*-----------------------------------------------------------------------------------------------
GYROSCOPE SCALE
usage: call this function at startup, after initialization, to set the right range for the
gyroscopes. Suitable ranges are:
BITS_FS_250DPS
BITS_FS_500DPS
BITS_FS_1000DPS
BITS_FS_2000DPS
returns the range set (250,500,1000 or 2000)
-----------------------------------------------------------------------------------------------*/
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;
}
/*-----------------------------------------------------------------------------------------------
WHO AM I?
usage: call this function to know if SPI is working correctly. It checks the I2C address of the
mpu6000 which should be 104 when in SPI mode.
returns the I2C address (104)
-----------------------------------------------------------------------------------------------*/
unsigned int mpu6000_spi::whoami(){
unsigned int response;
select();
response=spi.write(MPUREG_WHOAMI|READ_FLAG);
response=spi.write(0x00);
deselect();
return response;
}
/*-----------------------------------------------------------------------------------------------
READ ACCELEROMETER
usage: call this function to read accelerometer data. Axis represents selected axis:
0 -> X axis
1 -> Y axis
2 -> Z axis
returns the value in Gs
-----------------------------------------------------------------------------------------------*/
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;
}
/*-----------------------------------------------------------------------------------------------
READ GYROSCOPE
usage: call this function to read gyroscope data. Axis represents selected axis:
0 -> X axis
1 -> Y axis
2 -> Z axis
returns the value in Degrees per second
-----------------------------------------------------------------------------------------------*/
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;
}
/*-----------------------------------------------------------------------------------------------
READ TEMPERATURE
usage: call this function to read temperature data.
returns the value in °C
-----------------------------------------------------------------------------------------------*/
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;
}
/*-----------------------------------------------------------------------------------------------
READ ACCELEROMETER CALIBRATION
usage: call this function to read accelerometer data. Axis represents selected axis:
0 -> X axis
1 -> Y axis
2 -> Z axis
returns Factory Trim value
-----------------------------------------------------------------------------------------------*/
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;
}
/*-----------------------------------------------------------------------------------------------
SPI SELECT AND DESELECT
usage: enable and disable mpu6000 communication bus
-----------------------------------------------------------------------------------------------*/
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;
}

161
src/drivers/MPU6000.h Normal file
View File

@ -0,0 +1,161 @@
/*CODED by Bruno Alfano on 07/03/2014
www.xene.it
USAGE (example program):
#include "mbed.h"
#include "MPU6000.h" //Include library
SPI spi(p11, p12, p13); //define the SPI (mosi, miso, sclk)
mpu6000_spi imu(spi,p22); //define the mpu6000 object
int main(){
if(imu.init(1,BITS_DLPF_CFG_5HZ)){ //INIT the mpu6000
printf("\nCouldn't initialize MPU6000 via SPI!");
}
wait(0.1);
printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
wait(0.1);
printf("\nGyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
wait(1);
printf("\nAcc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
wait(0.1);
while(1) {
myled = 1;
wait(0.3);
myled = 0;
wait(0.3);
printf("\nT=%.3f",imu.read_temp());
printf(" X=%.3f",imu.read_acc(0));
printf(" Y=%.3f",imu.read_acc(1));
printf(" Z=%.3f",imu.read_acc(2));
printf(" rX=%.3f",imu.read_rot(0));
printf(" rY=%.3f",imu.read_rot(1));
printf(" rZ=%.3f",imu.read_rot(2));
}
}
*/
#ifndef MPU6000_h
#define MPU6000_h
#include "mbed.h"
class mpu6000_spi
{
SPI& spi;
DigitalOut cs;
public:
mpu6000_spi(SPI& _spi, PinName _cs);
bool init(int sample_rate_div,int low_pass_filter);
float read_acc(int axis);
float read_rot(int axis);
unsigned int set_gyro_scale(int scale);
unsigned int set_acc_scale(int scale);
int calib_acc(int axis);
float read_temp();
void select();
void deselect();
unsigned int whoami();
float acc_divider;
float gyro_divider;
private:
PinName _CS_pin;
PinName _SO_pin;
PinName _SCK_pin;
float _error;
};
#endif
// MPU6000 registers
#define MPUREG_XG_OFFS_TC 0x00
#define MPUREG_YG_OFFS_TC 0x01
#define MPUREG_ZG_OFFS_TC 0x02
#define MPUREG_X_FINE_GAIN 0x03
#define MPUREG_Y_FINE_GAIN 0x04
#define MPUREG_Z_FINE_GAIN 0x05
#define MPUREG_XA_OFFS_H 0x06
#define MPUREG_XA_OFFS_L 0x07
#define MPUREG_YA_OFFS_H 0x08
#define MPUREG_YA_OFFS_L 0x09
#define MPUREG_ZA_OFFS_H 0x0A
#define MPUREG_ZA_OFFS_L 0x0B
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_SELF_TEST_X 0x0D
#define MPUREG_SELF_TEST_Y 0x0E
#define MPUREG_SELF_TEST_Z 0x0F
#define MPUREG_SELF_TEST_A 0x10
#define MPUREG_XG_OFFS_USRH 0x13
#define MPUREG_XG_OFFS_USRL 0x14
#define MPUREG_YG_OFFS_USRH 0x15
#define MPUREG_YG_OFFS_USRL 0x16
#define MPUREG_ZG_OFFS_USRH 0x17
#define MPUREG_ZG_OFFS_USRL 0x18
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_BANK_SEL 0x6D
#define MPUREG_MEM_START_ADDR 0x6E
#define MPUREG_MEM_R_W 0x6F
#define MPUREG_DMP_CFG_1 0x70
#define MPUREG_DMP_CFG_2 0x71
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_WHOAMI 0x75
// Configuration bits MPU6000
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_2G 0x00
#define BITS_FS_4G 0x08
#define BITS_FS_8G 0x10
#define BITS_FS_16G 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define READ_FLAG 0x80