Kiko Ishimoto / MPU9250

MPU9250

Fork of MPU9250_SPI by Mu kylong

MPU9250.cpp@12:a70c193d8195, 2016-05-09 (annotated)

Committer:
kikoaac
Date:
Mon May 09 05:45:18 2016 +0000
Revision:
12:a70c193d8195
Parent:
11:084e8ba240c1 
MPU9250;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kylongmu 0:768d2e151834 1 /*CODED by Qiyong Mu on 21/06/2014
kylongmu 7:1bcd61cdbf18 2 kylongmu@msn.com
kylongmu 0:768d2e151834 3 */
kylongmu 0:768d2e151834 4
kylongmu 0:768d2e151834 5 #include <mbed.h>
kylongmu 1:f738165e54f0 6 #include "MPU9250.h"
kylongmu 0:768d2e151834 7
kikoaac 12:a70c193d8195 8 mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs)
kikoaac 12:a70c193d8195 9 {
kikoaac 12:a70c193d8195 10 /* uint8_t offset[3][2] =
kikoaac 12:a70c193d8195 11 {-21 , MPUREG_XG_OFFS_TC};
kikoaac 12:a70c193d8195 12 for(i=0; i<offset; i++) {
kikoaac 12:a70c193d8195 13 WriteReg(offset[i][1], offset[i][0]);
kikoaac 12:a70c193d8195 14 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
kikoaac 12:a70c193d8195 15 }*/
kikoaac 12:a70c193d8195 16 Magnetometer_offset[0]=0;
kikoaac 12:a70c193d8195 17 Magnetometer_offset[1]=0;
kikoaac 12:a70c193d8195 18 Magnetometer_offset[2]=0;
kikoaac 12:a70c193d8195 19 }
kylongmu 0:768d2e151834 20
kylongmu 0:768d2e151834 21 unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 0:768d2e151834 22 {
kylongmu 0:768d2e151834 23 unsigned int temp_val;
kylongmu 0:768d2e151834 24 select();
kylongmu 0:768d2e151834 25 spi.write(WriteAddr);
kylongmu 0:768d2e151834 26 temp_val=spi.write(WriteData);
kylongmu 0:768d2e151834 27 deselect();
kylongmu 0:768d2e151834 28 wait_us(50);
kylongmu 0:768d2e151834 29 return temp_val;
kylongmu 0:768d2e151834 30 }
kylongmu 2:f274ea3bced9 31 unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
kylongmu 2:f274ea3bced9 32 {
kylongmu 2:f274ea3bced9 33 return WriteReg(WriteAddr | READ_FLAG,WriteData);
kylongmu 2:f274ea3bced9 34 }
kylongmu 2:f274ea3bced9 35 void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
kylongmu 0:768d2e151834 36 {
kylongmu 0:768d2e151834 37 unsigned int i = 0;
kylongmu 0:768d2e151834 38
kylongmu 0:768d2e151834 39 select();
kylongmu 0:768d2e151834 40 spi.write(ReadAddr | READ_FLAG);
kylongmu 0:768d2e151834 41 for(i=0; i<Bytes; i++)
kylongmu 0:768d2e151834 42 ReadBuf[i] = spi.write(0x00);
kylongmu 0:768d2e151834 43 deselect();
kylongmu 0:768d2e151834 44 wait_us(50);
kylongmu 0:768d2e151834 45 }
kylongmu 0:768d2e151834 46
kylongmu 0:768d2e151834 47 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 48 INITIALIZATION
kylongmu 0:768d2e151834 49 usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
kylongmu 0:768d2e151834 50 low pass filter value; suitable values are:
kylongmu 0:768d2e151834 51 BITS_DLPF_CFG_256HZ_NOLPF2
kylongmu 0:768d2e151834 52 BITS_DLPF_CFG_188HZ
kylongmu 0:768d2e151834 53 BITS_DLPF_CFG_98HZ
kylongmu 0:768d2e151834 54 BITS_DLPF_CFG_42HZ
kylongmu 0:768d2e151834 55 BITS_DLPF_CFG_20HZ
kikoaac 12:a70c193d8195 56 BITS_DLPF_CFG_10HZ
kikoaac 12:a70c193d8195 57 BITS_DLPF_CFG_5HZ
kylongmu 0:768d2e151834 58 BITS_DLPF_CFG_2100HZ_NOLPF
kylongmu 0:768d2e151834 59 returns 1 if an error occurred
kylongmu 0:768d2e151834 60 -----------------------------------------------------------------------------------------------*/
kylongmu 5:f15d1d9d1561 61 #define MPU_InitRegNum 17
kylongmu 0:768d2e151834 62
kikoaac 12:a70c193d8195 63 bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter)
kikoaac 12:a70c193d8195 64 {
kylongmu 0:768d2e151834 65 uint8_t i = 0;
kylongmu 0:768d2e151834 66 uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
kylongmu 0:768d2e151834 67 {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
kylongmu 0:768d2e151834 68 {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
kylongmu 0:768d2e151834 69 {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
kylongmu 11:084e8ba240c1 70 {low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
kylongmu 0:768d2e151834 71 {0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
kylongmu 0:768d2e151834 72 {0x08, MPUREG_ACCEL_CONFIG}, // +-4G
kylongmu 9:a8733542d0fb 73 {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
kylongmu 0:768d2e151834 74 {0x30, MPUREG_INT_PIN_CFG}, //
kylongmu 4:79185409730f 75 //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
kylongmu 4:79185409730f 76 //{0x20, MPUREG_USER_CTRL}, // Enable AUX
kylongmu 5:f15d1d9d1561 77 {0x20, MPUREG_USER_CTRL}, // I2C Master mode
kylongmu 5:f15d1d9d1561 78 {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
kikoaac 12:a70c193d8195 79
kylongmu 5:f15d1d9d1561 80 {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
kylongmu 4:79185409730f 81 //{0x09, MPUREG_I2C_SLV4_CTRL},
kylongmu 5:f15d1d9d1561 82 //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
kylongmu 11:084e8ba240c1 83
kylongmu 5:f15d1d9d1561 84 {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
kylongmu 5:f15d1d9d1561 85 {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
kylongmu 5:f15d1d9d1561 86 {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
kylongmu 11:084e8ba240c1 87
kylongmu 5:f15d1d9d1561 88 {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
kylongmu 6:c82c48348f8c 89 {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
kylongmu 5:f15d1d9d1561 90 {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
kikoaac 12:a70c193d8195 91
kylongmu 0:768d2e151834 92 };
kylongmu 0:768d2e151834 93 spi.format(8,0);
kylongmu 0:768d2e151834 94 spi.frequency(1000000);
kylongmu 0:768d2e151834 95
kylongmu 0:768d2e151834 96 for(i=0; i<MPU_InitRegNum; i++) {
kylongmu 0:768d2e151834 97 WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
kylongmu 5:f15d1d9d1561 98 wait(0.001); //I2C must slow down the write speed, otherwise it won't work
kylongmu 0:768d2e151834 99 }
kylongmu 11:084e8ba240c1 100
kylongmu 11:084e8ba240c1 101 set_acc_scale(2);
kylongmu 11:084e8ba240c1 102 set_gyro_scale(250);
kikoaac 12:a70c193d8195 103
kylongmu 11:084e8ba240c1 104 //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
kylongmu 0:768d2e151834 105 return 0;
kylongmu 0:768d2e151834 106 }
kylongmu 0:768d2e151834 107 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 108 ACCELEROMETER SCALE
kylongmu 0:768d2e151834 109 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 110 accelerometers. Suitable ranges are:
kylongmu 0:768d2e151834 111 BITS_FS_2G
kylongmu 0:768d2e151834 112 BITS_FS_4G
kylongmu 0:768d2e151834 113 BITS_FS_8G
kylongmu 0:768d2e151834 114 BITS_FS_16G
kylongmu 0:768d2e151834 115 returns the range set (2,4,8 or 16)
kylongmu 0:768d2e151834 116 -----------------------------------------------------------------------------------------------*/
kikoaac 12:a70c193d8195 117 unsigned int mpu9250_spi::set_acc_scale(int scale)
kikoaac 12:a70c193d8195 118 {
kylongmu 0:768d2e151834 119 unsigned int temp_scale;
kylongmu 0:768d2e151834 120 WriteReg(MPUREG_ACCEL_CONFIG, scale);
kikoaac 12:a70c193d8195 121
kikoaac 12:a70c193d8195 122 switch (scale) {
kylongmu 0:768d2e151834 123 case BITS_FS_2G:
kylongmu 0:768d2e151834 124 acc_divider=16384;
kikoaac 12:a70c193d8195 125 break;
kylongmu 0:768d2e151834 126 case BITS_FS_4G:
kylongmu 0:768d2e151834 127 acc_divider=8192;
kikoaac 12:a70c193d8195 128 break;
kylongmu 0:768d2e151834 129 case BITS_FS_8G:
kylongmu 0:768d2e151834 130 acc_divider=4096;
kikoaac 12:a70c193d8195 131 break;
kylongmu 0:768d2e151834 132 case BITS_FS_16G:
kylongmu 0:768d2e151834 133 acc_divider=2048;
kikoaac 12:a70c193d8195 134 break;
kylongmu 0:768d2e151834 135 }
kylongmu 0:768d2e151834 136 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kikoaac 12:a70c193d8195 137
kikoaac 12:a70c193d8195 138 switch (temp_scale) {
kylongmu 0:768d2e151834 139 case BITS_FS_2G:
kylongmu 0:768d2e151834 140 temp_scale=2;
kikoaac 12:a70c193d8195 141 break;
kylongmu 0:768d2e151834 142 case BITS_FS_4G:
kylongmu 0:768d2e151834 143 temp_scale=4;
kikoaac 12:a70c193d8195 144 break;
kylongmu 0:768d2e151834 145 case BITS_FS_8G:
kylongmu 0:768d2e151834 146 temp_scale=8;
kikoaac 12:a70c193d8195 147 break;
kylongmu 0:768d2e151834 148 case BITS_FS_16G:
kylongmu 0:768d2e151834 149 temp_scale=16;
kikoaac 12:a70c193d8195 150 break;
kylongmu 0:768d2e151834 151 }
kylongmu 0:768d2e151834 152 return temp_scale;
kylongmu 0:768d2e151834 153 }
kylongmu 0:768d2e151834 154
kylongmu 0:768d2e151834 155
kylongmu 0:768d2e151834 156 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 157 GYROSCOPE SCALE
kylongmu 0:768d2e151834 158 usage: call this function at startup, after initialization, to set the right range for the
kylongmu 0:768d2e151834 159 gyroscopes. Suitable ranges are:
kylongmu 0:768d2e151834 160 BITS_FS_250DPS
kylongmu 0:768d2e151834 161 BITS_FS_500DPS
kylongmu 0:768d2e151834 162 BITS_FS_1000DPS
kylongmu 0:768d2e151834 163 BITS_FS_2000DPS
kylongmu 0:768d2e151834 164 returns the range set (250,500,1000 or 2000)
kylongmu 0:768d2e151834 165 -----------------------------------------------------------------------------------------------*/
kikoaac 12:a70c193d8195 166 unsigned int mpu9250_spi::set_gyro_scale(int scale)
kikoaac 12:a70c193d8195 167 {
kylongmu 0:768d2e151834 168 unsigned int temp_scale;
kylongmu 0:768d2e151834 169 WriteReg(MPUREG_GYRO_CONFIG, scale);
kikoaac 12:a70c193d8195 170 switch (scale) {
kylongmu 0:768d2e151834 171 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 172 gyro_divider=131;
kikoaac 12:a70c193d8195 173 break;
kylongmu 0:768d2e151834 174 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 175 gyro_divider=65.5;
kikoaac 12:a70c193d8195 176 break;
kylongmu 0:768d2e151834 177 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 178 gyro_divider=32.8;
kikoaac 12:a70c193d8195 179 break;
kylongmu 0:768d2e151834 180 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 181 gyro_divider=16.4;
kikoaac 12:a70c193d8195 182 break;
kylongmu 0:768d2e151834 183 }
kylongmu 0:768d2e151834 184 temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
kikoaac 12:a70c193d8195 185 switch (temp_scale) {
kylongmu 0:768d2e151834 186 case BITS_FS_250DPS:
kylongmu 0:768d2e151834 187 temp_scale=250;
kikoaac 12:a70c193d8195 188 break;
kylongmu 0:768d2e151834 189 case BITS_FS_500DPS:
kylongmu 0:768d2e151834 190 temp_scale=500;
kikoaac 12:a70c193d8195 191 break;
kylongmu 0:768d2e151834 192 case BITS_FS_1000DPS:
kylongmu 0:768d2e151834 193 temp_scale=1000;
kikoaac 12:a70c193d8195 194 break;
kylongmu 0:768d2e151834 195 case BITS_FS_2000DPS:
kylongmu 0:768d2e151834 196 temp_scale=2000;
kikoaac 12:a70c193d8195 197 break;
kylongmu 0:768d2e151834 198 }
kylongmu 0:768d2e151834 199 return temp_scale;
kylongmu 0:768d2e151834 200 }
kylongmu 0:768d2e151834 201
kylongmu 0:768d2e151834 202
kylongmu 0:768d2e151834 203 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 204 WHO AM I?
kylongmu 0:768d2e151834 205 usage: call this function to know if SPI is working correctly. It checks the I2C address of the
kylongmu 0:768d2e151834 206 mpu9250 which should be 104 when in SPI mode.
kylongmu 0:768d2e151834 207 returns the I2C address (104)
kylongmu 0:768d2e151834 208 -----------------------------------------------------------------------------------------------*/
kikoaac 12:a70c193d8195 209 unsigned int mpu9250_spi::whoami()
kikoaac 12:a70c193d8195 210 {
kylongmu 0:768d2e151834 211 unsigned int response;
kylongmu 0:768d2e151834 212 response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 213 return response;
kylongmu 0:768d2e151834 214 }
kylongmu 0:768d2e151834 215
kylongmu 0:768d2e151834 216
kylongmu 0:768d2e151834 217 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 218 READ ACCELEROMETER
kylongmu 0:768d2e151834 219 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 220 0 -> X axis
kylongmu 0:768d2e151834 221 1 -> Y axis
kylongmu 0:768d2e151834 222 2 -> Z axis
kylongmu 0:768d2e151834 223 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 224 void mpu9250_spi::read_acc()
kylongmu 2:f274ea3bced9 225 {
kylongmu 4:79185409730f 226 uint8_t response[6];
kylongmu 0:768d2e151834 227 int16_t bit_data;
kylongmu 0:768d2e151834 228 float data;
kylongmu 4:79185409730f 229 int i;
kylongmu 4:79185409730f 230 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
kylongmu 4:79185409730f 231 for(i=0; i<3; i++) {
kylongmu 4:79185409730f 232 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kylongmu 4:79185409730f 233 data=(float)bit_data;
kylongmu 4:79185409730f 234 accelerometer_data[i]=data/acc_divider;
kikoaac 12:a70c193d8195 235 accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i];
kikoaac 12:a70c193d8195 236 accelerometer_data_prev[i]=accelerometer_data[i];
kylongmu 4:79185409730f 237 }
kikoaac 12:a70c193d8195 238
kylongmu 0:768d2e151834 239 }
kylongmu 0:768d2e151834 240
kylongmu 0:768d2e151834 241 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 242 READ GYROSCOPE
kylongmu 0:768d2e151834 243 usage: call this function to read gyroscope data. Axis represents selected axis:
kylongmu 0:768d2e151834 244 0 -> X axis
kylongmu 0:768d2e151834 245 1 -> Y axis
kylongmu 0:768d2e151834 246 2 -> Z axis
kylongmu 0:768d2e151834 247 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 248 void mpu9250_spi::read_rot()
kylongmu 2:f274ea3bced9 249 {
kylongmu 4:79185409730f 250 uint8_t response[6];
kylongmu 0:768d2e151834 251 int16_t bit_data;
kylongmu 0:768d2e151834 252 float data;
kylongmu 4:79185409730f 253 int i;
kylongmu 4:79185409730f 254 ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
kylongmu 4:79185409730f 255 for(i=0; i<3; i++) {
kylongmu 4:79185409730f 256 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kylongmu 4:79185409730f 257 data=(float)bit_data;
kylongmu 4:79185409730f 258 gyroscope_data[i]=data/gyro_divider;
kylongmu 4:79185409730f 259 }
kylongmu 2:f274ea3bced9 260
kylongmu 0:768d2e151834 261 }
kylongmu 0:768d2e151834 262
kylongmu 0:768d2e151834 263 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 264 READ TEMPERATURE
kikoaac 12:a70c193d8195 265 usage: call this function to read temperature data.
kylongmu 0:768d2e151834 266 returns the value in °C
kylongmu 0:768d2e151834 267 -----------------------------------------------------------------------------------------------*/
kikoaac 12:a70c193d8195 268 void mpu9250_spi::read_temp()
kikoaac 12:a70c193d8195 269 {
kylongmu 2:f274ea3bced9 270 uint8_t response[2];
kylongmu 0:768d2e151834 271 int16_t bit_data;
kylongmu 0:768d2e151834 272 float data;
kylongmu 2:f274ea3bced9 273 ReadRegs(MPUREG_TEMP_OUT_H,response,2);
kylongmu 2:f274ea3bced9 274
kylongmu 2:f274ea3bced9 275 bit_data=((int16_t)response[0]<<8)|response[1];
kylongmu 0:768d2e151834 276 data=(float)bit_data;
kylongmu 3:f4fa24cc247d 277 Temperature=(data/340)+36.53;
kylongmu 0:768d2e151834 278 deselect();
kylongmu 0:768d2e151834 279 }
kylongmu 0:768d2e151834 280
kylongmu 0:768d2e151834 281 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 282 READ ACCELEROMETER CALIBRATION
kylongmu 0:768d2e151834 283 usage: call this function to read accelerometer data. Axis represents selected axis:
kylongmu 0:768d2e151834 284 0 -> X axis
kylongmu 0:768d2e151834 285 1 -> Y axis
kylongmu 0:768d2e151834 286 2 -> Z axis
kylongmu 0:768d2e151834 287 returns Factory Trim value
kylongmu 0:768d2e151834 288 -----------------------------------------------------------------------------------------------*/
kylongmu 2:f274ea3bced9 289 void mpu9250_spi::calib_acc()
kylongmu 2:f274ea3bced9 290 {
kylongmu 2:f274ea3bced9 291 uint8_t response[4];
kylongmu 0:768d2e151834 292 int temp_scale;
kylongmu 0:768d2e151834 293 //READ CURRENT ACC SCALE
kylongmu 0:768d2e151834 294 temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
kylongmu 0:768d2e151834 295 set_acc_scale(BITS_FS_8G);
kylongmu 2:f274ea3bced9 296 //ENABLE SELF TEST need modify
kylongmu 2:f274ea3bced9 297 //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
kylongmu 0:768d2e151834 298
kylongmu 2:f274ea3bced9 299 ReadRegs(MPUREG_SELF_TEST_X,response,4);
kylongmu 2:f274ea3bced9 300 calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
kylongmu 2:f274ea3bced9 301 calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
kylongmu 2:f274ea3bced9 302 calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
kylongmu 2:f274ea3bced9 303
kylongmu 0:768d2e151834 304 set_acc_scale(temp_scale);
kylongmu 2:f274ea3bced9 305 }
kikoaac 12:a70c193d8195 306 uint8_t mpu9250_spi::AK8963_whoami()
kikoaac 12:a70c193d8195 307 {
kylongmu 11:084e8ba240c1 308 uint8_t response;
kylongmu 5:f15d1d9d1561 309 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 4:79185409730f 310 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
kylongmu 11:084e8ba240c1 311 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
kylongmu 11:084e8ba240c1 312
kylongmu 11:084e8ba240c1 313 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
kylongmu 11:084e8ba240c1 314 wait(0.001);
kikoaac 12:a70c193d8195 315 response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 316 //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
kikoaac 12:a70c193d8195 317 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 318
kylongmu 11:084e8ba240c1 319 return response;
kylongmu 11:084e8ba240c1 320 }
kikoaac 12:a70c193d8195 321 double* mpu9250_spi::AK8936_read_Orientation(double *getdata)
kikoaac 12:a70c193d8195 322 {
kikoaac 12:a70c193d8195 323
kikoaac 12:a70c193d8195 324 double gx = accelerometer_data[0];
kikoaac 12:a70c193d8195 325 double gy = accelerometer_data[1];
kikoaac 12:a70c193d8195 326 double gz = accelerometer_data[2];
kikoaac 12:a70c193d8195 327 double mx = Magnetometer[0];//-centorx;
kikoaac 12:a70c193d8195 328 double my = Magnetometer[1];//-centory;
kikoaac 12:a70c193d8195 329 double mz = Magnetometer[2];//-centorz;
kikoaac 12:a70c193d8195 330 double g0x=0.01,g0y=0.001,g0z=0.99;
kikoaac 12:a70c193d8195 331 double Gproduct = gx*g0x+gy*g0y+gz*g0z;
kikoaac 12:a70c193d8195 332 double Glengh = sqrt(gx*gx+gy*gy+gz*gz);
kikoaac 12:a70c193d8195 333 double G0lengh =sqrt(g0x*g0x+g0y*g0y+g0z*g0z);
kikoaac 12:a70c193d8195 334 double nx = (gy * g0z - gz * g0y);
kikoaac 12:a70c193d8195 335 double ny = (gz * g0x - gx * g0z);
kikoaac 12:a70c193d8195 336 double nz = (gx * g0y - gy * g0x);
kikoaac 12:a70c193d8195 337 double GPlengh = sqrt(nx*nx+ny*ny+nz*nz);
kikoaac 12:a70c193d8195 338 nx/=GPlengh;
kikoaac 12:a70c193d8195 339 ny/=GPlengh;
kikoaac 12:a70c193d8195 340 nz/=GPlengh;
kikoaac 12:a70c193d8195 341 double accang = acos((Gproduct)/(fabs(Glengh)*fabs(G0lengh)));/*
kikoaac 12:a70c193d8195 342 double hx=(nx*nx*(1-cos(accang))+cos(accang))*mx + (nx*ny*(1-cos(accang))+nz*sin(accang))*my + (nz*nx*(1-cos(accang))+ny*sin(accang))*mz;
kikoaac 12:a70c193d8195 343 double hy=(nx*ny*(1-cos(accang))+nz*sin(accang))*mx + (ny*ny*(1-cos(accang))+cos(accang))*my + (ny*nz*(1-cos(accang))+nx*sin(accang))*mz;*/
kikoaac 12:a70c193d8195 344 double Mrot[3][3] = {
kikoaac 12:a70c193d8195 345 {nx*nx*(1-cos(accang))+cos(accang) , nx*ny*(1-cos(accang))+nz*sin(accang) ,nz*nx*(1-cos(accang))+ny*sin(accang)},
kikoaac 12:a70c193d8195 346 {nx*ny*(1-cos(accang))+nz*sin(accang) , ny*ny*(1-cos(accang))+cos(accang) ,ny*nz*(1-cos(accang))+nx*sin(accang)},
kikoaac 12:a70c193d8195 347 {nz*nx*(1-cos(accang))+ny*sin(accang) , ny*nz*(1-cos(accang))+nx*sin(accang) ,nz*nz*(1-cos(accang))+cos(accang)}
kikoaac 12:a70c193d8195 348 };
kikoaac 12:a70c193d8195 349 double MrotD[3][3];
kikoaac 12:a70c193d8195 350 double temp[3][3];
kikoaac 12:a70c193d8195 351 for(int i=0; i<3; i++)
kikoaac 12:a70c193d8195 352 for(int j=0; j<3; j++) {
kikoaac 12:a70c193d8195 353 temp[j][i]=Mrot[i][j];
kikoaac 12:a70c193d8195 354 MrotD[j][i]=temp[j][i];
kikoaac 12:a70c193d8195 355 }
kikoaac 12:a70c193d8195 356 double hx = MrotD[0][0]*mx+MrotD[0][1]*my+MrotD[0][2]*mz;
kikoaac 12:a70c193d8195 357 double hy = MrotD[1][0]*mx+MrotD[1][1]*my+MrotD[1][2]*mz;
kikoaac 12:a70c193d8195 358 double hz = MrotD[2][0]*mx+MrotD[2][1]*my+MrotD[2][2]*mz;
kikoaac 12:a70c193d8195 359 double heading = atan2(hy,hx)*180/3.14;
kikoaac 12:a70c193d8195 360 if (heading > 0)
kikoaac 12:a70c193d8195 361 heading = 360 - heading;
kikoaac 12:a70c193d8195 362 else
kikoaac 12:a70c193d8195 363 heading = -heading;
kikoaac 12:a70c193d8195 364 float pitch = atan(-hx/sqrt(hy*hy+hz*hz))*180/3.14; //invert gx because +pitch is up. range is +/-90 degrees
kikoaac 12:a70c193d8195 365 float roll = atan(hy/sqrt(hx*hx+hz*hz))*180/3.14; // right side down is +roll
kikoaac 12:a70c193d8195 366 if (gz > 0)
kikoaac 12:a70c193d8195 367 {
kikoaac 12:a70c193d8195 368 if ( roll > 0)
kikoaac 12:a70c193d8195 369 roll = 180 - roll;
kikoaac 12:a70c193d8195 370 else
kikoaac 12:a70c193d8195 371 roll = -180 - roll;
kikoaac 12:a70c193d8195 372 }
kikoaac 12:a70c193d8195 373 getdata[0]=heading;
kikoaac 12:a70c193d8195 374 getdata[1]=pitch;
kikoaac 12:a70c193d8195 375 getdata[2]=roll;
kikoaac 12:a70c193d8195 376 return getdata;
kikoaac 12:a70c193d8195 377 }
kikoaac 12:a70c193d8195 378 void mpu9250_spi::AK8963_setoffset(int x,double offset)
kikoaac 12:a70c193d8195 379 {
kikoaac 12:a70c193d8195 380 if((x<0)||(x>3))return;
kikoaac 12:a70c193d8195 381 Magnetometer_offset[x]+=offset;
kikoaac 12:a70c193d8195 382 }
kikoaac 12:a70c193d8195 383 void mpu9250_spi::AK8963_calib_Magnetometer()
kikoaac 12:a70c193d8195 384 {
kylongmu 11:084e8ba240c1 385 uint8_t response[3];
kylongmu 11:084e8ba240c1 386 float data;
kylongmu 11:084e8ba240c1 387 int i;
kylongmu 11:084e8ba240c1 388
kylongmu 11:084e8ba240c1 389 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 11:084e8ba240c1 390 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
kylongmu 11:084e8ba240c1 391 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
kylongmu 5:f15d1d9d1561 392
kylongmu 5:f15d1d9d1561 393 //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
kylongmu 6:c82c48348f8c 394 wait(0.001);
kikoaac 12:a70c193d8195 395 //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 396 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
kikoaac 12:a70c193d8195 397
kikoaac 12:a70c193d8195 398 //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
kylongmu 11:084e8ba240c1 399 for(i=0; i<3; i++) {
kylongmu 11:084e8ba240c1 400 data=response[i];
kylongmu 11:084e8ba240c1 401 Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
kylongmu 11:084e8ba240c1 402 }
kylongmu 4:79185409730f 403 }
kikoaac 12:a70c193d8195 404 void mpu9250_spi::AK8963_read_Magnetometer()
kikoaac 12:a70c193d8195 405 {
kylongmu 5:f15d1d9d1561 406 uint8_t response[7];
kylongmu 4:79185409730f 407 int16_t bit_data;
kylongmu 4:79185409730f 408 float data;
kylongmu 5:f15d1d9d1561 409 int i;
kylongmu 5:f15d1d9d1561 410
kylongmu 5:f15d1d9d1561 411 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 4:79185409730f 412 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
kylongmu 5:f15d1d9d1561 413 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
kylongmu 4:79185409730f 414
kylongmu 6:c82c48348f8c 415 wait(0.001);
kylongmu 5:f15d1d9d1561 416 ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
kylongmu 5:f15d1d9d1561 417 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
kylongmu 5:f15d1d9d1561 418 for(i=0; i<3; i++) {
kylongmu 10:d27b3298e9e0 419 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
kylongmu 5:f15d1d9d1561 420 data=(float)bit_data;
kikoaac 12:a70c193d8195 421 Magnetometer[i]=data*Magnetometer_ASA[i]-Magnetometer_offset[i];
kylongmu 5:f15d1d9d1561 422 }
kylongmu 4:79185409730f 423 }
kikoaac 12:a70c193d8195 424 void mpu9250_spi::read_all()
kikoaac 12:a70c193d8195 425 {
kylongmu 8:492028cb604e 426 uint8_t response[21];
kylongmu 8:492028cb604e 427 int16_t bit_data;
kylongmu 8:492028cb604e 428 float data;
kylongmu 8:492028cb604e 429 int i;
kylongmu 8:492028cb604e 430
kylongmu 8:492028cb604e 431 //Send I2C command at first
kylongmu 8:492028cb604e 432 WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
kylongmu 8:492028cb604e 433 WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
kylongmu 8:492028cb604e 434 WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
kylongmu 8:492028cb604e 435 //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
kylongmu 8:492028cb604e 436
kylongmu 8:492028cb604e 437 //wait(0.001);
kylongmu 8:492028cb604e 438 ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
kylongmu 8:492028cb604e 439 //Get accelerometer value
kylongmu 8:492028cb604e 440 for(i=0; i<3; i++) {
kylongmu 8:492028cb604e 441 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kylongmu 8:492028cb604e 442 data=(float)bit_data;
kylongmu 8:492028cb604e 443 accelerometer_data[i]=data/acc_divider;
kikoaac 12:a70c193d8195 444 //accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i];
kikoaac 12:a70c193d8195 445 //accelerometer_data_prev[i]=accelerometer_data[i];
kylongmu 8:492028cb604e 446 }
kylongmu 8:492028cb604e 447 //Get temperature
kylongmu 8:492028cb604e 448 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kylongmu 8:492028cb604e 449 data=(float)bit_data;
kylongmu 9:a8733542d0fb 450 Temperature=((data-21)/333.87)+21;
kylongmu 8:492028cb604e 451 //Get gyroscop value
kylongmu 8:492028cb604e 452 for(i=4; i<7; i++) {
kylongmu 8:492028cb604e 453 bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kylongmu 8:492028cb604e 454 data=(float)bit_data;
kylongmu 8:492028cb604e 455 gyroscope_data[i-4]=data/gyro_divider;
kylongmu 8:492028cb604e 456 }
kylongmu 8:492028cb604e 457 //Get Magnetometer value
kylongmu 8:492028cb604e 458 for(i=7; i<10; i++) {
kylongmu 10:d27b3298e9e0 459 bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
kylongmu 8:492028cb604e 460 data=(float)bit_data;
kikoaac 12:a70c193d8195 461 Magnetometer[i-7]=data*Magnetometer_ASA[i-7]-Magnetometer_offset[i-7];
kikoaac 12:a70c193d8195 462 }
kikoaac 12:a70c193d8195 463 }
kikoaac 12:a70c193d8195 464
kikoaac 12:a70c193d8195 465
kikoaac 12:a70c193d8195 466
kikoaac 12:a70c193d8195 467 void mpu9250_spi::SpeedSet()
kikoaac 12:a70c193d8195 468 {
kikoaac 12:a70c193d8195 469 get_speedrate();
kikoaac 12:a70c193d8195 470 read_acc();
kikoaac 12:a70c193d8195 471 /*for(int i=0;i<3;i++)
kikoaac 12:a70c193d8195 472 ratespeed[i]=acc[i];*/
kikoaac 12:a70c193d8195 473 double S[3];
kikoaac 12:a70c193d8195 474 speedT.stop();
kikoaac 12:a70c193d8195 475 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 476
kikoaac 12:a70c193d8195 477 S[i] =((double)(ratespeed[i]+prev_ratespeed[i])*speedT.read()/2);
kikoaac 12:a70c193d8195 478 speed[i]+=(double)S[i];
kikoaac 12:a70c193d8195 479 speed[i]=floor(speed[i]*100.0)/100.0;
kikoaac 12:a70c193d8195 480 Synthesis_speed[i]+=(double)S[i];
kikoaac 12:a70c193d8195 481 Synthesis_speed[i]=0.9*(double)(Synthesis_speed[i]+(double)ratespeed[i]/100.5)+0.1*ratespeed[i];
kikoaac 12:a70c193d8195 482 kalman_speed[i]=kalmaspeed[i].getAngle((double)kalman_speed[i], (double)ratespeed[i], (double)angleT.read()*1000);
kikoaac 12:a70c193d8195 483 comp_speed[i]=kalman_speed[i]*0.01+Synthesis_speed[i]*0.01+comp_speed[i]*0.98;
kikoaac 12:a70c193d8195 484 prev_ratespeed[i]=ratespeed[i];
kikoaac 12:a70c193d8195 485 }
kikoaac 12:a70c193d8195 486 speedT.reset();
kikoaac 12:a70c193d8195 487 speedT.start();
kikoaac 12:a70c193d8195 488 }
kikoaac 12:a70c193d8195 489 void mpu9250_spi::MeterSet()
kikoaac 12:a70c193d8195 490 {
kikoaac 12:a70c193d8195 491 double S[3];
kikoaac 12:a70c193d8195 492 for(int i = 0 ; i<3 ; i++)
kikoaac 12:a70c193d8195 493 ratemeter[i]=comp_speed[i]*1000;
kikoaac 12:a70c193d8195 494 meterT.stop();
kikoaac 12:a70c193d8195 495 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 496
kikoaac 12:a70c193d8195 497 S[i] =((double)(ratemeter[i]+prev_ratemeter[i])*meterT.read()/2);
kikoaac 12:a70c193d8195 498 meter[i]+=(double)S[i];
kikoaac 12:a70c193d8195 499 Synthesis_meter[i]+=(double)S[i];
kikoaac 12:a70c193d8195 500 Synthesis_meter[i]=0.9*(double)(Synthesis_meter[i]+(double)ratemeter[i]/100.5)+0.1*meter[i];
kikoaac 12:a70c193d8195 501 kalman_meter[i]=kalmameter[i].getAngle((double)kalman_meter[i], (double)ratemeter[i], (double)angleT.read()*1000);
kikoaac 12:a70c193d8195 502 comp_meter[i]=kalman_meter[i]*0.01+Synthesis_meter[i]*0.01+comp_meter[i]*0.98;
kikoaac 12:a70c193d8195 503 prev_ratemeter[i]=ratemeter[i];
kikoaac 12:a70c193d8195 504 }
kikoaac 12:a70c193d8195 505 meterT.reset();
kikoaac 12:a70c193d8195 506 meterT.start();
kikoaac 12:a70c193d8195 507 }
kikoaac 12:a70c193d8195 508
kikoaac 12:a70c193d8195 509
kikoaac 12:a70c193d8195 510
kikoaac 12:a70c193d8195 511 void mpu9250_spi::MPU_setup()
kikoaac 12:a70c193d8195 512 {
kikoaac 12:a70c193d8195 513 z_offset=2;
kikoaac 12:a70c193d8195 514 x_offset=0;
kikoaac 12:a70c193d8195 515 y_offset=0;
kikoaac 12:a70c193d8195 516 uint8_t buffer[6];
kikoaac 12:a70c193d8195 517
kikoaac 12:a70c193d8195 518 for(int i=0; i<sampleNum; i++) {
kikoaac 12:a70c193d8195 519 ReadRegs(MPUREG_ACCEL_XOUT_H,buffer,6);
kikoaac 12:a70c193d8195 520 int Xacc = (int)buffer[1] << 8 | (int)buffer[0];
kikoaac 12:a70c193d8195 521 int Yacc = (int)buffer[3] << 8 | (int)buffer[2];
kikoaac 12:a70c193d8195 522 int Zacc = (int)buffer[5] << 8 | (int)buffer[4]-255;
kikoaac 12:a70c193d8195 523 if((int)Xacc-x_offset>xnoise)
kikoaac 12:a70c193d8195 524 xnoise=(int)Xacc-x_offset;
kikoaac 12:a70c193d8195 525 else if((int)Xacc-x_offset<-xnoise)
kikoaac 12:a70c193d8195 526 xnoise=-(int)Xacc-x_offset;
kikoaac 12:a70c193d8195 527
kikoaac 12:a70c193d8195 528 if((int)Yacc-y_offset>ynoise)
kikoaac 12:a70c193d8195 529 ynoise=(int)Yacc-y_offset;
kikoaac 12:a70c193d8195 530 else if((int)Yacc-y_offset<-ynoise)
kikoaac 12:a70c193d8195 531 ynoise=-(int)Yacc-y_offset;
kikoaac 12:a70c193d8195 532
kikoaac 12:a70c193d8195 533 if((int)Zacc-z_offset>znoise)
kikoaac 12:a70c193d8195 534 znoise=(int)Zacc-z_offset;
kikoaac 12:a70c193d8195 535 else if((int)Zacc-z_offset<-znoise)
kikoaac 12:a70c193d8195 536 znoise=-(int)Zacc-z_offset;
kikoaac 12:a70c193d8195 537 }
kikoaac 12:a70c193d8195 538
kikoaac 12:a70c193d8195 539
kikoaac 12:a70c193d8195 540 }
kikoaac 12:a70c193d8195 541 void mpu9250_spi::MPU_setnum(int Num,float time,double rate)
kikoaac 12:a70c193d8195 542 {
kikoaac 12:a70c193d8195 543 sampleNum=Num;
kikoaac 12:a70c193d8195 544 sampleTime=time;
kikoaac 12:a70c193d8195 545 Rate=rate;
kikoaac 12:a70c193d8195 546 }
kikoaac 12:a70c193d8195 547
kikoaac 12:a70c193d8195 548
kikoaac 12:a70c193d8195 549 void mpu9250_spi::get_angle_acc()
kikoaac 12:a70c193d8195 550 {
kikoaac 12:a70c193d8195 551 //short data[3];
kikoaac 12:a70c193d8195 552 //get_axis_acc(data);
kikoaac 12:a70c193d8195 553 float R = sqrt(pow((float)accelerometer_data[0],2) + pow((float)accelerometer_data[1],2) + pow((float)accelerometer_data[2],2));
kikoaac 12:a70c193d8195 554 angle_acc[0]=atan2(accelerometer_data[0],accelerometer_data[2])*(180 / 3.1415);
kikoaac 12:a70c193d8195 555 angle_acc[1]=atan2(accelerometer_data[1],accelerometer_data[2])*(180 / 3.1415);
kikoaac 12:a70c193d8195 556 //angle_acc[2]=
kikoaac 12:a70c193d8195 557 /*angle_acc[1] = -((180 / 3.1415) * acos((float)accelerometer_data[1] / R)-90); // roll - angle of magnetic field vector in x direction
kikoaac 12:a70c193d8195 558 angle_acc[0] = (180 / 3.1415) * acos((float)accelerometer_data[0] / R)-90; // pitch - angle of magnetic field vector in y direction
kikoaac 12:a70c193d8195 559 angle_acc[2] = (180 / 3.1415) * acos((float)accelerometer_data[2] / R); //*/
kikoaac 12:a70c193d8195 560 /*angle_acc[0] = atan2(accelerometer_data[0],sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
kikoaac 12:a70c193d8195 561 angle_acc[1] = atan2(accelerometer_data[1],sqrt(pow((float)accelerometer_data[0],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
kikoaac 12:a70c193d8195 562 angle_acc[2] = atan2(sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)),accelerometer_data[2])*180/3.1415;*/
kikoaac 12:a70c193d8195 563
kikoaac 12:a70c193d8195 564 }
kikoaac 12:a70c193d8195 565 void mpu9250_spi::get_rate()
kikoaac 12:a70c193d8195 566 {
kikoaac 12:a70c193d8195 567 uint8_t response[6];
kikoaac 12:a70c193d8195 568 short offset_t[3]= {0,0,0};
kikoaac 12:a70c193d8195 569 ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
kikoaac 12:a70c193d8195 570 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 571 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kikoaac 12:a70c193d8195 572 rate[i]=(double)(bit_data*0.01)-offset_t[i];
kikoaac 12:a70c193d8195 573 //printf("%d",rate[i]);
kikoaac 12:a70c193d8195 574 }
kikoaac 12:a70c193d8195 575 }
kikoaac 12:a70c193d8195 576
kikoaac 12:a70c193d8195 577 void mpu9250_spi::get_speedrate()
kikoaac 12:a70c193d8195 578 {
kikoaac 12:a70c193d8195 579 uint8_t response[6];
kikoaac 12:a70c193d8195 580 short offset_t[3]= {0,0,0};
kikoaac 12:a70c193d8195 581 ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
kikoaac 12:a70c193d8195 582 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 583 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kikoaac 12:a70c193d8195 584 ratespeed[i]=(short)(bit_data*0.01)-offset_t[i];
kylongmu 8:492028cb604e 585 }
kylongmu 8:492028cb604e 586 }
kikoaac 12:a70c193d8195 587 void mpu9250_spi::get_angle(double *x,double *y,double *z)
kikoaac 12:a70c193d8195 588 {
kikoaac 12:a70c193d8195 589 *x=(floor(angle[0]*100.0)/100.0);
kikoaac 12:a70c193d8195 590 *y=(floor(angle[1]*100.0)/100.0);
kikoaac 12:a70c193d8195 591 *z=(floor(angle[2]*100.0)/100.0);
kikoaac 12:a70c193d8195 592 }
kikoaac 12:a70c193d8195 593 void mpu9250_spi::set_angle()
kikoaac 12:a70c193d8195 594 {
kikoaac 12:a70c193d8195 595 get_rate();
kikoaac 12:a70c193d8195 596 read_acc();
kikoaac 12:a70c193d8195 597 get_angle_acc();
kikoaac 12:a70c193d8195 598 double S[3];
kikoaac 12:a70c193d8195 599 angleT.stop();
kikoaac 12:a70c193d8195 600 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 601 //rate[i]=gyroscope_data[i]*0.01;
kikoaac 12:a70c193d8195 602 double angA=angle_acc[i];
kikoaac 12:a70c193d8195 603 S[i] =((double)(rate[i]+prev_rate[i])*angleT.read()/2);
kikoaac 12:a70c193d8195 604 // S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
kikoaac 12:a70c193d8195 605 angle[i]+=(floor(S[i]*10000.0)/10000.0);//-offset_angle[i];
kikoaac 12:a70c193d8195 606 //angle[i]+=(double)S[i];
kikoaac 12:a70c193d8195 607 Synthesis_angle[i]+=(double)angle[i];
kikoaac 12:a70c193d8195 608 Synthesis_angle[i]=0.92*(double)(Synthesis_angle[i]+(double)rate[i])+0.08*angA;
kikoaac 12:a70c193d8195 609 kalman_angle[i]=kalma[i].getAngle((double)angA, (double)rate[i], (double)angleT.read_ms());
kikoaac 12:a70c193d8195 610 comp_angle[i]=kalman_angle[i];//Synthesis_angle[i]*0.5+kalman_angle[i]*0+angle[i]*0.3+angle_acc[i]*0.2;
kikoaac 12:a70c193d8195 611 comp_angle[i]=kalman_angle[i]*0.4+Synthesis_angle[i]*0.4+comp_angle[i]*0.2;
kikoaac 12:a70c193d8195 612 prev_rate[i]=rate[i];
kikoaac 12:a70c193d8195 613 }
kikoaac 12:a70c193d8195 614 angleT.reset();
kikoaac 12:a70c193d8195 615 angleT.start();
kikoaac 12:a70c193d8195 616 }
kikoaac 12:a70c193d8195 617 void mpu9250_spi::set_angleoffset()
kikoaac 12:a70c193d8195 618 {
kikoaac 12:a70c193d8195 619 double axis[3],offseta[3];
kikoaac 12:a70c193d8195 620 offseta[0]=offseta[1]=offseta[2]=0;
kikoaac 12:a70c193d8195 621 offset_angle[0]=0;
kikoaac 12:a70c193d8195 622 offset_angle[1]=0;
kikoaac 12:a70c193d8195 623 offset_angle[2]=0;
kikoaac 12:a70c193d8195 624 for(int i=0; i<sampleNum; i++) {
kikoaac 12:a70c193d8195 625 offseta[0]+=rate[0];
kikoaac 12:a70c193d8195 626 offseta[1]+=rate[1];
kikoaac 12:a70c193d8195 627 offseta[2]+=rate[2];
kikoaac 12:a70c193d8195 628 }
kikoaac 12:a70c193d8195 629 offset_angle[0]=offseta[0]/sampleNum;
kikoaac 12:a70c193d8195 630 offset_angle[1]=offseta[1]/sampleNum;
kikoaac 12:a70c193d8195 631 offset_angle[2]=offseta[2]/sampleNum;
kikoaac 12:a70c193d8195 632 offset_angle[0]=(floor(offset_angle[0]*100.0)/100.0);
kikoaac 12:a70c193d8195 633 offset_angle[1]=(floor(offset_angle[1]*100.0)/100.0);
kikoaac 12:a70c193d8195 634 offset_angle[2]=(floor(offset_angle[2]*100.0)/100.0);
kikoaac 12:a70c193d8195 635 angle[0]=0;
kikoaac 12:a70c193d8195 636 angle[1]=0;
kikoaac 12:a70c193d8195 637 angle[2]=0;
kikoaac 12:a70c193d8195 638 }
kikoaac 12:a70c193d8195 639 void mpu9250_spi::set_noise()
kikoaac 12:a70c193d8195 640 {
kikoaac 12:a70c193d8195 641 short gyro[3];
kikoaac 12:a70c193d8195 642 noise[0]=noise[1]=noise[2]=0;
kikoaac 12:a70c193d8195 643 uint8_t response[6];
kikoaac 12:a70c193d8195 644 for(int i=0; i<sampleNum; i++) {
kikoaac 12:a70c193d8195 645
kikoaac 12:a70c193d8195 646 for(int t=0; t<3; t++) {
kikoaac 12:a70c193d8195 647 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 648 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kikoaac 12:a70c193d8195 649 gyro[i]=(short)bit_data;
kikoaac 12:a70c193d8195 650 }
kikoaac 12:a70c193d8195 651 if((int)gyro[t]>noise[t])
kikoaac 12:a70c193d8195 652 noise[t]=(int)gyro[t];
kikoaac 12:a70c193d8195 653 else if((int)gyro[t]<-noise[t])
kikoaac 12:a70c193d8195 654 noise[t]=-(int)gyro[t];
kikoaac 12:a70c193d8195 655 }
kikoaac 12:a70c193d8195 656 }
kikoaac 12:a70c193d8195 657 noise[0]*=sampleTime;
kikoaac 12:a70c193d8195 658 noise[1]*=sampleTime;
kikoaac 12:a70c193d8195 659 noise[2]*=sampleTime;
kikoaac 12:a70c193d8195 660 }
kikoaac 12:a70c193d8195 661 void mpu9250_spi::set_offset()
kikoaac 12:a70c193d8195 662 {
kikoaac 12:a70c193d8195 663 short axis[3],offseta[3];
kikoaac 12:a70c193d8195 664 uint8_t response[6];
kikoaac 12:a70c193d8195 665 offseta[0]=0;
kikoaac 12:a70c193d8195 666 offseta[1]=0;
kikoaac 12:a70c193d8195 667 offseta[2]=0;
kikoaac 12:a70c193d8195 668 for(int i=0; i<sampleNum; i++) {
kikoaac 12:a70c193d8195 669 for(int i=0; i<3; i++) {
kikoaac 12:a70c193d8195 670 short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
kikoaac 12:a70c193d8195 671 axis[i]=(short)bit_data;
kikoaac 12:a70c193d8195 672 }
kikoaac 12:a70c193d8195 673 offseta[0]+=axis[0];
kikoaac 12:a70c193d8195 674 offseta[1]+=axis[1];
kikoaac 12:a70c193d8195 675 offseta[2]+=axis[2];
kikoaac 12:a70c193d8195 676 }
kikoaac 12:a70c193d8195 677 offset[0]=offseta[0]/sampleNum;
kikoaac 12:a70c193d8195 678 offset[1]=offseta[1]/sampleNum;
kikoaac 12:a70c193d8195 679 offset[2]=offseta[2]/sampleNum;
kikoaac 12:a70c193d8195 680 }
kikoaac 12:a70c193d8195 681 mpu9250_spi::kalman::kalman(void)
kikoaac 12:a70c193d8195 682 {
kikoaac 12:a70c193d8195 683 P[0][0] = 0;
kikoaac 12:a70c193d8195 684 P[0][1] = 0;
kikoaac 12:a70c193d8195 685 P[1][0] = 0;
kikoaac 12:a70c193d8195 686 P[1][1] = 0;
kikoaac 12:a70c193d8195 687
kikoaac 12:a70c193d8195 688 angle = 0;
kikoaac 12:a70c193d8195 689 bias = 0;
kikoaac 12:a70c193d8195 690
kikoaac 12:a70c193d8195 691 Q_angle = 0.001;
kikoaac 12:a70c193d8195 692 Q_gyroBias = 0.003;
kikoaac 12:a70c193d8195 693 R_angle = 0.03;
kikoaac 12:a70c193d8195 694 }
kikoaac 12:a70c193d8195 695
kikoaac 12:a70c193d8195 696 double mpu9250_spi::kalman::getAngle(double newAngle, double newRate, double dt)
kikoaac 12:a70c193d8195 697 {
kikoaac 12:a70c193d8195 698 //predict the angle according to the gyroscope
kikoaac 12:a70c193d8195 699 rate = newRate - bias;
kikoaac 12:a70c193d8195 700 angle = dt * rate;
kikoaac 12:a70c193d8195 701 //update the error covariance
kikoaac 12:a70c193d8195 702 P[0][0] += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
kikoaac 12:a70c193d8195 703 P[0][1] -= dt * P[1][1];
kikoaac 12:a70c193d8195 704 P[1][0] -= dt * P[1][1];
kikoaac 12:a70c193d8195 705 P[1][1] += dt * Q_gyroBias;
kikoaac 12:a70c193d8195 706 //difference between the predicted angle and the accelerometer angle
kikoaac 12:a70c193d8195 707 y = newAngle - angle;
kikoaac 12:a70c193d8195 708 //estimation error
kikoaac 12:a70c193d8195 709 S = P[0][0] + R_angle;
kikoaac 12:a70c193d8195 710 //determine the kalman gain according to the error covariance and the distrust
kikoaac 12:a70c193d8195 711 K[0] = P[0][0]/S;
kikoaac 12:a70c193d8195 712 K[1] = P[1][0]/S;
kikoaac 12:a70c193d8195 713 //adjust the angle according to the kalman gain and the difference with the measurement
kikoaac 12:a70c193d8195 714 angle += K[0] * y;
kikoaac 12:a70c193d8195 715 bias += K[1] * y;
kikoaac 12:a70c193d8195 716 //update the error covariance
kikoaac 12:a70c193d8195 717 P[0][0] -= K[0] * P[0][0];
kikoaac 12:a70c193d8195 718 P[0][1] -= K[0] * P[0][1];
kikoaac 12:a70c193d8195 719 P[1][0] -= K[1] * P[0][0];
kikoaac 12:a70c193d8195 720 P[1][1] -= K[1] * P[0][1];
kikoaac 12:a70c193d8195 721
kikoaac 12:a70c193d8195 722 return angle;
kikoaac 12:a70c193d8195 723 }
kikoaac 12:a70c193d8195 724 void mpu9250_spi::kalman::setAngle(double newAngle)
kikoaac 12:a70c193d8195 725 {
kikoaac 12:a70c193d8195 726 angle = newAngle;
kikoaac 12:a70c193d8195 727 };
kikoaac 12:a70c193d8195 728 void mpu9250_spi::kalman::setQangle(double newQ_angle)
kikoaac 12:a70c193d8195 729 {
kikoaac 12:a70c193d8195 730 Q_angle = newQ_angle;
kikoaac 12:a70c193d8195 731 };
kikoaac 12:a70c193d8195 732 void mpu9250_spi::kalman::setQgyroBias(double newQ_gyroBias)
kikoaac 12:a70c193d8195 733 {
kikoaac 12:a70c193d8195 734 Q_gyroBias = newQ_gyroBias;
kikoaac 12:a70c193d8195 735 };
kikoaac 12:a70c193d8195 736 void mpu9250_spi::kalman::setRangle(double newR_angle)
kikoaac 12:a70c193d8195 737 {
kikoaac 12:a70c193d8195 738 R_angle = newR_angle;
kikoaac 12:a70c193d8195 739 };
kikoaac 12:a70c193d8195 740
kikoaac 12:a70c193d8195 741 double mpu9250_spi::kalman::getRate(void)
kikoaac 12:a70c193d8195 742 {
kikoaac 12:a70c193d8195 743 return rate;
kikoaac 12:a70c193d8195 744 };
kikoaac 12:a70c193d8195 745 double mpu9250_spi::kalman::getQangle(void)
kikoaac 12:a70c193d8195 746 {
kikoaac 12:a70c193d8195 747 return Q_angle;
kikoaac 12:a70c193d8195 748 };
kikoaac 12:a70c193d8195 749 double mpu9250_spi::kalman::getQbias(void)
kikoaac 12:a70c193d8195 750 {
kikoaac 12:a70c193d8195 751 return Q_gyroBias;
kikoaac 12:a70c193d8195 752 };
kikoaac 12:a70c193d8195 753 double mpu9250_spi::kalman::getRangle(void)
kikoaac 12:a70c193d8195 754 {
kikoaac 12:a70c193d8195 755 return R_angle;
kikoaac 12:a70c193d8195 756 };
kylongmu 0:768d2e151834 757
kylongmu 0:768d2e151834 758 /*-----------------------------------------------------------------------------------------------
kylongmu 0:768d2e151834 759 SPI SELECT AND DESELECT
kylongmu 0:768d2e151834 760 usage: enable and disable mpu9250 communication bus
kylongmu 0:768d2e151834 761 -----------------------------------------------------------------------------------------------*/
kikoaac 12:a70c193d8195 762 void mpu9250_spi::select()
kikoaac 12:a70c193d8195 763 {
kylongmu 0:768d2e151834 764 //Set CS low to start transmission (interrupts conversion)
kylongmu 0:768d2e151834 765 cs = 0;
kylongmu 0:768d2e151834 766 }
kikoaac 12:a70c193d8195 767 void mpu9250_spi::deselect()
kikoaac 12:a70c193d8195 768 {
kylongmu 0:768d2e151834 769 //Set CS high to stop transmission (restarts conversion)
kylongmu 0:768d2e151834 770 cs = 1;
kylongmu 0:768d2e151834 771 }