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