Nathaniel Honka / IMUdriver

Corrected header file include guards.

Fork of IMUdriver by HEL's Angels

MPU6000.cpp@9:878fd1610cd2, 2015-07-02 (annotated)

Committer:
nathanhonka
Date:
Thu Jul 02 21:38:28 2015 +0000
Revision:
9:878fd1610cd2
Parent:
8:a8f6366e4b7b 
Corrected header file include guards (did not encapsulate entire header).

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 6:d9198ff2dcf9 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);
mzling 3:86d8320a62b4 29 spi.frequency(10000000);
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 6:d9198ff2dcf9 200
cashdollar 1:1d985e2d60a6 201 // Begin transmission
perr1940 0:5c2f529b85f8 202 select();
cashdollar 6:d9198ff2dcf9 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 6:d9198ff2dcf9 207
cashdollar 1:1d985e2d60a6 208 // End transmission
perr1940 0:5c2f529b85f8 209 deselect();
cashdollar 6:d9198ff2dcf9 210
perr1940 0:5c2f529b85f8 211 return response;
perr1940 0:5c2f529b85f8 212 }
perr1940 0:5c2f529b85f8 213
cashdollar 6:d9198ff2dcf9 214 /** High = Safe. Low = Unsafe */
perr1940 8:a8f6366e4b7b 215 int mpu6000_spi::whoamiCheck()
cashdollar 1:1d985e2d60a6 216 {
cashdollar 2:b54fd8d53035 217 // mask whoami() against expected value
cashdollar 2:b54fd8d53035 218 return whoami()!=0x68;
cashdollar 1:1d985e2d60a6 219 }
cashdollar 1:1d985e2d60a6 220
perr1940 0:5c2f529b85f8 221
cashdollar 7:5e299dbf98ff 222 // Get Tilt Angle from Accelerometer
perr1940 0:5c2f529b85f8 223 float mpu6000_spi::getAccTilt()
perr1940 0:5c2f529b85f8 224 {
perr1940 0:5c2f529b85f8 225 float Z,X;
perr1940 0:5c2f529b85f8 226 wait(0.0001);
perr1940 0:5c2f529b85f8 227 Z = read_acc(2);
perr1940 0:5c2f529b85f8 228 wait_us(10);
perr1940 0:5c2f529b85f8 229 X=read_acc(0);
perr1940 0:5c2f529b85f8 230 float temp=Z/X;
perr1940 0:5c2f529b85f8 231 if(temp !=temp) {
perr1940 0:5c2f529b85f8 232 if (X>=0 && Z>=0) {
perr1940 0:5c2f529b85f8 233 temp=FLT_MAX;
perr1940 0:5c2f529b85f8 234 } else {
perr1940 0:5c2f529b85f8 235 temp=FLT_MIN;
perr1940 0:5c2f529b85f8 236 }
perr1940 0:5c2f529b85f8 237 }
cashdollar 6:d9198ff2dcf9 238
perr1940 0:5c2f529b85f8 239 return atan(temp)*180/pi;
cashdollar 6:d9198ff2dcf9 240
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
cashdollar 6:d9198ff2dcf9 413 // complementary filter
perr1940 0:5c2f529b85f8 414 accFilterCurrent = 0.8187*accFilterPre+0.1813*acc;
perr1940 0:5c2f529b85f8 415 gyroFilterCurrent = 0.8187*gyroFliterPre+0.0009063*gyro;
perr1940 0:5c2f529b85f8 416
perr1940 0:5c2f529b85f8 417 //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 418 //pc.printf("acc=%f\n\r",acc);
perr1940 0:5c2f529b85f8 419 //pc.printf("GYRO=%f\n\r",gyro);
perr1940 0:5c2f529b85f8 420 accFilterPre = accFilterCurrent;
perr1940 0:5c2f529b85f8 421 gyroFliterPre = gyroFilterCurrent;
perr1940 0:5c2f529b85f8 422 return accFilterCurrent + gyroFilterCurrent;
perr1940 0:5c2f529b85f8 423 }
mzling 4:492c7470dbfb 424
cashdollar 6:d9198ff2dcf9 425 float mpu6000_spi::rereadAngle_y()
mzling 4:492c7470dbfb 426 {
mzling 4:492c7470dbfb 427 return accFilterCurrent + gyroFilterCurrent;
mzling 4:492c7470dbfb 428 }