Nathaniel Honka / IMUdriver

Corrected header file include guards.

Fork of IMUdriver by HEL's Angels

MPU6000.cpp@2:b54fd8d53035, 2015-02-25 (annotated)

Committer:
cashdollar
Date:
Wed Feb 25 19:21:51 2015 +0000
Revision:
2:b54fd8d53035
Parent:
1:1d985e2d60a6
Child:
3:86d8320a62b4
Added safety check functions. Included doxygen for all new changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
perr1940 0:5c2f529b85f8 1 /*CODED by Bruno Alfano on 07/03/2014
perr1940 0:5c2f529b85f8 2 www.xene.it
perr1940 0:5c2f529b85f8 3 */
perr1940 0:5c2f529b85f8 4
perr1940 0:5c2f529b85f8 5 #include <mbed.h>
perr1940 0:5c2f529b85f8 6 #include "MPU6000.h"
perr1940 0:5c2f529b85f8 7 #include "float.h"
perr1940 0:5c2f529b85f8 8
cashdollar 2:b54fd8d53035 9 mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(0), who_error(0){}
perr1940 0:5c2f529b85f8 10
perr1940 0:5c2f529b85f8 11 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 12 INITIALIZATION
perr1940 0:5c2f529b85f8 13 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
perr1940 0:5c2f529b85f8 14 low pass filter value; suitable values are:
perr1940 0:5c2f529b85f8 15 BITS_DLPF_CFG_256HZ_NOLPF2
perr1940 0:5c2f529b85f8 16 BITS_DLPF_CFG_188HZ
perr1940 0:5c2f529b85f8 17 BITS_DLPF_CFG_98HZ
perr1940 0:5c2f529b85f8 18 BITS_DLPF_CFG_42HZ
perr1940 0:5c2f529b85f8 19 BITS_DLPF_CFG_20HZ
perr1940 0:5c2f529b85f8 20 BITS_DLPF_CFG_10HZ
perr1940 0:5c2f529b85f8 21 BITS_DLPF_CFG_5HZ
perr1940 0:5c2f529b85f8 22 BITS_DLPF_CFG_2100HZ_NOLPF
perr1940 0:5c2f529b85f8 23 returns 1 if an error occurred
perr1940 0:5c2f529b85f8 24 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 25 bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter)
perr1940 0:5c2f529b85f8 26 {
perr1940 0:5c2f529b85f8 27 unsigned int response;
perr1940 0:5c2f529b85f8 28 spi.format(8,0);
perr1940 0:5c2f529b85f8 29 spi.frequency(1000000);
perr1940 0:5c2f529b85f8 30 //FIRST OF ALL DISABLE I2C
perr1940 0:5c2f529b85f8 31 select();
perr1940 0:5c2f529b85f8 32 response=spi.write(MPUREG_USER_CTRL);
perr1940 0:5c2f529b85f8 33 response=spi.write(BIT_I2C_IF_DIS);
perr1940 0:5c2f529b85f8 34 deselect();
perr1940 0:5c2f529b85f8 35 //RESET CHIP
perr1940 0:5c2f529b85f8 36 select();
perr1940 0:5c2f529b85f8 37 response=spi.write(MPUREG_PWR_MGMT_1);
perr1940 0:5c2f529b85f8 38 response=spi.write(BIT_H_RESET);
perr1940 0:5c2f529b85f8 39 deselect();
perr1940 0:5c2f529b85f8 40 wait(0.15);
perr1940 0:5c2f529b85f8 41 //WAKE UP AND SET GYROZ CLOCK
perr1940 0:5c2f529b85f8 42 select();
perr1940 0:5c2f529b85f8 43 response=spi.write(MPUREG_PWR_MGMT_1);
perr1940 0:5c2f529b85f8 44 response=spi.write(MPU_CLK_SEL_PLLGYROZ);
perr1940 0:5c2f529b85f8 45 deselect();
perr1940 0:5c2f529b85f8 46 //DISABLE I2C
perr1940 0:5c2f529b85f8 47 select();
perr1940 0:5c2f529b85f8 48 response=spi.write(MPUREG_USER_CTRL);
perr1940 0:5c2f529b85f8 49 response=spi.write(BIT_I2C_IF_DIS);
perr1940 0:5c2f529b85f8 50 deselect();
cashdollar 1:1d985e2d60a6 51
perr1940 0:5c2f529b85f8 52 //WHO AM I?
cashdollar 1:1d985e2d60a6 53 // Begin transmission
perr1940 0:5c2f529b85f8 54 select();
cashdollar 1:1d985e2d60a6 55 // Bitwise OR of WHOAMI Register (default 0x68) and READ_FLAG
perr1940 0:5c2f529b85f8 56 response=spi.write(MPUREG_WHOAMI|READ_FLAG);
perr1940 0:5c2f529b85f8 57 response=spi.write(0x00);
cashdollar 1:1d985e2d60a6 58 // end transmission
perr1940 0:5c2f529b85f8 59 deselect();
perr1940 0:5c2f529b85f8 60 if(response<100) {
perr1940 0:5c2f529b85f8 61 return 0; //COULDN'T RECEIVE WHOAMI
perr1940 0:5c2f529b85f8 62 }
cashdollar 1:1d985e2d60a6 63
perr1940 0:5c2f529b85f8 64 //SET SAMPLE RATE
perr1940 0:5c2f529b85f8 65 select();
perr1940 0:5c2f529b85f8 66 response=spi.write(MPUREG_SMPLRT_DIV);
perr1940 0:5c2f529b85f8 67 response=spi.write(sample_rate_div);
perr1940 0:5c2f529b85f8 68 deselect();
perr1940 0:5c2f529b85f8 69 // FS & DLPF
perr1940 0:5c2f529b85f8 70 select();
perr1940 0:5c2f529b85f8 71 response=spi.write(MPUREG_CONFIG);
perr1940 0:5c2f529b85f8 72 response=spi.write(low_pass_filter);
perr1940 0:5c2f529b85f8 73 deselect();
perr1940 0:5c2f529b85f8 74 //DISABLE INTERRUPTS
perr1940 0:5c2f529b85f8 75 select();
perr1940 0:5c2f529b85f8 76 response=spi.write(MPUREG_INT_ENABLE);
perr1940 0:5c2f529b85f8 77 response=spi.write(0x00);
perr1940 0:5c2f529b85f8 78 deselect();
perr1940 0:5c2f529b85f8 79 return 0;
perr1940 0:5c2f529b85f8 80 }
perr1940 0:5c2f529b85f8 81
perr1940 0:5c2f529b85f8 82 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 83 ACCELEROMETER SCALE
perr1940 0:5c2f529b85f8 84 usage: call this function at startup, after initialization, to set the right range for the
perr1940 0:5c2f529b85f8 85 accelerometers. Suitable ranges are:
perr1940 0:5c2f529b85f8 86 BITS_FS_2G
perr1940 0:5c2f529b85f8 87 BITS_FS_4G
perr1940 0:5c2f529b85f8 88 BITS_FS_8G
perr1940 0:5c2f529b85f8 89 BITS_FS_16G
perr1940 0:5c2f529b85f8 90 returns the range set (2,4,8 or 16)
perr1940 0:5c2f529b85f8 91 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 92 unsigned int mpu6000_spi::set_acc_scale(int scale)
perr1940 0:5c2f529b85f8 93 {
perr1940 0:5c2f529b85f8 94 unsigned int temp_scale;
perr1940 0:5c2f529b85f8 95 select();
perr1940 0:5c2f529b85f8 96 spi.write(MPUREG_ACCEL_CONFIG);
perr1940 0:5c2f529b85f8 97 spi.write(scale);
perr1940 0:5c2f529b85f8 98 deselect();
perr1940 0:5c2f529b85f8 99 switch (scale) {
perr1940 0:5c2f529b85f8 100 case BITS_FS_2G:
perr1940 0:5c2f529b85f8 101 acc_divider=16384;
perr1940 0:5c2f529b85f8 102 break;
perr1940 0:5c2f529b85f8 103 case BITS_FS_4G:
perr1940 0:5c2f529b85f8 104 acc_divider=8192;
perr1940 0:5c2f529b85f8 105 break;
perr1940 0:5c2f529b85f8 106 case BITS_FS_8G:
perr1940 0:5c2f529b85f8 107 acc_divider=4096;
perr1940 0:5c2f529b85f8 108 break;
perr1940 0:5c2f529b85f8 109 case BITS_FS_16G:
perr1940 0:5c2f529b85f8 110 acc_divider=2048;
perr1940 0:5c2f529b85f8 111 break;
perr1940 0:5c2f529b85f8 112 }
perr1940 0:5c2f529b85f8 113 wait(0.01);
perr1940 0:5c2f529b85f8 114 select();
perr1940 0:5c2f529b85f8 115 temp_scale=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 116 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 117 deselect();
perr1940 0:5c2f529b85f8 118 switch (temp_scale) {
perr1940 0:5c2f529b85f8 119 case BITS_FS_2G:
perr1940 0:5c2f529b85f8 120 temp_scale=2;
perr1940 0:5c2f529b85f8 121 break;
perr1940 0:5c2f529b85f8 122 case BITS_FS_4G:
perr1940 0:5c2f529b85f8 123 temp_scale=4;
perr1940 0:5c2f529b85f8 124 break;
perr1940 0:5c2f529b85f8 125 case BITS_FS_8G:
perr1940 0:5c2f529b85f8 126 temp_scale=8;
perr1940 0:5c2f529b85f8 127 break;
perr1940 0:5c2f529b85f8 128 case BITS_FS_16G:
perr1940 0:5c2f529b85f8 129 temp_scale=16;
perr1940 0:5c2f529b85f8 130 break;
perr1940 0:5c2f529b85f8 131 }
perr1940 0:5c2f529b85f8 132 return temp_scale;
perr1940 0:5c2f529b85f8 133 }
perr1940 0:5c2f529b85f8 134
perr1940 0:5c2f529b85f8 135
perr1940 0:5c2f529b85f8 136 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 137 GYROSCOPE SCALE
perr1940 0:5c2f529b85f8 138 usage: call this function at startup, after initialization, to set the right range for the
perr1940 0:5c2f529b85f8 139 gyroscopes. Suitable ranges are:
perr1940 0:5c2f529b85f8 140 BITS_FS_250DPS
perr1940 0:5c2f529b85f8 141 BITS_FS_500DPS
perr1940 0:5c2f529b85f8 142 BITS_FS_1000DPS
perr1940 0:5c2f529b85f8 143 BITS_FS_2000DPS
perr1940 0:5c2f529b85f8 144 returns the range set (250,500,1000 or 2000)
perr1940 0:5c2f529b85f8 145 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 146 unsigned int mpu6000_spi::set_gyro_scale(int scale)
perr1940 0:5c2f529b85f8 147 {
perr1940 0:5c2f529b85f8 148 unsigned int temp_scale;
perr1940 0:5c2f529b85f8 149 select();
perr1940 0:5c2f529b85f8 150 spi.write(MPUREG_GYRO_CONFIG);
perr1940 0:5c2f529b85f8 151 spi.write(scale);
perr1940 0:5c2f529b85f8 152 deselect();
perr1940 0:5c2f529b85f8 153 switch (scale) {
perr1940 0:5c2f529b85f8 154 case BITS_FS_250DPS:
perr1940 0:5c2f529b85f8 155 gyro_divider=131;
perr1940 0:5c2f529b85f8 156 break;
perr1940 0:5c2f529b85f8 157 case BITS_FS_500DPS:
perr1940 0:5c2f529b85f8 158 gyro_divider=65.5;
perr1940 0:5c2f529b85f8 159 break;
perr1940 0:5c2f529b85f8 160 case BITS_FS_1000DPS:
perr1940 0:5c2f529b85f8 161 gyro_divider=32.8;
perr1940 0:5c2f529b85f8 162 break;
perr1940 0:5c2f529b85f8 163 case BITS_FS_2000DPS:
perr1940 0:5c2f529b85f8 164 gyro_divider=16.4;
perr1940 0:5c2f529b85f8 165 break;
perr1940 0:5c2f529b85f8 166 }
perr1940 0:5c2f529b85f8 167 wait(0.01);
perr1940 0:5c2f529b85f8 168 select();
perr1940 0:5c2f529b85f8 169 temp_scale=spi.write(MPUREG_GYRO_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 170 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 171 deselect();
perr1940 0:5c2f529b85f8 172 switch (temp_scale) {
perr1940 0:5c2f529b85f8 173 case BITS_FS_250DPS:
perr1940 0:5c2f529b85f8 174 temp_scale=250;
perr1940 0:5c2f529b85f8 175 break;
perr1940 0:5c2f529b85f8 176 case BITS_FS_500DPS:
perr1940 0:5c2f529b85f8 177 temp_scale=500;
perr1940 0:5c2f529b85f8 178 break;
perr1940 0:5c2f529b85f8 179 case BITS_FS_1000DPS:
perr1940 0:5c2f529b85f8 180 temp_scale=1000;
perr1940 0:5c2f529b85f8 181 break;
perr1940 0:5c2f529b85f8 182 case BITS_FS_2000DPS:
perr1940 0:5c2f529b85f8 183 temp_scale=2000;
perr1940 0:5c2f529b85f8 184 break;
perr1940 0:5c2f529b85f8 185 }
perr1940 0:5c2f529b85f8 186 return temp_scale;
perr1940 0:5c2f529b85f8 187 }
perr1940 0:5c2f529b85f8 188
perr1940 0:5c2f529b85f8 189
perr1940 0:5c2f529b85f8 190 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 191 WHO AM I?
perr1940 0:5c2f529b85f8 192 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
perr1940 0:5c2f529b85f8 193 mpu6000 which should be 104 when in SPI mode.
perr1940 0:5c2f529b85f8 194 returns the I2C address (104)
perr1940 0:5c2f529b85f8 195 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 196 unsigned int mpu6000_spi::whoami()
perr1940 0:5c2f529b85f8 197 {
cashdollar 2:b54fd8d53035 198 // create instance of response
perr1940 0:5c2f529b85f8 199 unsigned int response;
cashdollar 2:b54fd8d53035 200
cashdollar 1:1d985e2d60a6 201 // Begin transmission
perr1940 0:5c2f529b85f8 202 select();
cashdollar 2:b54fd8d53035 203
cashdollar 1:1d985e2d60a6 204 // Bitwise OR of WHOAMI Register (default 0x68) and READ_FLAG
perr1940 0:5c2f529b85f8 205 response=spi.write(MPUREG_WHOAMI|READ_FLAG);
perr1940 0:5c2f529b85f8 206 response=spi.write(0x00);
cashdollar 2:b54fd8d53035 207
cashdollar 1:1d985e2d60a6 208 // End transmission
perr1940 0:5c2f529b85f8 209 deselect();
cashdollar 2:b54fd8d53035 210
perr1940 0:5c2f529b85f8 211 return response;
perr1940 0:5c2f529b85f8 212 }
perr1940 0:5c2f529b85f8 213
cashdollar 1:1d985e2d60a6 214 /***********************************************************
cashdollar 1:1d985e2d60a6 215 Function: whoami_check()
cashdollar 1:1d985e2d60a6 216 Access Specifier: Public
cashdollar 1:1d985e2d60a6 217 Use: Safety check. Compares WHO_AM_I response to expected response (See description of whoami() for expected response detail).
cashdollar 1:1d985e2d60a6 218 Input: None
cashdollar 1:1d985e2d60a6 219 Output: who_error. High = Safe. Low = Unsafe
cashdollar 1:1d985e2d60a6 220 ***********************************************************/
cashdollar 1:1d985e2d60a6 221 int mpu6000_spi::whoami_check()
cashdollar 1:1d985e2d60a6 222 {
cashdollar 2:b54fd8d53035 223 // mask whoami() against expected value
cashdollar 2:b54fd8d53035 224 return whoami()!=0x68;
cashdollar 1:1d985e2d60a6 225 }
cashdollar 1:1d985e2d60a6 226
perr1940 0:5c2f529b85f8 227
perr1940 0:5c2f529b85f8 228 /*------------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 229 Get Tilt Angle from Accerometer
perr1940 0:5c2f529b85f8 230 8/21/2014 edited by Grace (Yi-Wen Liao)
perr1940 0:5c2f529b85f8 231 ------------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 232 float mpu6000_spi::getAccTilt()
perr1940 0:5c2f529b85f8 233 {
perr1940 0:5c2f529b85f8 234 float Z,X;
perr1940 0:5c2f529b85f8 235 wait(0.0001);
perr1940 0:5c2f529b85f8 236 Z = read_acc(2);
perr1940 0:5c2f529b85f8 237 wait_us(10);
perr1940 0:5c2f529b85f8 238 X=read_acc(0);
perr1940 0:5c2f529b85f8 239 float temp=Z/X;
perr1940 0:5c2f529b85f8 240 if(temp !=temp) {
perr1940 0:5c2f529b85f8 241 if (X>=0 && Z>=0) {
perr1940 0:5c2f529b85f8 242 temp=FLT_MAX;
perr1940 0:5c2f529b85f8 243 } else {
perr1940 0:5c2f529b85f8 244 temp=FLT_MIN;
perr1940 0:5c2f529b85f8 245 }
perr1940 0:5c2f529b85f8 246 }
perr1940 0:5c2f529b85f8 247 /*if(Z>=0 && X<=0) {
perr1940 0:5c2f529b85f8 248 return atan(temp)*180/pi-180;
perr1940 0:5c2f529b85f8 249 }
perr1940 0:5c2f529b85f8 250 if(Z<=0 && X<=0) {
perr1940 0:5c2f529b85f8 251 return atan(temp)*180/pi+180;*/
perr1940 0:5c2f529b85f8 252 //}else{
perr1940 0:5c2f529b85f8 253 return atan(temp)*180/pi;
perr1940 0:5c2f529b85f8 254 //}
perr1940 0:5c2f529b85f8 255 //printf("Z=%f\n\r",Z);
perr1940 0:5c2f529b85f8 256 /*if (Z >= 0 && Z < 0.7071) {
perr1940 0:5c2f529b85f8 257 wait(0.0001);
perr1940 0:5c2f529b85f8 258 if (read_acc(0) >= 0) sign = 1;
perr1940 0:5c2f529b85f8 259 else sign = -1;
perr1940 0:5c2f529b85f8 260 angleData = (pio2 - atan(Z/sqrt(1-Z*Z)))*sign*180/pi; //arccos
perr1940 0:5c2f529b85f8 261 } else if (Z >= 0.7071) {
perr1940 0:5c2f529b85f8 262 wait(0.0001);
perr1940 0:5c2f529b85f8 263 X = read_acc(0);
perr1940 0:5c2f529b85f8 264 angleData = atan(X/sqrt(1-X*X))*180/pi; //arcsin
perr1940 0:5c2f529b85f8 265 }
perr1940 0:5c2f529b85f8 266 return angleData;*/
perr1940 0:5c2f529b85f8 267 }
perr1940 0:5c2f529b85f8 268
perr1940 0:5c2f529b85f8 269
perr1940 0:5c2f529b85f8 270 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 271 READ ACCELEROMETER
perr1940 0:5c2f529b85f8 272 usage: call this function to read accelerometer data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 273 0 -> X axis
perr1940 0:5c2f529b85f8 274 1 -> Y axis
perr1940 0:5c2f529b85f8 275 2 -> Z axis
perr1940 0:5c2f529b85f8 276 returns the value in Gs
perr1940 0:5c2f529b85f8 277 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 278 float mpu6000_spi::read_acc(int axis)
perr1940 0:5c2f529b85f8 279 {
perr1940 0:5c2f529b85f8 280 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 281 int16_t bit_data;
perr1940 0:5c2f529b85f8 282 float data;
perr1940 0:5c2f529b85f8 283 select();
perr1940 0:5c2f529b85f8 284 switch (axis) {
perr1940 0:5c2f529b85f8 285 case 0:
perr1940 0:5c2f529b85f8 286 responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 287 break;
perr1940 0:5c2f529b85f8 288 case 1:
perr1940 0:5c2f529b85f8 289 responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 290 break;
perr1940 0:5c2f529b85f8 291 case 2:
perr1940 0:5c2f529b85f8 292 responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 293 break;
perr1940 0:5c2f529b85f8 294 }
perr1940 0:5c2f529b85f8 295 // wait(0.0001);
perr1940 0:5c2f529b85f8 296 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 297 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 298 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 299 data=(float)bit_data;
perr1940 0:5c2f529b85f8 300 data = data/acc_divider;
perr1940 0:5c2f529b85f8 301 deselect();
perr1940 0:5c2f529b85f8 302 //printf("data = %f\n\r",data);
perr1940 0:5c2f529b85f8 303 return data;
perr1940 0:5c2f529b85f8 304 }
perr1940 0:5c2f529b85f8 305
perr1940 0:5c2f529b85f8 306 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 307 READ GYROSCOPE
perr1940 0:5c2f529b85f8 308 usage: call this function to read gyroscope data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 309 0 -> X axis
perr1940 0:5c2f529b85f8 310 1 -> Y axis
perr1940 0:5c2f529b85f8 311 2 -> Z axis
perr1940 0:5c2f529b85f8 312 returns the value in Degrees per second
perr1940 0:5c2f529b85f8 313 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 314 float mpu6000_spi::read_rot(int axis)
perr1940 0:5c2f529b85f8 315 {
perr1940 0:5c2f529b85f8 316 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 317 int16_t bit_data;
perr1940 0:5c2f529b85f8 318 float data;
perr1940 0:5c2f529b85f8 319 select();
perr1940 0:5c2f529b85f8 320 switch (axis) {
perr1940 0:5c2f529b85f8 321 case 0:
perr1940 0:5c2f529b85f8 322 responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 323 break;
perr1940 0:5c2f529b85f8 324 case 1:
perr1940 0:5c2f529b85f8 325 responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 326 break;
perr1940 0:5c2f529b85f8 327 case 2:
perr1940 0:5c2f529b85f8 328 responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 329 break;
perr1940 0:5c2f529b85f8 330 }
perr1940 0:5c2f529b85f8 331 wait(0.0001);
perr1940 0:5c2f529b85f8 332 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 333 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 334 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 335 data=(float)bit_data;
perr1940 0:5c2f529b85f8 336 data=data/gyro_divider;
perr1940 0:5c2f529b85f8 337 deselect();
perr1940 0:5c2f529b85f8 338 return data;
perr1940 0:5c2f529b85f8 339 }
perr1940 0:5c2f529b85f8 340
perr1940 0:5c2f529b85f8 341 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 342 READ TEMPERATURE
perr1940 0:5c2f529b85f8 343 usage: call this function to read temperature data.
perr1940 0:5c2f529b85f8 344 returns the value in °C
perr1940 0:5c2f529b85f8 345 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 346 float mpu6000_spi::read_temp()
perr1940 0:5c2f529b85f8 347 {
perr1940 0:5c2f529b85f8 348 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 349 int16_t bit_data;
perr1940 0:5c2f529b85f8 350 float data;
perr1940 0:5c2f529b85f8 351 select();
perr1940 0:5c2f529b85f8 352 responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 353 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 354 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 355 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 356 data=(float)bit_data;
perr1940 0:5c2f529b85f8 357 data=(data/340)+36.53;
perr1940 0:5c2f529b85f8 358 deselect();
perr1940 0:5c2f529b85f8 359 return data;
perr1940 0:5c2f529b85f8 360 }
perr1940 0:5c2f529b85f8 361
perr1940 0:5c2f529b85f8 362 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 363 READ ACCELEROMETER CALIBRATION
perr1940 0:5c2f529b85f8 364 usage: call this function to read accelerometer data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 365 0 -> X axis
perr1940 0:5c2f529b85f8 366 1 -> Y axis
perr1940 0:5c2f529b85f8 367 2 -> Z axis
perr1940 0:5c2f529b85f8 368 returns Factory Trim value
perr1940 0:5c2f529b85f8 369 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 370 int mpu6000_spi::calib_acc(int axis)
perr1940 0:5c2f529b85f8 371 {
perr1940 0:5c2f529b85f8 372 uint8_t responseH,responseL,calib_data;
perr1940 0:5c2f529b85f8 373 int temp_scale;
perr1940 0:5c2f529b85f8 374 //READ CURRENT ACC SCALE
perr1940 0:5c2f529b85f8 375 select();
perr1940 0:5c2f529b85f8 376 responseH=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 377 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 378 deselect();
perr1940 0:5c2f529b85f8 379 wait(0.01);
perr1940 0:5c2f529b85f8 380 set_acc_scale(BITS_FS_8G);
perr1940 0:5c2f529b85f8 381 wait(0.01);
perr1940 0:5c2f529b85f8 382 //ENABLE SELF TEST
perr1940 0:5c2f529b85f8 383 select();
perr1940 0:5c2f529b85f8 384 responseH=spi.write(MPUREG_ACCEL_CONFIG);
perr1940 0:5c2f529b85f8 385 temp_scale=spi.write(0x80>>axis);
perr1940 0:5c2f529b85f8 386 deselect();
perr1940 0:5c2f529b85f8 387 wait(0.01);
perr1940 0:5c2f529b85f8 388 select();
perr1940 0:5c2f529b85f8 389 responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
perr1940 0:5c2f529b85f8 390 switch(axis) {
perr1940 0:5c2f529b85f8 391 case 0:
perr1940 0:5c2f529b85f8 392 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 393 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 394 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 395 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 396 calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
perr1940 0:5c2f529b85f8 397 break;
perr1940 0:5c2f529b85f8 398 case 1:
perr1940 0:5c2f529b85f8 399 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 400 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 401 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 402 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 403 calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
perr1940 0:5c2f529b85f8 404 break;
perr1940 0:5c2f529b85f8 405 case 2:
perr1940 0:5c2f529b85f8 406 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 407 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 408 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 409 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 410 calib_data=((responseH&11100000)>>3)|((responseL&00000011));
perr1940 0:5c2f529b85f8 411 break;
perr1940 0:5c2f529b85f8 412 }
perr1940 0:5c2f529b85f8 413 deselect();
perr1940 0:5c2f529b85f8 414 wait(0.01);
perr1940 0:5c2f529b85f8 415 set_acc_scale(temp_scale);
perr1940 0:5c2f529b85f8 416 return calib_data;
perr1940 0:5c2f529b85f8 417 }
perr1940 0:5c2f529b85f8 418
perr1940 0:5c2f529b85f8 419 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 420 SPI SELECT AND DESELECT
perr1940 0:5c2f529b85f8 421 usage: enable and disable mpu6000 communication bus
perr1940 0:5c2f529b85f8 422 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 423 void mpu6000_spi::select()
perr1940 0:5c2f529b85f8 424 {
perr1940 0:5c2f529b85f8 425 //Set CS low to start transmission (interrupts conversion)
perr1940 0:5c2f529b85f8 426 cs = 0;
perr1940 0:5c2f529b85f8 427 }
perr1940 0:5c2f529b85f8 428 void mpu6000_spi::deselect()
perr1940 0:5c2f529b85f8 429 {
perr1940 0:5c2f529b85f8 430 //Set CS high to stop transmission (restarts conversion)
perr1940 0:5c2f529b85f8 431 cs = 1;
perr1940 0:5c2f529b85f8 432 }
perr1940 0:5c2f529b85f8 433
perr1940 0:5c2f529b85f8 434 float mpu6000_spi::angle_y()
perr1940 0:5c2f529b85f8 435 {
perr1940 0:5c2f529b85f8 436 float gyro = read_rot(1);
perr1940 0:5c2f529b85f8 437 float acc = getAccTilt();
perr1940 0:5c2f529b85f8 438
perr1940 0:5c2f529b85f8 439 accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
perr1940 0:5c2f529b85f8 440 gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
perr1940 0:5c2f529b85f8 441
perr1940 0:5c2f529b85f8 442 //pc.printf("\n\nWHOAMI=%u\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104
perr1940 0:5c2f529b85f8 443 //pc.printf("acc=%f\n\r",acc);
perr1940 0:5c2f529b85f8 444 //pc.printf("GYRO=%f\n\r",gyro);
perr1940 0:5c2f529b85f8 445 accFilterPre = accFilterCurrent;
perr1940 0:5c2f529b85f8 446 gyroFliterPre = gyroFilterCurrent;
perr1940 0:5c2f529b85f8 447 return accFilterCurrent + gyroFilterCurrent;
perr1940 0:5c2f529b85f8 448 }