Kleber Silva
/
IOTON-API
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: ton_demo ton_template
Embed:
(wiki syntax)
Show/hide line numbers
BMX055.h
00001 /* IMU chipset BMX055 Library 00002 * Copyright (c) 2016 Ioton Technology 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #ifndef BMX055_H_ 00018 #define BMX055_H_ 00019 00020 /* Includes ------------------------------------------------------------------*/ 00021 #include "mbed.h" 00022 00023 00024 #ifndef M_PI 00025 #define M_PI 3.1415927f 00026 #endif 00027 00028 // Accelerometer registers 00029 #define BMX055_ACC_WHOAMI 0x00 // should return 0xFA 00030 //#define BMX055_ACC_Reserved 0x01 00031 #define BMX055_ACC_D_X_LSB 0x02 00032 #define BMX055_ACC_D_X_MSB 0x03 00033 #define BMX055_ACC_D_Y_LSB 0x04 00034 #define BMX055_ACC_D_Y_MSB 0x05 00035 #define BMX055_ACC_D_Z_LSB 0x06 00036 #define BMX055_ACC_D_Z_MSB 0x07 00037 #define BMX055_ACC_D_TEMP 0x08 00038 #define BMX055_ACC_INT_STATUS_0 0x09 00039 #define BMX055_ACC_INT_STATUS_1 0x0A 00040 #define BMX055_ACC_INT_STATUS_2 0x0B 00041 #define BMX055_ACC_INT_STATUS_3 0x0C 00042 //#define BMX055_ACC_Reserved 0x0D 00043 #define BMX055_ACC_FIFO_STATUS 0x0E 00044 #define BMX055_ACC_PMU_RANGE 0x0F 00045 #define BMX055_ACC_PMU_BW 0x10 00046 #define BMX055_ACC_PMU_LPW 0x11 00047 #define BMX055_ACC_PMU_LOW_POWER 0x12 00048 #define BMX055_ACC_D_HBW 0x13 00049 #define BMX055_ACC_BGW_SOFTRESET 0x14 00050 //#define BMX055_ACC_Reserved 0x15 00051 #define BMX055_ACC_INT_EN_0 0x16 00052 #define BMX055_ACC_INT_EN_1 0x17 00053 #define BMX055_ACC_INT_EN_2 0x18 00054 #define BMX055_ACC_INT_MAP_0 0x19 00055 #define BMX055_ACC_INT_MAP_1 0x1A 00056 #define BMX055_ACC_INT_MAP_2 0x1B 00057 //#define BMX055_ACC_Reserved 0x1C 00058 //#define BMX055_ACC_Reserved 0x1D 00059 #define BMX055_ACC_INT_SRC 0x1E 00060 //#define BMX055_ACC_Reserved 0x1F 00061 #define BMX055_ACC_INT_OUT_CTRL 0x20 00062 #define BMX055_ACC_INT_RST_LATCH 0x21 00063 #define BMX055_ACC_INT_0 0x22 00064 #define BMX055_ACC_INT_1 0x23 00065 #define BMX055_ACC_INT_2 0x24 00066 #define BMX055_ACC_INT_3 0x25 00067 #define BMX055_ACC_INT_4 0x26 00068 #define BMX055_ACC_INT_5 0x27 00069 #define BMX055_ACC_INT_6 0x28 00070 #define BMX055_ACC_INT_7 0x29 00071 #define BMX055_ACC_INT_8 0x2A 00072 #define BMX055_ACC_INT_9 0x2B 00073 #define BMX055_ACC_INT_A 0x2C 00074 #define BMX055_ACC_INT_B 0x2D 00075 #define BMX055_ACC_INT_C 0x2E 00076 #define BMX055_ACC_INT_D 0x2F 00077 #define BMX055_ACC_FIFO_CONFIG_0 0x30 00078 //#define BMX055_ACC_Reserved 0x31 00079 #define BMX055_ACC_PMU_SELF_TEST 0x32 00080 #define BMX055_ACC_TRIM_NVM_CTRL 0x33 00081 #define BMX055_ACC_BGW_SPI3_WDT 0x34 00082 //#define BMX055_ACC_Reserved 0x35 00083 #define BMX055_ACC_OFC_CTRL 0x36 00084 #define BMX055_ACC_OFC_SETTING 0x37 00085 #define BMX055_ACC_OFC_OFFSET_X 0x38 00086 #define BMX055_ACC_OFC_OFFSET_Y 0x39 00087 #define BMX055_ACC_OFC_OFFSET_Z 0x3A 00088 #define BMX055_ACC_TRIM_GPO 0x3B 00089 #define BMX055_ACC_TRIM_GP1 0x3C 00090 //#define BMX055_ACC_Reserved 0x3D 00091 #define BMX055_ACC_FIFO_CONFIG_1 0x3E 00092 #define BMX055_ACC_FIFO_DATA 0x3F 00093 00094 // BMX055 Gyroscope Registers 00095 #define BMX055_GYRO_WHOAMI 0x00 // should return 0x0F 00096 //#define BMX055_GYRO_Reserved 0x01 00097 #define BMX055_GYRO_RATE_X_LSB 0x02 00098 #define BMX055_GYRO_RATE_X_MSB 0x03 00099 #define BMX055_GYRO_RATE_Y_LSB 0x04 00100 #define BMX055_GYRO_RATE_Y_MSB 0x05 00101 #define BMX055_GYRO_RATE_Z_LSB 0x06 00102 #define BMX055_GYRO_RATE_Z_MSB 0x07 00103 //#define BMX055_GYRO_Reserved 0x08 00104 #define BMX055_GYRO_INT_STATUS_0 0x09 00105 #define BMX055_GYRO_INT_STATUS_1 0x0A 00106 #define BMX055_GYRO_INT_STATUS_2 0x0B 00107 #define BMX055_GYRO_INT_STATUS_3 0x0C 00108 //#define BMX055_GYRO_Reserved 0x0D 00109 #define BMX055_GYRO_FIFO_STATUS 0x0E 00110 #define BMX055_GYRO_RANGE 0x0F 00111 #define BMX055_GYRO_BW 0x10 00112 #define BMX055_GYRO_LPM1 0x11 00113 #define BMX055_GYRO_LPM2 0x12 00114 #define BMX055_GYRO_RATE_HBW 0x13 00115 #define BMX055_GYRO_BGW_SOFTRESET 0x14 00116 #define BMX055_GYRO_INT_EN_0 0x15 00117 #define BMX055_GYRO_INT_EN_1 0x16 00118 #define BMX055_GYRO_INT_MAP_0 0x17 00119 #define BMX055_GYRO_INT_MAP_1 0x18 00120 #define BMX055_GYRO_INT_MAP_2 0x19 00121 #define BMX055_GYRO_INT_SRC_1 0x1A 00122 #define BMX055_GYRO_INT_SRC_2 0x1B 00123 #define BMX055_GYRO_INT_SRC_3 0x1C 00124 //#define BMX055_GYRO_Reserved 0x1D 00125 #define BMX055_GYRO_FIFO_EN 0x1E 00126 //#define BMX055_GYRO_Reserved 0x1F 00127 //#define BMX055_GYRO_Reserved 0x20 00128 #define BMX055_GYRO_INT_RST_LATCH 0x21 00129 #define BMX055_GYRO_HIGH_TH_X 0x22 00130 #define BMX055_GYRO_HIGH_DUR_X 0x23 00131 #define BMX055_GYRO_HIGH_TH_Y 0x24 00132 #define BMX055_GYRO_HIGH_DUR_Y 0x25 00133 #define BMX055_GYRO_HIGH_TH_Z 0x26 00134 #define BMX055_GYRO_HIGH_DUR_Z 0x27 00135 //#define BMX055_GYRO_Reserved 0x28 00136 //#define BMX055_GYRO_Reserved 0x29 00137 //#define BMX055_GYRO_Reserved 0x2A 00138 #define BMX055_GYRO_SOC 0x31 00139 #define BMX055_GYRO_A_FOC 0x32 00140 #define BMX055_GYRO_TRIM_NVM_CTRL 0x33 00141 #define BMX055_GYRO_BGW_SPI3_WDT 0x34 00142 //#define BMX055_GYRO_Reserved 0x35 00143 #define BMX055_GYRO_OFC1 0x36 00144 #define BMX055_GYRO_OFC2 0x37 00145 #define BMX055_GYRO_OFC3 0x38 00146 #define BMX055_GYRO_OFC4 0x39 00147 #define BMX055_GYRO_TRIM_GP0 0x3A 00148 #define BMX055_GYRO_TRIM_GP1 0x3B 00149 #define BMX055_GYRO_BIST 0x3C 00150 #define BMX055_GYRO_FIFO_CONFIG_0 0x3D 00151 #define BMX055_GYRO_FIFO_CONFIG_1 0x3E 00152 00153 // BMX055 magnetometer registers 00154 #define BMX055_MAG_WHOAMI 0x40 // should return 0x32 00155 #define BMX055_MAG_Reserved 0x41 00156 #define BMX055_MAG_XOUT_LSB 0x42 00157 #define BMX055_MAG_XOUT_MSB 0x43 00158 #define BMX055_MAG_YOUT_LSB 0x44 00159 #define BMX055_MAG_YOUT_MSB 0x45 00160 #define BMX055_MAG_ZOUT_LSB 0x46 00161 #define BMX055_MAG_ZOUT_MSB 0x47 00162 #define BMX055_MAG_ROUT_LSB 0x48 00163 #define BMX055_MAG_ROUT_MSB 0x49 00164 #define BMX055_MAG_INT_STATUS 0x4A 00165 #define BMX055_MAG_PWR_CNTL1 0x4B 00166 #define BMX055_MAG_PWR_CNTL2 0x4C 00167 #define BMX055_MAG_INT_EN_1 0x4D 00168 #define BMX055_MAG_INT_EN_2 0x4E 00169 #define BMX055_MAG_LOW_THS 0x4F 00170 #define BMX055_MAG_HIGH_THS 0x50 00171 #define BMX055_MAG_REP_XY 0x51 00172 #define BMX055_MAG_REP_Z 0x52 00173 /* Trim Extended Registers */ 00174 #define BMM050_DIG_X1 0x5D // needed for magnetic field calculation 00175 #define BMM050_DIG_Y1 0x5E 00176 #define BMM050_DIG_Z4_LSB 0x62 00177 #define BMM050_DIG_Z4_MSB 0x63 00178 #define BMM050_DIG_X2 0x64 00179 #define BMM050_DIG_Y2 0x65 00180 #define BMM050_DIG_Z2_LSB 0x68 00181 #define BMM050_DIG_Z2_MSB 0x69 00182 #define BMM050_DIG_Z1_LSB 0x6A 00183 #define BMM050_DIG_Z1_MSB 0x6B 00184 #define BMM050_DIG_XYZ1_LSB 0x6C 00185 #define BMM050_DIG_XYZ1_MSB 0x6D 00186 #define BMM050_DIG_Z3_LSB 0x6E 00187 #define BMM050_DIG_Z3_MSB 0x6F 00188 #define BMM050_DIG_XY2 0x70 00189 #define BMM050_DIG_XY1 0x71 00190 00191 // Using SDO1 = SDO2 = CSB3 = GND as designed 00192 // Seven-bit device addresses are ACC = 0x18, GYRO = 0x68, MAG = 0x10 00193 #define BMX055_ACC_ADDRESS 0x18 << 1 // Address of BMX055 accelerometer 00194 #define BMX055_GYRO_ADDRESS 0x68 << 1 // Address of BMX055 gyroscope 00195 #define BMX055_MAG_ADDRESS 0x10 << 1 // Address of BMX055 magnetometer 00196 00197 // Set initial input parameters 00198 // define BMX055 ACC full scale options 00199 #define AFS_2G 0x03 00200 #define AFS_4G 0x05 00201 #define AFS_8G 0x08 00202 #define AFS_16G 0x0C 00203 00204 enum ACCBW { // define BMX055 accelerometer bandwidths 00205 ABW_8Hz, // 7.81 Hz, 64 ms update time 00206 ABW_16Hz, // 15.63 Hz, 32 ms update time 00207 ABW_31Hz, // 31.25 Hz, 16 ms update time 00208 ABW_63Hz, // 62.5 Hz, 8 ms update time 00209 ABW_125Hz, // 125 Hz, 4 ms update time 00210 ABW_250Hz, // 250 Hz, 2 ms update time 00211 ABW_500Hz, // 500 Hz, 1 ms update time 00212 ABW_100Hz // 1000 Hz, 0.5 ms update time 00213 }; 00214 00215 enum Gscale { 00216 GFS_2000DPS = 0, 00217 GFS_1000DPS, 00218 GFS_500DPS, 00219 GFS_250DPS, 00220 GFS_125DPS 00221 }; 00222 00223 enum GODRBW { 00224 G_2000Hz523Hz = 0, // 2000 Hz ODR and unfiltered (bandwidth 523Hz) 00225 G_2000Hz230Hz, 00226 G_1000Hz116Hz, 00227 G_400Hz47Hz, 00228 G_200Hz23Hz, 00229 G_100Hz12Hz, 00230 G_200Hz64Hz, 00231 G_100Hz32Hz // 100 Hz ODR and 32 Hz bandwidth 00232 }; 00233 00234 enum MODR { 00235 MODR_10Hz = 0, // 10 Hz ODR 00236 MODR_2Hz , // 2 Hz ODR 00237 MODR_6Hz , // 6 Hz ODR 00238 MODR_8Hz , // 8 Hz ODR 00239 MODR_15Hz , // 15 Hz ODR 00240 MODR_20Hz , // 20 Hz ODR 00241 MODR_25Hz , // 25 Hz ODR 00242 MODR_30Hz // 30 Hz ODR 00243 }; 00244 00245 enum Mmode { 00246 lowPower = 0, // rms noise ~1.0 microTesla, 0.17 mA power 00247 Regular , // rms noise ~0.6 microTesla, 0.5 mA power 00248 enhancedRegular , // rms noise ~0.5 microTesla, 0.8 mA power 00249 highAccuracy // rms noise ~0.3 microTesla, 4.9 mA power 00250 }; 00251 00252 // Set up I2C, (SDA,SCL) 00253 I2C i2c2(PB_11, PB_10); 00254 00255 // Specify sensor full scale 00256 uint8_t Ascale = AFS_2G; 00257 uint8_t Gscale = GFS_125DPS; 00258 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors 00259 00260 // Parameters to hold BMX055 trim values 00261 int8_t dig_x1; 00262 int8_t dig_y1; 00263 int8_t dig_x2; 00264 int8_t dig_y2; 00265 uint16_t dig_z1; 00266 int16_t dig_z2; 00267 int16_t dig_z3; 00268 int16_t dig_z4; 00269 uint8_t dig_xy1; 00270 int8_t dig_xy2; 00271 uint16_t dig_xyz1; 00272 00273 // BMX055 variables 00274 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output 00275 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output 00276 int16_t magCount[3]; // Stores the 13/15-bit signed magnetometer sensor output 00277 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, mag 00278 float SelfTest[6]; // holds results of gyro and accelerometer self test 00279 00280 // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System) 00281 float GyroMeasError = M_PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s) 00282 float GyroMeasDrift = M_PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) 00283 // There is a tradeoff in the beta parameter between accuracy and response speed. 00284 // In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. 00285 // However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion. 00286 // Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car! 00287 // By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec 00288 // I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense; 00289 // the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy. 00290 // In any case, this is the free parameter in the Madgwick filtering and fusion scheme. 00291 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta 00292 float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value 00293 #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral 00294 #define Ki 0.0f 00295 00296 // Declination at Sao Paulo, Brazil is -21 degrees 7 minutes on 2016-03-27 00297 #define LOCAL_DECLINATION -21.1f 00298 00299 float deltat = 0.0f; // integration interval for both filter schemes 00300 00301 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion 00302 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method 00303 00304 00305 class BMX055 { 00306 private: 00307 float pitch, yaw, roll; 00308 00309 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) 00310 { 00311 char data_write[2]; 00312 data_write[0] = subAddress; 00313 data_write[1] = data; 00314 i2c2.write(address, data_write, 2, 0); 00315 } 00316 00317 char readByte(uint8_t address, uint8_t subAddress) 00318 { 00319 char data[1]; // `data` will store the register data 00320 char data_write[1]; 00321 data_write[0] = subAddress; 00322 i2c2.write(address, data_write, 1, 1); // no stop 00323 i2c2.read(address, data, 1, 0); 00324 return data[0]; 00325 } 00326 00327 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) 00328 { 00329 char data[14]; 00330 char data_write[1]; 00331 data_write[0] = subAddress; 00332 i2c2.write(address, data_write, 1, 1); // no stop 00333 i2c2.read(address, data, count, 0); 00334 for(int ii = 0; ii < count; ii++) 00335 { 00336 dest[ii] = data[ii]; 00337 } 00338 } 00339 00340 public: 00341 BMX055() 00342 { 00343 //Set up I2C 00344 i2c2.frequency(400000); // use fast (400 kHz) I2C 00345 } 00346 00347 float getAres(void) 00348 { 00349 switch (Ascale) 00350 { 00351 // Possible accelerometer scales (and their register bit settings) are: 00352 // 2 Gs (0011), 4 Gs (0101), 8 Gs (1000), and 16 Gs (1100). 00353 // BMX055 ACC data is signed 12 bit 00354 case AFS_2G: 00355 aRes = 2.0/2048.0; 00356 break; 00357 case AFS_4G: 00358 aRes = 4.0/2048.0; 00359 break; 00360 case AFS_8G: 00361 aRes = 8.0/2048.0; 00362 break; 00363 case AFS_16G: 00364 aRes = 16.0/2048.0; 00365 break; 00366 } 00367 00368 return aRes; 00369 } 00370 00371 float getGres(void) 00372 { 00373 switch (Gscale) 00374 { 00375 // Possible gyro scales (and their register bit settings) are: 00376 // 125 DPS (100), 250 DPS (011), 500 DPS (010), 1000 DPS (001), and 2000 DPS (000). 00377 case GFS_125DPS: 00378 gRes = 124.87/32768.0; // per data sheet, not exactly 125!? 00379 break; 00380 case GFS_250DPS: 00381 gRes = 249.75/32768.0; 00382 break; 00383 case GFS_500DPS: 00384 gRes = 499.5/32768.0; 00385 break; 00386 case GFS_1000DPS: 00387 gRes = 999.0/32768.0; 00388 break; 00389 case GFS_2000DPS: 00390 gRes = 1998.0/32768.0; 00391 break; 00392 } 00393 00394 return gRes; 00395 } 00396 00397 float getPitch(void) 00398 { 00399 return pitch; 00400 } 00401 00402 float getRoll(void) 00403 { 00404 return roll; 00405 } 00406 00407 float getYaw(void) 00408 { 00409 return yaw; 00410 } 00411 00412 void readAccelData(int16_t * destination) 00413 { 00414 uint8_t rawData[6]; // x/y/z accel register data stored here 00415 00416 // Read the six raw data registers into data array 00417 readBytes(BMX055_ACC_ADDRESS, BMX055_ACC_D_X_LSB, 6, &rawData[0]); 00418 if((rawData[0] & 0x01) && (rawData[2] & 0x01) && (rawData[4] & 0x01)) 00419 { // Check that all 3 axes have new data 00420 // Turn the MSB and LSB into a signed 12-bit value 00421 destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]) >> 4; 00422 destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]) >> 4; 00423 destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]) >> 4; 00424 } 00425 } 00426 00427 void readGyroData(int16_t * destination) 00428 { 00429 uint8_t rawData[6]; // x/y/z gyro register data stored here 00430 00431 // Read the six raw data registers sequentially into data array 00432 readBytes(BMX055_GYRO_ADDRESS, BMX055_GYRO_RATE_X_LSB, 6, &rawData[0]); 00433 // Turn the MSB and LSB into a signed 16-bit value 00434 destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); 00435 destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]); 00436 destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]); 00437 } 00438 00439 void readMagData(int16_t * magData) 00440 { 00441 int16_t mdata_x = 0, mdata_y = 0, mdata_z = 0, temp = 0; 00442 uint16_t data_r = 0; 00443 uint8_t rawData[8]; // x/y/z hall magnetic field data, and Hall resistance data 00444 readBytes(BMX055_MAG_ADDRESS, BMX055_MAG_XOUT_LSB, 8, &rawData[0]); // Read the eight raw data registers sequentially into data array 00445 if(rawData[6] & 0x01) { // Check if data ready status bit is set 00446 mdata_x = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]) >> 3; // 13-bit signed integer for x-axis field 00447 mdata_y = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]) >> 3; // 13-bit signed integer for y-axis field 00448 mdata_z = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]) >> 1; // 15-bit signed integer for z-axis field 00449 data_r = (uint16_t) (((uint16_t)rawData[7] << 8) | rawData[6]) >> 2; // 14-bit unsigned integer for Hall resistance 00450 00451 // calculate temperature compensated 16-bit magnetic fields 00452 temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); 00453 magData[0] = ((int16_t)((((int32_t)mdata_x) * 00454 ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + 00455 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + 00456 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + 00457 (((int16_t)dig_x1) << 3); 00458 00459 temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000))); 00460 magData[1] = ((int16_t)((((int32_t)mdata_y) * 00461 ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) + 00462 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) + 00463 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) + 00464 (((int16_t)dig_y1) << 3); 00465 magData[2] = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) - 00466 ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16)))); 00467 } 00468 } 00469 00470 float getTemperature() 00471 { 00472 uint8_t c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_D_TEMP); // Read the raw data register 00473 int16_t tempCount = ((int16_t)((int16_t)c << 8)) >> 8 ; // Turn the byte into a signed 8-bit integer 00474 00475 return ((((float)tempCount) * 0.5f) + 23.0f); // temperature in degrees Centigrade 00476 } 00477 00478 void fastcompaccelBMX055(float * dest1) 00479 { 00480 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x80); // set all accel offset compensation registers to zero 00481 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_SETTING, 0x20); // set offset targets to 0, 0, and +1 g for x, y, z axes 00482 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x20); // calculate x-axis offset 00483 00484 uint8_t c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL); 00485 while(!(c & 0x10)) 00486 { // check if fast calibration complete 00487 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL); 00488 wait_ms(10); 00489 } 00490 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x40); // calculate y-axis offset 00491 00492 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL); 00493 while(!(c & 0x10)) 00494 { // check if fast calibration complete 00495 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL); 00496 wait_ms(10); 00497 } 00498 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x60); // calculate z-axis offset 00499 00500 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL); 00501 while(!(c & 0x10)) 00502 { // check if fast calibration complete 00503 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL); 00504 wait_ms(10); 00505 } 00506 00507 int8_t compx = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_OFFSET_X); 00508 int8_t compy = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_OFFSET_Y); 00509 int8_t compz = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_OFFSET_Z); 00510 00511 dest1[0] = (float) compx/128.0f; // accleration bias in g 00512 dest1[1] = (float) compy/128.0f; // accleration bias in g 00513 dest1[2] = (float) compz/128.0f; // accleration bias in g 00514 } 00515 00516 void magcalBMX055(float * dest1) 00517 { 00518 uint16_t ii = 0, sample_count = 0; 00519 int32_t mag_bias[3] = {0, 0, 0}; 00520 int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0}; 00521 00522 // ## "Mag Calibration: Wave device in a figure eight until done!" ## 00523 //wait(4); 00524 00525 sample_count = 48; 00526 for(ii = 0; ii < sample_count; ii++) 00527 { 00528 int16_t mag_temp[3] = {0, 0, 0}; 00529 readMagData(mag_temp); 00530 for (int jj = 0; jj < 3; jj++) 00531 { 00532 if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj]; 00533 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj]; 00534 } 00535 wait_ms(105); // at 10 Hz ODR, new mag data is available every 100 ms 00536 } 00537 00538 mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts 00539 mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts 00540 mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts 00541 00542 // save mag biases in G for main program 00543 dest1[0] = (float) mag_bias[0]*mRes; 00544 dest1[1] = (float) mag_bias[1]*mRes; 00545 dest1[2] = (float) mag_bias[2]*mRes; 00546 00547 // ## "Mag Calibration done!" ## 00548 } 00549 00550 // Get trim values for magnetometer sensitivity 00551 void trim(void) 00552 { 00553 uint8_t rawData[2]; //placeholder for 2-byte trim data 00554 00555 dig_x1 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_X1); 00556 dig_x2 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_X2); 00557 dig_y1 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_Y1); 00558 dig_y2 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_Y2); 00559 dig_xy1 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_XY1); 00560 dig_xy2 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_XY2); 00561 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z1_LSB, 2, &rawData[0]); 00562 dig_z1 = (uint16_t) (((uint16_t)rawData[1] << 8) | rawData[0]); 00563 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z2_LSB, 2, &rawData[0]); 00564 dig_z2 = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); 00565 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z3_LSB, 2, &rawData[0]); 00566 dig_z3 = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); 00567 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z4_LSB, 2, &rawData[0]); 00568 dig_z4 = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]); 00569 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_XYZ1_LSB, 2, &rawData[0]); 00570 dig_xyz1 = (uint16_t) (((uint16_t)rawData[1] << 8) | rawData[0]); 00571 } 00572 00573 /** Initialize device for active mode 00574 * @param Ascale set accel full scale 00575 * @param ACCBW set bandwidth for accelerometer 00576 * @param Gscale set gyro full scale 00577 * @param GODRBW set gyro ODR and bandwidth 00578 * @param Mmode set magnetometer operation mode 00579 * @param MODR set magnetometer data rate 00580 * @see ACCBW, GODRBW and MODR enums 00581 */ 00582 void init(uint8_t mAscale = AFS_2G, uint8_t ACCBW = ABW_16Hz, 00583 uint8_t mGscale = GFS_125DPS, uint8_t GODRBW = G_200Hz23Hz, 00584 uint8_t Mmode = Regular, uint8_t MODR = MODR_10Hz) 00585 { 00586 Ascale = mAscale; 00587 Gscale = mGscale; 00588 00589 // Configure accelerometer 00590 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_PMU_RANGE, Ascale & 0x0F); // Set accelerometer full scale 00591 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_PMU_BW, (0x08 | ACCBW) & 0x0F); // Set accelerometer bandwidth 00592 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_D_HBW, 0x00); // Use filtered data 00593 00594 // Configure Gyro 00595 writeByte(BMX055_GYRO_ADDRESS, BMX055_GYRO_RANGE, Gscale); // set GYRO FS range 00596 writeByte(BMX055_GYRO_ADDRESS, BMX055_GYRO_BW, GODRBW); // set GYRO ODR and Bandwidth 00597 00598 // Configure magnetometer 00599 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_PWR_CNTL1, 0x82); // Softreset magnetometer, ends up in sleep mode 00600 wait_ms(100); 00601 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_PWR_CNTL1, 0x01); // Wake up magnetometer 00602 wait_ms(100); 00603 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_PWR_CNTL2, MODR << 3); // Normal mode 00604 00605 // Set up four standard configurations for the magnetometer 00606 switch (Mmode) 00607 { 00608 case lowPower: 00609 // Low-power 00610 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x01); // 3 repetitions (oversampling) 00611 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x02); // 3 repetitions (oversampling) 00612 break; 00613 case Regular: 00614 // Regular 00615 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x04); // 9 repetitions (oversampling) 00616 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x16); // 15 repetitions (oversampling) 00617 break; 00618 case enhancedRegular: 00619 // Enhanced Regular 00620 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x07); // 15 repetitions (oversampling) 00621 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x22); // 27 repetitions (oversampling) 00622 break; 00623 case highAccuracy: 00624 // High Accuracy 00625 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x17); // 47 repetitions (oversampling) 00626 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x51); // 83 repetitions (oversampling) 00627 break; 00628 } 00629 00630 // get sensor resolutions, only need to do this once 00631 getAres(); 00632 getGres(); 00633 // magnetometer resolution is 1 microTesla/16 counts or 1/1.6 milliGauss/count 00634 mRes = 1./1.6; 00635 00636 trim(); // read the magnetometer calibration data 00637 00638 fastcompaccelBMX055(accelBias); 00639 magcalBMX055(magBias); 00640 // TODO: see magcalBMX055(): 128 samples * 105ms = 13.44s 00641 // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically 00642 // like the gyro and accelerometer biases 00643 // magBias[0] = -5.; // User environmental x-axis correction in milliGauss 00644 // magBias[1] = -95.; // User environmental y-axis correction in milliGauss 00645 // magBias[2] = -260.; // User environmental z-axis correction in milliGauss 00646 00647 } 00648 00649 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" 00650 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details) 00651 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute 00652 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. 00653 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms 00654 // but is much less computationally intensive 00655 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz) 00656 { 00657 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability 00658 float norm; 00659 float hx, hy, _2bx, _2bz; 00660 float s1, s2, s3, s4; 00661 float qDot1, qDot2, qDot3, qDot4; 00662 00663 // Auxiliary variables to avoid repeated arithmetic 00664 float _2q1mx; 00665 float _2q1my; 00666 float _2q1mz; 00667 float _2q2mx; 00668 float _4bx; 00669 float _4bz; 00670 float _2q1 = 2.0f * q1; 00671 float _2q2 = 2.0f * q2; 00672 float _2q3 = 2.0f * q3; 00673 float _2q4 = 2.0f * q4; 00674 float _2q1q3 = 2.0f * q1 * q3; 00675 float _2q3q4 = 2.0f * q3 * q4; 00676 float q1q1 = q1 * q1; 00677 float q1q2 = q1 * q2; 00678 float q1q3 = q1 * q3; 00679 float q1q4 = q1 * q4; 00680 float q2q2 = q2 * q2; 00681 float q2q3 = q2 * q3; 00682 float q2q4 = q2 * q4; 00683 float q3q3 = q3 * q3; 00684 float q3q4 = q3 * q4; 00685 float q4q4 = q4 * q4; 00686 00687 // Normalise accelerometer measurement 00688 norm = sqrt(ax * ax + ay * ay + az * az); 00689 if (norm == 0.0f) return; // handle NaN 00690 norm = 1.0f/norm; 00691 ax *= norm; 00692 ay *= norm; 00693 az *= norm; 00694 00695 // Normalise magnetometer measurement 00696 norm = sqrt(mx * mx + my * my + mz * mz); 00697 if (norm == 0.0f) return; // handle NaN 00698 norm = 1.0f/norm; 00699 mx *= norm; 00700 my *= norm; 00701 mz *= norm; 00702 00703 // Reference direction of Earth's magnetic field 00704 _2q1mx = 2.0f * q1 * mx; 00705 _2q1my = 2.0f * q1 * my; 00706 _2q1mz = 2.0f * q1 * mz; 00707 _2q2mx = 2.0f * q2 * mx; 00708 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; 00709 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; 00710 _2bx = sqrt(hx * hx + hy * hy); 00711 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; 00712 _4bx = 2.0f * _2bx; 00713 _4bz = 2.0f * _2bz; 00714 00715 // Gradient decent algorithm corrective step 00716 s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00717 s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00718 s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00719 s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); 00720 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude 00721 norm = 1.0f/norm; 00722 s1 *= norm; 00723 s2 *= norm; 00724 s3 *= norm; 00725 s4 *= norm; 00726 00727 // Compute rate of change of quaternion 00728 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; 00729 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; 00730 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; 00731 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; 00732 00733 // Integrate to yield quaternion 00734 q1 += qDot1 * deltat; 00735 q2 += qDot2 * deltat; 00736 q3 += qDot3 * deltat; 00737 q4 += qDot4 * deltat; 00738 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion 00739 norm = 1.0f/norm; 00740 q[0] = q1 * norm; 00741 q[1] = q2 * norm; 00742 q[2] = q3 * norm; 00743 q[3] = q4 * norm; 00744 00745 } 00746 00747 /** Get raw 9-axis motion sensor readings (accel/gyro/compass). 00748 * @param ax 12-bit signed integer container for accelerometer X-axis value 00749 * @param ay 12-bit signed integer container for accelerometer Y-axis value 00750 * @param az 12-bit signed integer container for accelerometer Z-axis value 00751 * @param gx 16-bit signed integer container for gyroscope X-axis value 00752 * @param gy 16-bit signed integer container for gyroscope Y-axis value 00753 * @param gz 16-bit signed integer container for gyroscope Z-axis value 00754 * @param mx 13-bit signed integer container for magnetometer X-axis value 00755 * @param my 13-bit signed integer container for magnetometer Y-axis value 00756 * @param mz 15-bit signed integer container for magnetometer Z-axis value 00757 * @see getAcceleration() 00758 * @see getRotation() 00759 * @see getMag() 00760 */ 00761 void getRaw9( int16_t* ax, int16_t* ay, int16_t* az, 00762 int16_t* gx, int16_t* gy, int16_t* gz, 00763 int16_t* mx, int16_t* my, int16_t* mz) 00764 { 00765 uint8_t rawData[8]; // x/y/z MSB and LSB registers raw data stored here 00766 00767 // Read the six raw data registers into data array 00768 // Turn the MSB and LSB into a signed 12-bit value 00769 readBytes(BMX055_ACC_ADDRESS, BMX055_ACC_D_X_LSB, 6, rawData); 00770 *ax = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]) >> 4; 00771 *ay = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) >> 4; 00772 *az = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) >> 4; 00773 00774 // Read the six raw data registers sequentially into data array 00775 // Turn the MSB and LSB into a signed 16-bit value 00776 readBytes(BMX055_GYRO_ADDRESS, BMX055_GYRO_RATE_X_LSB, 6, rawData); 00777 *gx = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); 00778 *gy = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]); 00779 *gz = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]); 00780 00781 // Read the six raw data registers into data array 00782 // 13-bit signed integer for x-axis and y-axis field 00783 // 15-bit signed integer for z-axis field 00784 readBytes(BMX055_MAG_ADDRESS, BMX055_MAG_XOUT_LSB, 8, rawData); 00785 if(rawData[6] & 0x01) // Check if data ready status bit is set 00786 { 00787 *mx = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]) >> 3; 00788 *my = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) >> 3; 00789 *mz = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) >> 1; 00790 } 00791 } 00792 00793 /** Get raw 9-axis motion sensor readings (accel/gyro/compass). 00794 * @param ax accelerometer X-axis value (g's) 00795 * @param ay accelerometer Y-axis value (g's) 00796 * @param az accelerometer Z-axis value (g's) 00797 * @param gx gyroscope X-axis value (degrees per second) 00798 * @param gy gyroscope Y-axis value (degrees per second) 00799 * @param gz gyroscope Z-axis value (degrees per second) 00800 * @param mx magnetometer X-axis value (milliGauss) 00801 * @param my magnetometer Y-axis value (milliGauss) 00802 * @param mz magnetometer Z-axis value (milliGauss) 00803 * @see getAcceleration() 00804 * @see getRotation() 00805 * @see getMag() 00806 */ 00807 void getMotion9( float* ax, float* ay, float* az, 00808 float* gx, float* gy, float* gz, 00809 float* mx, float* my, float* mz) 00810 { 00811 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output 00812 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output 00813 int16_t magCount[3]; // Stores the 13/15-bit signed magnetometer sensor output 00814 00815 // Read the x/y/z raw values 00816 readAccelData(accelCount); 00817 00818 // Calculate the accleration value into actual g's 00819 // get actual g value, this depends on scale being set 00820 *ax = (float)accelCount[0]*aRes; // + accelBias[0]; 00821 *ay = (float)accelCount[1]*aRes; // + accelBias[1]; 00822 *az = (float)accelCount[2]*aRes; // + accelBias[2]; 00823 00824 00825 // Read the x/y/z raw values 00826 readGyroData(gyroCount); 00827 00828 // Calculate the gyro value into actual degrees per second 00829 // get actual gyro value, this depends on scale being set 00830 *gx = (float)gyroCount[0]*gRes; 00831 *gy = (float)gyroCount[1]*gRes; 00832 *gz = (float)gyroCount[2]*gRes; 00833 00834 00835 // Read the x/y/z raw values 00836 readMagData(magCount); 00837 00838 // Calculate the magnetometer values in milliGauss 00839 // Temperature-compensated magnetic field is in 16 LSB/microTesla 00840 // get actual magnetometer value, this depends on scale being set 00841 *mx = (float)magCount[0]*mRes - magBias[0]; 00842 *my = (float)magCount[1]*mRes - magBias[1]; 00843 *mz = (float)magCount[2]*mRes - magBias[2]; 00844 } 00845 00846 void runAHRS(float mdeltat, float local_declination = LOCAL_DECLINATION) 00847 { 00848 float ax, ay, az, gx, gy, gz, mx, my, mz; 00849 00850 getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz); 00851 deltat = mdeltat; 00852 00853 // Sensors x (y)-axis of the accelerometer is aligned with the -y (x)-axis of the magnetometer; 00854 // the magnetometer z-axis (+ up) is aligned with z-axis (+ up) of accelerometer and gyro! 00855 // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter. 00856 // For the BMX-055, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like 00857 // in the MPU9250 sensor. This rotation can be modified to allow any convenient orientation convention. 00858 // This is ok by aircraft orientation standards! 00859 // Pass gyro rate as rad/s 00860 //MadgwickQuaternionUpdate(ax, ay, az, gx*M_PI/180.0f, gy*M_PI/180.0f, gz*M_PI/180.0f, -my, mx, mz); 00861 MadgwickQuaternionUpdate(-ay, ax, az, -gy*M_PI/180.0f, gx*M_PI/180.0f, gz*M_PI/180.0f, mx, my, mz); 00862 00863 00864 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 00865 // In this coordinate system, the positive z-axis is down toward Earth. 00866 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. 00867 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 00868 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 00869 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 00870 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 00871 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 00872 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 00873 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); 00874 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00875 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); 00876 pitch *= 180.0f / M_PI; 00877 yaw *= 180.0f / M_PI; 00878 yaw -= local_declination; 00879 roll *= 180.0f / M_PI; 00880 } 00881 }; 00882 00883 #endif /* BMX055_H_ */
Generated on Tue Jul 12 2022 20:52:33 by
1.7.2