Corrected header file include guards.
Fork of IMUdriver by
Diff: MPU6000.cpp
- Revision:
- 0:5c2f529b85f8
- Child:
- 1:1d985e2d60a6
diff -r 000000000000 -r 5c2f529b85f8 MPU6000.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6000.cpp Mon Nov 24 03:33:43 2014 +0000 @@ -0,0 +1,422 @@ +/*CODED by Bruno Alfano on 07/03/2014 +www.xene.it +*/ + +#include <mbed.h> +#include "MPU6000.h" +#include "float.h" + +mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(0) {} + +/*----------------------------------------------------------------------------------------------- + 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; +} + + +/*------------------------------------------------------------------------------------------------ + Get Tilt Angle from Accerometer +8/21/2014 edited by Grace (Yi-Wen Liao) +------------------------------------------------------------------------------------------------*/ +float mpu6000_spi::getAccTilt() +{ + float Z,X; + wait(0.0001); + Z = read_acc(2); + wait_us(10); + X=read_acc(0); + float temp=Z/X; + if(temp !=temp) { + if (X>=0 && Z>=0) { + temp=FLT_MAX; + } else { + temp=FLT_MIN; + } + } + /*if(Z>=0 && X<=0) { + return atan(temp)*180/pi-180; + } + if(Z<=0 && X<=0) { + return atan(temp)*180/pi+180;*/ + //}else{ + return atan(temp)*180/pi; + //} + //printf("Z=%f\n\r",Z); + /*if (Z >= 0 && Z < 0.7071) { + wait(0.0001); + if (read_acc(0) >= 0) sign = 1; + else sign = -1; + angleData = (pio2 - atan(Z/sqrt(1-Z*Z)))*sign*180/pi; //arccos + } else if (Z >= 0.7071) { + wait(0.0001); + X = read_acc(0); + angleData = atan(X/sqrt(1-X*X))*180/pi; //arcsin + } + return angleData;*/ +} + + +/*----------------------------------------------------------------------------------------------- + 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; + } +// wait(0.0001); + responseH=spi.write(0x00); + responseL=spi.write(0x00); + bit_data=((int16_t)responseH<<8)|responseL; + data=(float)bit_data; + data = data/acc_divider; + deselect(); + //printf("data = %f\n\r",data); + 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; + } + wait(0.0001); + 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; +} + +float mpu6000_spi::angle_y() +{ + float gyro = read_rot(1); + float acc = getAccTilt(); + + accFilterCurrent = 0.8187*accFilterPre+0.1813*acc; + gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro; + + //pc.printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104 + //pc.printf("acc=%f\n\r",acc); + //pc.printf("GYRO=%f\n\r",gyro); + accFilterPre = accFilterCurrent; + gyroFliterPre = gyroFilterCurrent; + return accFilterCurrent + gyroFilterCurrent; +}