Nathaniel Honka / IMUdriver

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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 }