Corrected header file include guards.
Fork of IMUdriver by
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?
User | Revision | Line number | New 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 | } |