Revision 0:2edbd561b520, committed 2014-03-08
- Comitter:
- brunoalfano
- Date:
- Sat Mar 08 12:38:14 2014 +0000
- Commit message:
- First version of a simple MPU6000 SPI library to read sensors' data
Changed in this revision
diff -r 000000000000 -r 2edbd561b520 MPU6000.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6000.cpp Sat Mar 08 12:38:14 2014 +0000
@@ -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;
+}
\ No newline at end of file
diff -r 000000000000 -r 2edbd561b520 MPU6000.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6000.h Sat Mar 08 12:38:14 2014 +0000
@@ -0,0 +1,162 @@
+/*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
+
+