Nathaniel Honka / IMUdriver

Corrected header file include guards.

Fork of IMUdriver by HEL's Angels

MPU6000.cpp@0:5c2f529b85f8, 2014-11-24 (annotated)

Committer:
perr1940
Date:
Mon Nov 24 03:33:43 2014 +0000
Revision:
0:5c2f529b85f8
Child:
1:1d985e2d60a6
IMU stuff... going to Italy!

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
perr1940 0:5c2f529b85f8 9 mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs), accFilterCurrent(0), accFilterPre(0), gyroFilterCurrent(0), gyroFliterPre(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();
perr1940 0:5c2f529b85f8 51 //WHO AM I?
perr1940 0:5c2f529b85f8 52 select();
perr1940 0:5c2f529b85f8 53 response=spi.write(MPUREG_WHOAMI|READ_FLAG);
perr1940 0:5c2f529b85f8 54 response=spi.write(0x00);
perr1940 0:5c2f529b85f8 55 deselect();
perr1940 0:5c2f529b85f8 56 if(response<100) {
perr1940 0:5c2f529b85f8 57 return 0; //COULDN'T RECEIVE WHOAMI
perr1940 0:5c2f529b85f8 58 }
perr1940 0:5c2f529b85f8 59 //SET SAMPLE RATE
perr1940 0:5c2f529b85f8 60 select();
perr1940 0:5c2f529b85f8 61 response=spi.write(MPUREG_SMPLRT_DIV);
perr1940 0:5c2f529b85f8 62 response=spi.write(sample_rate_div);
perr1940 0:5c2f529b85f8 63 deselect();
perr1940 0:5c2f529b85f8 64 // FS & DLPF
perr1940 0:5c2f529b85f8 65 select();
perr1940 0:5c2f529b85f8 66 response=spi.write(MPUREG_CONFIG);
perr1940 0:5c2f529b85f8 67 response=spi.write(low_pass_filter);
perr1940 0:5c2f529b85f8 68 deselect();
perr1940 0:5c2f529b85f8 69 //DISABLE INTERRUPTS
perr1940 0:5c2f529b85f8 70 select();
perr1940 0:5c2f529b85f8 71 response=spi.write(MPUREG_INT_ENABLE);
perr1940 0:5c2f529b85f8 72 response=spi.write(0x00);
perr1940 0:5c2f529b85f8 73 deselect();
perr1940 0:5c2f529b85f8 74 return 0;
perr1940 0:5c2f529b85f8 75 }
perr1940 0:5c2f529b85f8 76
perr1940 0:5c2f529b85f8 77 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 78 ACCELEROMETER SCALE
perr1940 0:5c2f529b85f8 79 usage: call this function at startup, after initialization, to set the right range for the
perr1940 0:5c2f529b85f8 80 accelerometers. Suitable ranges are:
perr1940 0:5c2f529b85f8 81 BITS_FS_2G
perr1940 0:5c2f529b85f8 82 BITS_FS_4G
perr1940 0:5c2f529b85f8 83 BITS_FS_8G
perr1940 0:5c2f529b85f8 84 BITS_FS_16G
perr1940 0:5c2f529b85f8 85 returns the range set (2,4,8 or 16)
perr1940 0:5c2f529b85f8 86 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 87 unsigned int mpu6000_spi::set_acc_scale(int scale)
perr1940 0:5c2f529b85f8 88 {
perr1940 0:5c2f529b85f8 89 unsigned int temp_scale;
perr1940 0:5c2f529b85f8 90 select();
perr1940 0:5c2f529b85f8 91 spi.write(MPUREG_ACCEL_CONFIG);
perr1940 0:5c2f529b85f8 92 spi.write(scale);
perr1940 0:5c2f529b85f8 93 deselect();
perr1940 0:5c2f529b85f8 94 switch (scale) {
perr1940 0:5c2f529b85f8 95 case BITS_FS_2G:
perr1940 0:5c2f529b85f8 96 acc_divider=16384;
perr1940 0:5c2f529b85f8 97 break;
perr1940 0:5c2f529b85f8 98 case BITS_FS_4G:
perr1940 0:5c2f529b85f8 99 acc_divider=8192;
perr1940 0:5c2f529b85f8 100 break;
perr1940 0:5c2f529b85f8 101 case BITS_FS_8G:
perr1940 0:5c2f529b85f8 102 acc_divider=4096;
perr1940 0:5c2f529b85f8 103 break;
perr1940 0:5c2f529b85f8 104 case BITS_FS_16G:
perr1940 0:5c2f529b85f8 105 acc_divider=2048;
perr1940 0:5c2f529b85f8 106 break;
perr1940 0:5c2f529b85f8 107 }
perr1940 0:5c2f529b85f8 108 wait(0.01);
perr1940 0:5c2f529b85f8 109 select();
perr1940 0:5c2f529b85f8 110 temp_scale=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 111 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 112 deselect();
perr1940 0:5c2f529b85f8 113 switch (temp_scale) {
perr1940 0:5c2f529b85f8 114 case BITS_FS_2G:
perr1940 0:5c2f529b85f8 115 temp_scale=2;
perr1940 0:5c2f529b85f8 116 break;
perr1940 0:5c2f529b85f8 117 case BITS_FS_4G:
perr1940 0:5c2f529b85f8 118 temp_scale=4;
perr1940 0:5c2f529b85f8 119 break;
perr1940 0:5c2f529b85f8 120 case BITS_FS_8G:
perr1940 0:5c2f529b85f8 121 temp_scale=8;
perr1940 0:5c2f529b85f8 122 break;
perr1940 0:5c2f529b85f8 123 case BITS_FS_16G:
perr1940 0:5c2f529b85f8 124 temp_scale=16;
perr1940 0:5c2f529b85f8 125 break;
perr1940 0:5c2f529b85f8 126 }
perr1940 0:5c2f529b85f8 127 return temp_scale;
perr1940 0:5c2f529b85f8 128 }
perr1940 0:5c2f529b85f8 129
perr1940 0:5c2f529b85f8 130
perr1940 0:5c2f529b85f8 131 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 132 GYROSCOPE SCALE
perr1940 0:5c2f529b85f8 133 usage: call this function at startup, after initialization, to set the right range for the
perr1940 0:5c2f529b85f8 134 gyroscopes. Suitable ranges are:
perr1940 0:5c2f529b85f8 135 BITS_FS_250DPS
perr1940 0:5c2f529b85f8 136 BITS_FS_500DPS
perr1940 0:5c2f529b85f8 137 BITS_FS_1000DPS
perr1940 0:5c2f529b85f8 138 BITS_FS_2000DPS
perr1940 0:5c2f529b85f8 139 returns the range set (250,500,1000 or 2000)
perr1940 0:5c2f529b85f8 140 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 141 unsigned int mpu6000_spi::set_gyro_scale(int scale)
perr1940 0:5c2f529b85f8 142 {
perr1940 0:5c2f529b85f8 143 unsigned int temp_scale;
perr1940 0:5c2f529b85f8 144 select();
perr1940 0:5c2f529b85f8 145 spi.write(MPUREG_GYRO_CONFIG);
perr1940 0:5c2f529b85f8 146 spi.write(scale);
perr1940 0:5c2f529b85f8 147 deselect();
perr1940 0:5c2f529b85f8 148 switch (scale) {
perr1940 0:5c2f529b85f8 149 case BITS_FS_250DPS:
perr1940 0:5c2f529b85f8 150 gyro_divider=131;
perr1940 0:5c2f529b85f8 151 break;
perr1940 0:5c2f529b85f8 152 case BITS_FS_500DPS:
perr1940 0:5c2f529b85f8 153 gyro_divider=65.5;
perr1940 0:5c2f529b85f8 154 break;
perr1940 0:5c2f529b85f8 155 case BITS_FS_1000DPS:
perr1940 0:5c2f529b85f8 156 gyro_divider=32.8;
perr1940 0:5c2f529b85f8 157 break;
perr1940 0:5c2f529b85f8 158 case BITS_FS_2000DPS:
perr1940 0:5c2f529b85f8 159 gyro_divider=16.4;
perr1940 0:5c2f529b85f8 160 break;
perr1940 0:5c2f529b85f8 161 }
perr1940 0:5c2f529b85f8 162 wait(0.01);
perr1940 0:5c2f529b85f8 163 select();
perr1940 0:5c2f529b85f8 164 temp_scale=spi.write(MPUREG_GYRO_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 165 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 166 deselect();
perr1940 0:5c2f529b85f8 167 switch (temp_scale) {
perr1940 0:5c2f529b85f8 168 case BITS_FS_250DPS:
perr1940 0:5c2f529b85f8 169 temp_scale=250;
perr1940 0:5c2f529b85f8 170 break;
perr1940 0:5c2f529b85f8 171 case BITS_FS_500DPS:
perr1940 0:5c2f529b85f8 172 temp_scale=500;
perr1940 0:5c2f529b85f8 173 break;
perr1940 0:5c2f529b85f8 174 case BITS_FS_1000DPS:
perr1940 0:5c2f529b85f8 175 temp_scale=1000;
perr1940 0:5c2f529b85f8 176 break;
perr1940 0:5c2f529b85f8 177 case BITS_FS_2000DPS:
perr1940 0:5c2f529b85f8 178 temp_scale=2000;
perr1940 0:5c2f529b85f8 179 break;
perr1940 0:5c2f529b85f8 180 }
perr1940 0:5c2f529b85f8 181 return temp_scale;
perr1940 0:5c2f529b85f8 182 }
perr1940 0:5c2f529b85f8 183
perr1940 0:5c2f529b85f8 184
perr1940 0:5c2f529b85f8 185 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 186 WHO AM I?
perr1940 0:5c2f529b85f8 187 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
perr1940 0:5c2f529b85f8 188 mpu6000 which should be 104 when in SPI mode.
perr1940 0:5c2f529b85f8 189 returns the I2C address (104)
perr1940 0:5c2f529b85f8 190 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 191 unsigned int mpu6000_spi::whoami()
perr1940 0:5c2f529b85f8 192 {
perr1940 0:5c2f529b85f8 193 unsigned int response;
perr1940 0:5c2f529b85f8 194 select();
perr1940 0:5c2f529b85f8 195 response=spi.write(MPUREG_WHOAMI|READ_FLAG);
perr1940 0:5c2f529b85f8 196 response=spi.write(0x00);
perr1940 0:5c2f529b85f8 197 deselect();
perr1940 0:5c2f529b85f8 198 return response;
perr1940 0:5c2f529b85f8 199 }
perr1940 0:5c2f529b85f8 200
perr1940 0:5c2f529b85f8 201
perr1940 0:5c2f529b85f8 202 /*------------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 203 Get Tilt Angle from Accerometer
perr1940 0:5c2f529b85f8 204 8/21/2014 edited by Grace (Yi-Wen Liao)
perr1940 0:5c2f529b85f8 205 ------------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 206 float mpu6000_spi::getAccTilt()
perr1940 0:5c2f529b85f8 207 {
perr1940 0:5c2f529b85f8 208 float Z,X;
perr1940 0:5c2f529b85f8 209 wait(0.0001);
perr1940 0:5c2f529b85f8 210 Z = read_acc(2);
perr1940 0:5c2f529b85f8 211 wait_us(10);
perr1940 0:5c2f529b85f8 212 X=read_acc(0);
perr1940 0:5c2f529b85f8 213 float temp=Z/X;
perr1940 0:5c2f529b85f8 214 if(temp !=temp) {
perr1940 0:5c2f529b85f8 215 if (X>=0 && Z>=0) {
perr1940 0:5c2f529b85f8 216 temp=FLT_MAX;
perr1940 0:5c2f529b85f8 217 } else {
perr1940 0:5c2f529b85f8 218 temp=FLT_MIN;
perr1940 0:5c2f529b85f8 219 }
perr1940 0:5c2f529b85f8 220 }
perr1940 0:5c2f529b85f8 221 /*if(Z>=0 && X<=0) {
perr1940 0:5c2f529b85f8 222 return atan(temp)*180/pi-180;
perr1940 0:5c2f529b85f8 223 }
perr1940 0:5c2f529b85f8 224 if(Z<=0 && X<=0) {
perr1940 0:5c2f529b85f8 225 return atan(temp)*180/pi+180;*/
perr1940 0:5c2f529b85f8 226 //}else{
perr1940 0:5c2f529b85f8 227 return atan(temp)*180/pi;
perr1940 0:5c2f529b85f8 228 //}
perr1940 0:5c2f529b85f8 229 //printf("Z=%f\n\r",Z);
perr1940 0:5c2f529b85f8 230 /*if (Z >= 0 && Z < 0.7071) {
perr1940 0:5c2f529b85f8 231 wait(0.0001);
perr1940 0:5c2f529b85f8 232 if (read_acc(0) >= 0) sign = 1;
perr1940 0:5c2f529b85f8 233 else sign = -1;
perr1940 0:5c2f529b85f8 234 angleData = (pio2 - atan(Z/sqrt(1-Z*Z)))*sign*180/pi; //arccos
perr1940 0:5c2f529b85f8 235 } else if (Z >= 0.7071) {
perr1940 0:5c2f529b85f8 236 wait(0.0001);
perr1940 0:5c2f529b85f8 237 X = read_acc(0);
perr1940 0:5c2f529b85f8 238 angleData = atan(X/sqrt(1-X*X))*180/pi; //arcsin
perr1940 0:5c2f529b85f8 239 }
perr1940 0:5c2f529b85f8 240 return angleData;*/
perr1940 0:5c2f529b85f8 241 }
perr1940 0:5c2f529b85f8 242
perr1940 0:5c2f529b85f8 243
perr1940 0:5c2f529b85f8 244 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 245 READ ACCELEROMETER
perr1940 0:5c2f529b85f8 246 usage: call this function to read accelerometer data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 247 0 -> X axis
perr1940 0:5c2f529b85f8 248 1 -> Y axis
perr1940 0:5c2f529b85f8 249 2 -> Z axis
perr1940 0:5c2f529b85f8 250 returns the value in Gs
perr1940 0:5c2f529b85f8 251 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 252 float mpu6000_spi::read_acc(int axis)
perr1940 0:5c2f529b85f8 253 {
perr1940 0:5c2f529b85f8 254 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 255 int16_t bit_data;
perr1940 0:5c2f529b85f8 256 float data;
perr1940 0:5c2f529b85f8 257 select();
perr1940 0:5c2f529b85f8 258 switch (axis) {
perr1940 0:5c2f529b85f8 259 case 0:
perr1940 0:5c2f529b85f8 260 responseH=spi.write(MPUREG_ACCEL_XOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 261 break;
perr1940 0:5c2f529b85f8 262 case 1:
perr1940 0:5c2f529b85f8 263 responseH=spi.write(MPUREG_ACCEL_YOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 264 break;
perr1940 0:5c2f529b85f8 265 case 2:
perr1940 0:5c2f529b85f8 266 responseH=spi.write(MPUREG_ACCEL_ZOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 267 break;
perr1940 0:5c2f529b85f8 268 }
perr1940 0:5c2f529b85f8 269 // wait(0.0001);
perr1940 0:5c2f529b85f8 270 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 271 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 272 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 273 data=(float)bit_data;
perr1940 0:5c2f529b85f8 274 data = data/acc_divider;
perr1940 0:5c2f529b85f8 275 deselect();
perr1940 0:5c2f529b85f8 276 //printf("data = %f\n\r",data);
perr1940 0:5c2f529b85f8 277 return data;
perr1940 0:5c2f529b85f8 278 }
perr1940 0:5c2f529b85f8 279
perr1940 0:5c2f529b85f8 280 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 281 READ GYROSCOPE
perr1940 0:5c2f529b85f8 282 usage: call this function to read gyroscope data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 283 0 -> X axis
perr1940 0:5c2f529b85f8 284 1 -> Y axis
perr1940 0:5c2f529b85f8 285 2 -> Z axis
perr1940 0:5c2f529b85f8 286 returns the value in Degrees per second
perr1940 0:5c2f529b85f8 287 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 288 float mpu6000_spi::read_rot(int axis)
perr1940 0:5c2f529b85f8 289 {
perr1940 0:5c2f529b85f8 290 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 291 int16_t bit_data;
perr1940 0:5c2f529b85f8 292 float data;
perr1940 0:5c2f529b85f8 293 select();
perr1940 0:5c2f529b85f8 294 switch (axis) {
perr1940 0:5c2f529b85f8 295 case 0:
perr1940 0:5c2f529b85f8 296 responseH=spi.write(MPUREG_GYRO_XOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 297 break;
perr1940 0:5c2f529b85f8 298 case 1:
perr1940 0:5c2f529b85f8 299 responseH=spi.write(MPUREG_GYRO_YOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 300 break;
perr1940 0:5c2f529b85f8 301 case 2:
perr1940 0:5c2f529b85f8 302 responseH=spi.write(MPUREG_GYRO_ZOUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 303 break;
perr1940 0:5c2f529b85f8 304 }
perr1940 0:5c2f529b85f8 305 wait(0.0001);
perr1940 0:5c2f529b85f8 306 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 307 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 308 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 309 data=(float)bit_data;
perr1940 0:5c2f529b85f8 310 data=data/gyro_divider;
perr1940 0:5c2f529b85f8 311 deselect();
perr1940 0:5c2f529b85f8 312 return data;
perr1940 0:5c2f529b85f8 313 }
perr1940 0:5c2f529b85f8 314
perr1940 0:5c2f529b85f8 315 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 316 READ TEMPERATURE
perr1940 0:5c2f529b85f8 317 usage: call this function to read temperature data.
perr1940 0:5c2f529b85f8 318 returns the value in °C
perr1940 0:5c2f529b85f8 319 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 320 float mpu6000_spi::read_temp()
perr1940 0:5c2f529b85f8 321 {
perr1940 0:5c2f529b85f8 322 uint8_t responseH,responseL;
perr1940 0:5c2f529b85f8 323 int16_t bit_data;
perr1940 0:5c2f529b85f8 324 float data;
perr1940 0:5c2f529b85f8 325 select();
perr1940 0:5c2f529b85f8 326 responseH=spi.write(MPUREG_TEMP_OUT_H | READ_FLAG);
perr1940 0:5c2f529b85f8 327 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 328 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 329 bit_data=((int16_t)responseH<<8)|responseL;
perr1940 0:5c2f529b85f8 330 data=(float)bit_data;
perr1940 0:5c2f529b85f8 331 data=(data/340)+36.53;
perr1940 0:5c2f529b85f8 332 deselect();
perr1940 0:5c2f529b85f8 333 return data;
perr1940 0:5c2f529b85f8 334 }
perr1940 0:5c2f529b85f8 335
perr1940 0:5c2f529b85f8 336 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 337 READ ACCELEROMETER CALIBRATION
perr1940 0:5c2f529b85f8 338 usage: call this function to read accelerometer data. Axis represents selected axis:
perr1940 0:5c2f529b85f8 339 0 -> X axis
perr1940 0:5c2f529b85f8 340 1 -> Y axis
perr1940 0:5c2f529b85f8 341 2 -> Z axis
perr1940 0:5c2f529b85f8 342 returns Factory Trim value
perr1940 0:5c2f529b85f8 343 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 344 int mpu6000_spi::calib_acc(int axis)
perr1940 0:5c2f529b85f8 345 {
perr1940 0:5c2f529b85f8 346 uint8_t responseH,responseL,calib_data;
perr1940 0:5c2f529b85f8 347 int temp_scale;
perr1940 0:5c2f529b85f8 348 //READ CURRENT ACC SCALE
perr1940 0:5c2f529b85f8 349 select();
perr1940 0:5c2f529b85f8 350 responseH=spi.write(MPUREG_ACCEL_CONFIG|READ_FLAG);
perr1940 0:5c2f529b85f8 351 temp_scale=spi.write(0x00);
perr1940 0:5c2f529b85f8 352 deselect();
perr1940 0:5c2f529b85f8 353 wait(0.01);
perr1940 0:5c2f529b85f8 354 set_acc_scale(BITS_FS_8G);
perr1940 0:5c2f529b85f8 355 wait(0.01);
perr1940 0:5c2f529b85f8 356 //ENABLE SELF TEST
perr1940 0:5c2f529b85f8 357 select();
perr1940 0:5c2f529b85f8 358 responseH=spi.write(MPUREG_ACCEL_CONFIG);
perr1940 0:5c2f529b85f8 359 temp_scale=spi.write(0x80>>axis);
perr1940 0:5c2f529b85f8 360 deselect();
perr1940 0:5c2f529b85f8 361 wait(0.01);
perr1940 0:5c2f529b85f8 362 select();
perr1940 0:5c2f529b85f8 363 responseH=spi.write(MPUREG_SELF_TEST_X|READ_FLAG);
perr1940 0:5c2f529b85f8 364 switch(axis) {
perr1940 0:5c2f529b85f8 365 case 0:
perr1940 0:5c2f529b85f8 366 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 367 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 368 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 369 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 370 calib_data=((responseH&11100000)>>3)|((responseL&00110000)>>4);
perr1940 0:5c2f529b85f8 371 break;
perr1940 0:5c2f529b85f8 372 case 1:
perr1940 0:5c2f529b85f8 373 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 374 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 375 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 376 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 377 calib_data=((responseH&11100000)>>3)|((responseL&00001100)>>2);
perr1940 0:5c2f529b85f8 378 break;
perr1940 0:5c2f529b85f8 379 case 2:
perr1940 0:5c2f529b85f8 380 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 381 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 382 responseH=spi.write(0x00);
perr1940 0:5c2f529b85f8 383 responseL=spi.write(0x00);
perr1940 0:5c2f529b85f8 384 calib_data=((responseH&11100000)>>3)|((responseL&00000011));
perr1940 0:5c2f529b85f8 385 break;
perr1940 0:5c2f529b85f8 386 }
perr1940 0:5c2f529b85f8 387 deselect();
perr1940 0:5c2f529b85f8 388 wait(0.01);
perr1940 0:5c2f529b85f8 389 set_acc_scale(temp_scale);
perr1940 0:5c2f529b85f8 390 return calib_data;
perr1940 0:5c2f529b85f8 391 }
perr1940 0:5c2f529b85f8 392
perr1940 0:5c2f529b85f8 393 /*-----------------------------------------------------------------------------------------------
perr1940 0:5c2f529b85f8 394 SPI SELECT AND DESELECT
perr1940 0:5c2f529b85f8 395 usage: enable and disable mpu6000 communication bus
perr1940 0:5c2f529b85f8 396 -----------------------------------------------------------------------------------------------*/
perr1940 0:5c2f529b85f8 397 void mpu6000_spi::select()
perr1940 0:5c2f529b85f8 398 {
perr1940 0:5c2f529b85f8 399 //Set CS low to start transmission (interrupts conversion)
perr1940 0:5c2f529b85f8 400 cs = 0;
perr1940 0:5c2f529b85f8 401 }
perr1940 0:5c2f529b85f8 402 void mpu6000_spi::deselect()
perr1940 0:5c2f529b85f8 403 {
perr1940 0:5c2f529b85f8 404 //Set CS high to stop transmission (restarts conversion)
perr1940 0:5c2f529b85f8 405 cs = 1;
perr1940 0:5c2f529b85f8 406 }
perr1940 0:5c2f529b85f8 407
perr1940 0:5c2f529b85f8 408 float mpu6000_spi::angle_y()
perr1940 0:5c2f529b85f8 409 {
perr1940 0:5c2f529b85f8 410 float gyro = read_rot(1);
perr1940 0:5c2f529b85f8 411 float acc = getAccTilt();
perr1940 0:5c2f529b85f8 412
perr1940 0:5c2f529b85f8 413 accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
perr1940 0:5c2f529b85f8 414 gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
perr1940 0:5c2f529b85f8 415
perr1940 0:5c2f529b85f8 416 //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 417 //pc.printf("acc=%f\n\r",acc);
perr1940 0:5c2f529b85f8 418 //pc.printf("GYRO=%f\n\r",gyro);
perr1940 0:5c2f529b85f8 419 accFilterPre = accFilterCurrent;
perr1940 0:5c2f529b85f8 420 gyroFliterPre = gyroFilterCurrent;
perr1940 0:5c2f529b85f8 421 return accFilterCurrent + gyroFilterCurrent;
perr1940 0:5c2f529b85f8 422 }