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@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 }