Nathaniel Honka / IMUdriver

Corrected header file include guards.

Fork of IMUdriver by HEL's Angels

MPU6000.cpp@1:1d985e2d60a6, 2015-02-13 (annotated)

Committer:
cashdollar
Date:
Fri Feb 13 23:00:03 2015 +0000
Revision:
1:1d985e2d60a6
Parent:
0:5c2f529b85f8
Child:
2:b54fd8d53035
Implemented safety checks. Added function whoami_check() to compare expected value of whoami and actual value of whoami.

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 1:1d985e2d60a6 9 mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(0), whoami_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 {
perr1940 0:5c2f529b85f8 198 unsigned int response;
cashdollar 1:1d985e2d60a6 199 // Begin transmission
perr1940 0:5c2f529b85f8 200 select();
cashdollar 1:1d985e2d60a6 201 // Bitwise OR of WHOAMI Register (default 0x68) and READ_FLAG
perr1940 0:5c2f529b85f8 202 response=spi.write(MPUREG_WHOAMI|READ_FLAG);
perr1940 0:5c2f529b85f8 203 response=spi.write(0x00);
cashdollar 1:1d985e2d60a6 204 // End transmission
perr1940 0:5c2f529b85f8 205 deselect();
perr1940 0:5c2f529b85f8 206 return response;
perr1940 0:5c2f529b85f8 207 }
perr1940 0:5c2f529b85f8 208
cashdollar 1:1d985e2d60a6 209 /***********************************************************
cashdollar 1:1d985e2d60a6 210 Function: whoami_check()
cashdollar 1:1d985e2d60a6 211 Access Specifier: Public
cashdollar 1:1d985e2d60a6 212 Use: Safety check. Compares WHO_AM_I response to expected response (See description of whoami() for expected response detail).
cashdollar 1:1d985e2d60a6 213 Input: None
cashdollar 1:1d985e2d60a6 214 Output: who_error. High = Safe. Low = Unsafe
cashdollar 1:1d985e2d60a6 215 ***********************************************************/
cashdollar 1:1d985e2d60a6 216 int mpu6000_spi::whoami_check()
cashdollar 1:1d985e2d60a6 217 {
cashdollar 1:1d985e2d60a6 218 // bitwise OR of response and 104. 104 is expected response
cashdollar 1:1d985e2d60a6 219 who_error = (response||0x104) // all high means whoami is safe
cashdollar 1:1d985e2d60a6 220
cashdollar 1:1d985e2d60a6 221 // if whoami is not all high/safe
cashdollar 1:1d985e2d60a6 222 if who_error == !0x000{
cashdollar 1:1d985e2d60a6 223
cashdollar 1:1d985e2d60a6 224 // then raise the value of who_error to indicate unsafe state
cashdollar 1:1d985e2d60a6 225 who_error=1
cashdollar 1:1d985e2d60a6 226
cashdollar 1:1d985e2d60a6 227 }
cashdollar 1:1d985e2d60a6 228 return who_error;
cashdollar 1:1d985e2d60a6 229 }
cashdollar 1:1d985e2d60a6 230
perr1940 0:5c2f529b85f8 231
perr1940 0:5c2f529b85f8 232 /*------------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 233 Get Tilt Angle from Accerometer
perr1940 0:5c2f529b85f8 234 8/21/2014 edited by Grace (Yi-Wen Liao)
perr1940 0:5c2f529b85f8 235 ------------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 236 float mpu6000_spi::getAccTilt()
perr1940 0:5c2f529b85f8 237 {
perr1940 0:5c2f529b85f8 238 float Z,X;
perr1940 0:5c2f529b85f8 239 wait(0.0001);
perr1940 0:5c2f529b85f8 240 Z = read_acc(2);
perr1940 0:5c2f529b85f8 241 wait_us(10);
perr1940 0:5c2f529b85f8 242 X=read_acc(0);
perr1940 0:5c2f529b85f8 243 float temp=Z/X;
perr1940 0:5c2f529b85f8 244 if(temp !=temp) {
perr1940 0:5c2f529b85f8 245 if (X>=0 && Z>=0) {
perr1940 0:5c2f529b85f8 246 temp=FLT_MAX;
perr1940 0:5c2f529b85f8 247 } else {
perr1940 0:5c2f529b85f8 248 temp=FLT_MIN;
perr1940 0:5c2f529b85f8 249 }
perr1940 0:5c2f529b85f8 250 }
perr1940 0:5c2f529b85f8 251 /*if(Z>=0 && X<=0) {
perr1940 0:5c2f529b85f8 252 return atan(temp)*180/pi-180;
perr1940 0:5c2f529b85f8 253 }
perr1940 0:5c2f529b85f8 254 if(Z<=0 && X<=0) {
perr1940 0:5c2f529b85f8 255 return atan(temp)*180/pi+180;*/
perr1940 0:5c2f529b85f8 256 //}else{
perr1940 0:5c2f529b85f8 257 return atan(temp)*180/pi;
perr1940 0:5c2f529b85f8 258 //}
perr1940 0:5c2f529b85f8 259 //printf("Z=%f\n\r",Z);
perr1940 0:5c2f529b85f8 260 /*if (Z >= 0 && Z < 0.7071) {
perr1940 0:5c2f529b85f8 261 wait(0.0001);
perr1940 0:5c2f529b85f8 262 if (read_acc(0) >= 0) sign = 1;
perr1940 0:5c2f529b85f8 263 else sign = -1;
perr1940 0:5c2f529b85f8 264 angleData = (pio2 - atan(Z/sqrt(1-Z*Z)))*sign*180/pi; //arccos
perr1940 0:5c2f529b85f8 265 } else if (Z >= 0.7071) {
perr1940 0:5c2f529b85f8 266 wait(0.0001);
perr1940 0:5c2f529b85f8 267 X = read_acc(0);
perr1940 0:5c2f529b85f8 268 angleData = atan(X/sqrt(1-X*X))*180/pi; //arcsin
perr1940 0:5c2f529b85f8 269 }
perr1940 0:5c2f529b85f8 270 return angleData;*/
perr1940 0:5c2f529b85f8 271 }
perr1940 0:5c2f529b85f8 272
perr1940 0:5c2f529b85f8 273
perr1940 0:5c2f529b85f8 274 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 275 READ ACCELEROMETER
perr1940 0:5c2f529b85f8 276 usage: call this function to read accelerometer data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 277 0 -> X axis
perr1940 0:5c2f529b85f8 278 1 -> Y axis
perr1940 0:5c2f529b85f8 279 2 -> Z axis
perr1940 0:5c2f529b85f8 280 returns the value in Gs
perr1940 0:5c2f529b85f8 281 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 282 float mpu6000_spi::read_acc(int axis)
perr1940 0:5c2f529b85f8 283 {
perr1940 0:5c2f529b85f8 284 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 285 int16_t bit_data;
perr1940 0:5c2f529b85f8 286 float data;
perr1940 0:5c2f529b85f8 287 select();
perr1940 0:5c2f529b85f8 288 switch (axis) {
perr1940 0:5c2f529b85f8 289 case 0:
perr1940 0:5c2f529b85f8 290 responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 291 break;
perr1940 0:5c2f529b85f8 292 case 1:
perr1940 0:5c2f529b85f8 293 responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 294 break;
perr1940 0:5c2f529b85f8 295 case 2:
perr1940 0:5c2f529b85f8 296 responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 297 break;
perr1940 0:5c2f529b85f8 298 }
perr1940 0:5c2f529b85f8 299 // wait(0.0001);
perr1940 0:5c2f529b85f8 300 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 301 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 302 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 303 data=(float)bit_data;
perr1940 0:5c2f529b85f8 304 data = data/acc_divider;
perr1940 0:5c2f529b85f8 305 deselect();
perr1940 0:5c2f529b85f8 306 //printf("data = %f\n\r",data);
perr1940 0:5c2f529b85f8 307 return data;
perr1940 0:5c2f529b85f8 308 }
perr1940 0:5c2f529b85f8 309
perr1940 0:5c2f529b85f8 310 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 311 READ GYROSCOPE
perr1940 0:5c2f529b85f8 312 usage: call this function to read gyroscope data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 313 0 -> X axis
perr1940 0:5c2f529b85f8 314 1 -> Y axis
perr1940 0:5c2f529b85f8 315 2 -> Z axis
perr1940 0:5c2f529b85f8 316 returns the value in Degrees per second
perr1940 0:5c2f529b85f8 317 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 318 float mpu6000_spi::read_rot(int axis)
perr1940 0:5c2f529b85f8 319 {
perr1940 0:5c2f529b85f8 320 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 321 int16_t bit_data;
perr1940 0:5c2f529b85f8 322 float data;
perr1940 0:5c2f529b85f8 323 select();
perr1940 0:5c2f529b85f8 324 switch (axis) {
perr1940 0:5c2f529b85f8 325 case 0:
perr1940 0:5c2f529b85f8 326 responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 327 break;
perr1940 0:5c2f529b85f8 328 case 1:
perr1940 0:5c2f529b85f8 329 responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 330 break;
perr1940 0:5c2f529b85f8 331 case 2:
perr1940 0:5c2f529b85f8 332 responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 333 break;
perr1940 0:5c2f529b85f8 334 }
perr1940 0:5c2f529b85f8 335 wait(0.0001);
perr1940 0:5c2f529b85f8 336 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 337 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 338 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 339 data=(float)bit_data;
perr1940 0:5c2f529b85f8 340 data=data/gyro_divider;
perr1940 0:5c2f529b85f8 341 deselect();
perr1940 0:5c2f529b85f8 342 return data;
perr1940 0:5c2f529b85f8 343 }
perr1940 0:5c2f529b85f8 344
perr1940 0:5c2f529b85f8 345 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 346 READ TEMPERATURE
perr1940 0:5c2f529b85f8 347 usage: call this function to read temperature data.
perr1940 0:5c2f529b85f8 348 returns the value in °C
perr1940 0:5c2f529b85f8 349 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 350 float mpu6000_spi::read_temp()
perr1940 0:5c2f529b85f8 351 {
perr1940 0:5c2f529b85f8 352 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 353 int16_t bit_data;
perr1940 0:5c2f529b85f8 354 float data;
perr1940 0:5c2f529b85f8 355 select();
perr1940 0:5c2f529b85f8 356 responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 357 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 358 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 359 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 360 data=(float)bit_data;
perr1940 0:5c2f529b85f8 361 data=(data/340)+36.53;
perr1940 0:5c2f529b85f8 362 deselect();
perr1940 0:5c2f529b85f8 363 return data;
perr1940 0:5c2f529b85f8 364 }
perr1940 0:5c2f529b85f8 365
perr1940 0:5c2f529b85f8 366 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 367 READ ACCELEROMETER CALIBRATION
perr1940 0:5c2f529b85f8 368 usage: call this function to read accelerometer data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 369 0 -> X axis
perr1940 0:5c2f529b85f8 370 1 -> Y axis
perr1940 0:5c2f529b85f8 371 2 -> Z axis
perr1940 0:5c2f529b85f8 372 returns Factory Trim value
perr1940 0:5c2f529b85f8 373 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 374 int mpu6000_spi::calib_acc(int axis)
perr1940 0:5c2f529b85f8 375 {
perr1940 0:5c2f529b85f8 376 uint8_t responseH,responseL,calib_data;
perr1940 0:5c2f529b85f8 377 int temp_scale;
perr1940 0:5c2f529b85f8 378 //READ CURRENT ACC SCALE
perr1940 0:5c2f529b85f8 379 select();
perr1940 0:5c2f529b85f8 380 responseH=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 381 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 382 deselect();
perr1940 0:5c2f529b85f8 383 wait(0.01);
perr1940 0:5c2f529b85f8 384 set_acc_scale(BITS_FS_8G);
perr1940 0:5c2f529b85f8 385 wait(0.01);
perr1940 0:5c2f529b85f8 386 //ENABLE SELF TEST
perr1940 0:5c2f529b85f8 387 select();
perr1940 0:5c2f529b85f8 388 responseH=spi.write(MPUREG_ACCEL_CONFIG);
perr1940 0:5c2f529b85f8 389 temp_scale=spi.write(0x80>>axis);
perr1940 0:5c2f529b85f8 390 deselect();
perr1940 0:5c2f529b85f8 391 wait(0.01);
perr1940 0:5c2f529b85f8 392 select();
perr1940 0:5c2f529b85f8 393 responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
perr1940 0:5c2f529b85f8 394 switch(axis) {
perr1940 0:5c2f529b85f8 395 case 0:
perr1940 0:5c2f529b85f8 396 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 397 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 398 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 399 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 400 calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
perr1940 0:5c2f529b85f8 401 break;
perr1940 0:5c2f529b85f8 402 case 1:
perr1940 0:5c2f529b85f8 403 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 404 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 405 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 406 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 407 calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
perr1940 0:5c2f529b85f8 408 break;
perr1940 0:5c2f529b85f8 409 case 2:
perr1940 0:5c2f529b85f8 410 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 411 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 412 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 413 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 414 calib_data=((responseH&11100000)>>3)|((responseL&00000011));
perr1940 0:5c2f529b85f8 415 break;
perr1940 0:5c2f529b85f8 416 }
perr1940 0:5c2f529b85f8 417 deselect();
perr1940 0:5c2f529b85f8 418 wait(0.01);
perr1940 0:5c2f529b85f8 419 set_acc_scale(temp_scale);
perr1940 0:5c2f529b85f8 420 return calib_data;
perr1940 0:5c2f529b85f8 421 }
perr1940 0:5c2f529b85f8 422
perr1940 0:5c2f529b85f8 423 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 424 SPI SELECT AND DESELECT
perr1940 0:5c2f529b85f8 425 usage: enable and disable mpu6000 communication bus
perr1940 0:5c2f529b85f8 426 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 427 void mpu6000_spi::select()
perr1940 0:5c2f529b85f8 428 {
perr1940 0:5c2f529b85f8 429 //Set CS low to start transmission (interrupts conversion)
perr1940 0:5c2f529b85f8 430 cs = 0;
perr1940 0:5c2f529b85f8 431 }
perr1940 0:5c2f529b85f8 432 void mpu6000_spi::deselect()
perr1940 0:5c2f529b85f8 433 {
perr1940 0:5c2f529b85f8 434 //Set CS high to stop transmission (restarts conversion)
perr1940 0:5c2f529b85f8 435 cs = 1;
perr1940 0:5c2f529b85f8 436 }
perr1940 0:5c2f529b85f8 437
perr1940 0:5c2f529b85f8 438 float mpu6000_spi::angle_y()
perr1940 0:5c2f529b85f8 439 {
perr1940 0:5c2f529b85f8 440 float gyro = read_rot(1);
perr1940 0:5c2f529b85f8 441 float acc = getAccTilt();
perr1940 0:5c2f529b85f8 442
perr1940 0:5c2f529b85f8 443 accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
perr1940 0:5c2f529b85f8 444 gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
perr1940 0:5c2f529b85f8 445
perr1940 0:5c2f529b85f8 446 //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 447 //pc.printf("acc=%f\n\r",acc);
perr1940 0:5c2f529b85f8 448 //pc.printf("GYRO=%f\n\r",gyro);
perr1940 0:5c2f529b85f8 449 accFilterPre = accFilterCurrent;
perr1940 0:5c2f529b85f8 450 gyroFliterPre = gyroFilterCurrent;
perr1940 0:5c2f529b85f8 451 return accFilterCurrent + gyroFilterCurrent;
perr1940 0:5c2f529b85f8 452 }