IOTON Technology / IOTON-API

IOTON boards API using mbed SDK - http://ioton.cc/plataforma-ton

Dependents:   ton-bot_teste ton-bot_seguidor_linha ton-bot_seguidor_parede

Fork of IOTON-API by Kleber Silva

BMX055.h@3:9c7195d31602, 2017-06-29 (annotated)

Committer:
krebyy
Date:
Thu Jun 29 20:20:49 2017 +0000
Revision:
3:9c7195d31602
Parent:
2:b3c3bf0b9101 
Update to TON Board V1.2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krebyy 0:cbba28a205fa 1 /* IMU chipset BMX055 Library
krebyy 0:cbba28a205fa 2 * Copyright (c) 2016 Ioton Technology
krebyy 0:cbba28a205fa 3 *
krebyy 0:cbba28a205fa 4 * Licensed under the Apache License, Version 2.0 (the "License");
krebyy 0:cbba28a205fa 5 * you may not use this file except in compliance with the License.
krebyy 0:cbba28a205fa 6 * You may obtain a copy of the License at
krebyy 0:cbba28a205fa 7 *
krebyy 0:cbba28a205fa 8 * http://www.apache.org/licenses/LICENSE-2.0
krebyy 0:cbba28a205fa 9 *
krebyy 0:cbba28a205fa 10 * Unless required by applicable law or agreed to in writing, software
krebyy 0:cbba28a205fa 11 * distributed under the License is distributed on an "AS IS" BASIS,
krebyy 0:cbba28a205fa 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
krebyy 0:cbba28a205fa 13 * See the License for the specific language governing permissions and
krebyy 0:cbba28a205fa 14 * limitations under the License.
krebyy 0:cbba28a205fa 15 */
krebyy 0:cbba28a205fa 16
krebyy 0:cbba28a205fa 17 #ifndef BMX055_H_
krebyy 0:cbba28a205fa 18 #define BMX055_H_
krebyy 0:cbba28a205fa 19
krebyy 0:cbba28a205fa 20 /* Includes ------------------------------------------------------------------*/
krebyy 0:cbba28a205fa 21 #include "mbed.h"
krebyy 0:cbba28a205fa 22
krebyy 0:cbba28a205fa 23
krebyy 0:cbba28a205fa 24 #ifndef M_PI
krebyy 2:b3c3bf0b9101 25 #define M_PI 3.1415927f
krebyy 0:cbba28a205fa 26 #endif
krebyy 0:cbba28a205fa 27
krebyy 0:cbba28a205fa 28 // Accelerometer registers
krebyy 0:cbba28a205fa 29 #define BMX055_ACC_WHOAMI 0x00 // should return 0xFA
krebyy 0:cbba28a205fa 30 //#define BMX055_ACC_Reserved 0x01
krebyy 0:cbba28a205fa 31 #define BMX055_ACC_D_X_LSB 0x02
krebyy 0:cbba28a205fa 32 #define BMX055_ACC_D_X_MSB 0x03
krebyy 0:cbba28a205fa 33 #define BMX055_ACC_D_Y_LSB 0x04
krebyy 0:cbba28a205fa 34 #define BMX055_ACC_D_Y_MSB 0x05
krebyy 0:cbba28a205fa 35 #define BMX055_ACC_D_Z_LSB 0x06
krebyy 0:cbba28a205fa 36 #define BMX055_ACC_D_Z_MSB 0x07
krebyy 0:cbba28a205fa 37 #define BMX055_ACC_D_TEMP 0x08
krebyy 0:cbba28a205fa 38 #define BMX055_ACC_INT_STATUS_0 0x09
krebyy 0:cbba28a205fa 39 #define BMX055_ACC_INT_STATUS_1 0x0A
krebyy 0:cbba28a205fa 40 #define BMX055_ACC_INT_STATUS_2 0x0B
krebyy 0:cbba28a205fa 41 #define BMX055_ACC_INT_STATUS_3 0x0C
krebyy 0:cbba28a205fa 42 //#define BMX055_ACC_Reserved 0x0D
krebyy 0:cbba28a205fa 43 #define BMX055_ACC_FIFO_STATUS 0x0E
krebyy 0:cbba28a205fa 44 #define BMX055_ACC_PMU_RANGE 0x0F
krebyy 0:cbba28a205fa 45 #define BMX055_ACC_PMU_BW 0x10
krebyy 0:cbba28a205fa 46 #define BMX055_ACC_PMU_LPW 0x11
krebyy 0:cbba28a205fa 47 #define BMX055_ACC_PMU_LOW_POWER 0x12
krebyy 0:cbba28a205fa 48 #define BMX055_ACC_D_HBW 0x13
krebyy 0:cbba28a205fa 49 #define BMX055_ACC_BGW_SOFTRESET 0x14
krebyy 0:cbba28a205fa 50 //#define BMX055_ACC_Reserved 0x15
krebyy 0:cbba28a205fa 51 #define BMX055_ACC_INT_EN_0 0x16
krebyy 0:cbba28a205fa 52 #define BMX055_ACC_INT_EN_1 0x17
krebyy 0:cbba28a205fa 53 #define BMX055_ACC_INT_EN_2 0x18
krebyy 0:cbba28a205fa 54 #define BMX055_ACC_INT_MAP_0 0x19
krebyy 0:cbba28a205fa 55 #define BMX055_ACC_INT_MAP_1 0x1A
krebyy 0:cbba28a205fa 56 #define BMX055_ACC_INT_MAP_2 0x1B
krebyy 0:cbba28a205fa 57 //#define BMX055_ACC_Reserved 0x1C
krebyy 0:cbba28a205fa 58 //#define BMX055_ACC_Reserved 0x1D
krebyy 0:cbba28a205fa 59 #define BMX055_ACC_INT_SRC 0x1E
krebyy 0:cbba28a205fa 60 //#define BMX055_ACC_Reserved 0x1F
krebyy 0:cbba28a205fa 61 #define BMX055_ACC_INT_OUT_CTRL 0x20
krebyy 0:cbba28a205fa 62 #define BMX055_ACC_INT_RST_LATCH 0x21
krebyy 0:cbba28a205fa 63 #define BMX055_ACC_INT_0 0x22
krebyy 0:cbba28a205fa 64 #define BMX055_ACC_INT_1 0x23
krebyy 0:cbba28a205fa 65 #define BMX055_ACC_INT_2 0x24
krebyy 0:cbba28a205fa 66 #define BMX055_ACC_INT_3 0x25
krebyy 0:cbba28a205fa 67 #define BMX055_ACC_INT_4 0x26
krebyy 0:cbba28a205fa 68 #define BMX055_ACC_INT_5 0x27
krebyy 0:cbba28a205fa 69 #define BMX055_ACC_INT_6 0x28
krebyy 0:cbba28a205fa 70 #define BMX055_ACC_INT_7 0x29
krebyy 0:cbba28a205fa 71 #define BMX055_ACC_INT_8 0x2A
krebyy 0:cbba28a205fa 72 #define BMX055_ACC_INT_9 0x2B
krebyy 0:cbba28a205fa 73 #define BMX055_ACC_INT_A 0x2C
krebyy 0:cbba28a205fa 74 #define BMX055_ACC_INT_B 0x2D
krebyy 0:cbba28a205fa 75 #define BMX055_ACC_INT_C 0x2E
krebyy 0:cbba28a205fa 76 #define BMX055_ACC_INT_D 0x2F
krebyy 0:cbba28a205fa 77 #define BMX055_ACC_FIFO_CONFIG_0 0x30
krebyy 0:cbba28a205fa 78 //#define BMX055_ACC_Reserved 0x31
krebyy 0:cbba28a205fa 79 #define BMX055_ACC_PMU_SELF_TEST 0x32
krebyy 0:cbba28a205fa 80 #define BMX055_ACC_TRIM_NVM_CTRL 0x33
krebyy 0:cbba28a205fa 81 #define BMX055_ACC_BGW_SPI3_WDT 0x34
krebyy 0:cbba28a205fa 82 //#define BMX055_ACC_Reserved 0x35
krebyy 0:cbba28a205fa 83 #define BMX055_ACC_OFC_CTRL 0x36
krebyy 0:cbba28a205fa 84 #define BMX055_ACC_OFC_SETTING 0x37
krebyy 0:cbba28a205fa 85 #define BMX055_ACC_OFC_OFFSET_X 0x38
krebyy 0:cbba28a205fa 86 #define BMX055_ACC_OFC_OFFSET_Y 0x39
krebyy 0:cbba28a205fa 87 #define BMX055_ACC_OFC_OFFSET_Z 0x3A
krebyy 0:cbba28a205fa 88 #define BMX055_ACC_TRIM_GPO 0x3B
krebyy 0:cbba28a205fa 89 #define BMX055_ACC_TRIM_GP1 0x3C
krebyy 0:cbba28a205fa 90 //#define BMX055_ACC_Reserved 0x3D
krebyy 0:cbba28a205fa 91 #define BMX055_ACC_FIFO_CONFIG_1 0x3E
krebyy 0:cbba28a205fa 92 #define BMX055_ACC_FIFO_DATA 0x3F
krebyy 0:cbba28a205fa 93
krebyy 0:cbba28a205fa 94 // BMX055 Gyroscope Registers
krebyy 0:cbba28a205fa 95 #define BMX055_GYRO_WHOAMI 0x00 // should return 0x0F
krebyy 0:cbba28a205fa 96 //#define BMX055_GYRO_Reserved 0x01
krebyy 0:cbba28a205fa 97 #define BMX055_GYRO_RATE_X_LSB 0x02
krebyy 0:cbba28a205fa 98 #define BMX055_GYRO_RATE_X_MSB 0x03
krebyy 0:cbba28a205fa 99 #define BMX055_GYRO_RATE_Y_LSB 0x04
krebyy 0:cbba28a205fa 100 #define BMX055_GYRO_RATE_Y_MSB 0x05
krebyy 0:cbba28a205fa 101 #define BMX055_GYRO_RATE_Z_LSB 0x06
krebyy 0:cbba28a205fa 102 #define BMX055_GYRO_RATE_Z_MSB 0x07
krebyy 0:cbba28a205fa 103 //#define BMX055_GYRO_Reserved 0x08
krebyy 0:cbba28a205fa 104 #define BMX055_GYRO_INT_STATUS_0 0x09
krebyy 0:cbba28a205fa 105 #define BMX055_GYRO_INT_STATUS_1 0x0A
krebyy 0:cbba28a205fa 106 #define BMX055_GYRO_INT_STATUS_2 0x0B
krebyy 0:cbba28a205fa 107 #define BMX055_GYRO_INT_STATUS_3 0x0C
krebyy 0:cbba28a205fa 108 //#define BMX055_GYRO_Reserved 0x0D
krebyy 0:cbba28a205fa 109 #define BMX055_GYRO_FIFO_STATUS 0x0E
krebyy 0:cbba28a205fa 110 #define BMX055_GYRO_RANGE 0x0F
krebyy 0:cbba28a205fa 111 #define BMX055_GYRO_BW 0x10
krebyy 0:cbba28a205fa 112 #define BMX055_GYRO_LPM1 0x11
krebyy 0:cbba28a205fa 113 #define BMX055_GYRO_LPM2 0x12
krebyy 0:cbba28a205fa 114 #define BMX055_GYRO_RATE_HBW 0x13
krebyy 0:cbba28a205fa 115 #define BMX055_GYRO_BGW_SOFTRESET 0x14
krebyy 0:cbba28a205fa 116 #define BMX055_GYRO_INT_EN_0 0x15
krebyy 0:cbba28a205fa 117 #define BMX055_GYRO_INT_EN_1 0x16
krebyy 0:cbba28a205fa 118 #define BMX055_GYRO_INT_MAP_0 0x17
krebyy 0:cbba28a205fa 119 #define BMX055_GYRO_INT_MAP_1 0x18
krebyy 0:cbba28a205fa 120 #define BMX055_GYRO_INT_MAP_2 0x19
krebyy 0:cbba28a205fa 121 #define BMX055_GYRO_INT_SRC_1 0x1A
krebyy 0:cbba28a205fa 122 #define BMX055_GYRO_INT_SRC_2 0x1B
krebyy 0:cbba28a205fa 123 #define BMX055_GYRO_INT_SRC_3 0x1C
krebyy 0:cbba28a205fa 124 //#define BMX055_GYRO_Reserved 0x1D
krebyy 0:cbba28a205fa 125 #define BMX055_GYRO_FIFO_EN 0x1E
krebyy 0:cbba28a205fa 126 //#define BMX055_GYRO_Reserved 0x1F
krebyy 0:cbba28a205fa 127 //#define BMX055_GYRO_Reserved 0x20
krebyy 0:cbba28a205fa 128 #define BMX055_GYRO_INT_RST_LATCH 0x21
krebyy 0:cbba28a205fa 129 #define BMX055_GYRO_HIGH_TH_X 0x22
krebyy 0:cbba28a205fa 130 #define BMX055_GYRO_HIGH_DUR_X 0x23
krebyy 0:cbba28a205fa 131 #define BMX055_GYRO_HIGH_TH_Y 0x24
krebyy 0:cbba28a205fa 132 #define BMX055_GYRO_HIGH_DUR_Y 0x25
krebyy 0:cbba28a205fa 133 #define BMX055_GYRO_HIGH_TH_Z 0x26
krebyy 0:cbba28a205fa 134 #define BMX055_GYRO_HIGH_DUR_Z 0x27
krebyy 0:cbba28a205fa 135 //#define BMX055_GYRO_Reserved 0x28
krebyy 0:cbba28a205fa 136 //#define BMX055_GYRO_Reserved 0x29
krebyy 0:cbba28a205fa 137 //#define BMX055_GYRO_Reserved 0x2A
krebyy 0:cbba28a205fa 138 #define BMX055_GYRO_SOC 0x31
krebyy 0:cbba28a205fa 139 #define BMX055_GYRO_A_FOC 0x32
krebyy 0:cbba28a205fa 140 #define BMX055_GYRO_TRIM_NVM_CTRL 0x33
krebyy 0:cbba28a205fa 141 #define BMX055_GYRO_BGW_SPI3_WDT 0x34
krebyy 0:cbba28a205fa 142 //#define BMX055_GYRO_Reserved 0x35
krebyy 0:cbba28a205fa 143 #define BMX055_GYRO_OFC1 0x36
krebyy 0:cbba28a205fa 144 #define BMX055_GYRO_OFC2 0x37
krebyy 0:cbba28a205fa 145 #define BMX055_GYRO_OFC3 0x38
krebyy 0:cbba28a205fa 146 #define BMX055_GYRO_OFC4 0x39
krebyy 0:cbba28a205fa 147 #define BMX055_GYRO_TRIM_GP0 0x3A
krebyy 0:cbba28a205fa 148 #define BMX055_GYRO_TRIM_GP1 0x3B
krebyy 0:cbba28a205fa 149 #define BMX055_GYRO_BIST 0x3C
krebyy 0:cbba28a205fa 150 #define BMX055_GYRO_FIFO_CONFIG_0 0x3D
krebyy 0:cbba28a205fa 151 #define BMX055_GYRO_FIFO_CONFIG_1 0x3E
krebyy 0:cbba28a205fa 152
krebyy 0:cbba28a205fa 153 // BMX055 magnetometer registers
krebyy 0:cbba28a205fa 154 #define BMX055_MAG_WHOAMI 0x40 // should return 0x32
krebyy 0:cbba28a205fa 155 #define BMX055_MAG_Reserved 0x41
krebyy 0:cbba28a205fa 156 #define BMX055_MAG_XOUT_LSB 0x42
krebyy 0:cbba28a205fa 157 #define BMX055_MAG_XOUT_MSB 0x43
krebyy 0:cbba28a205fa 158 #define BMX055_MAG_YOUT_LSB 0x44
krebyy 0:cbba28a205fa 159 #define BMX055_MAG_YOUT_MSB 0x45
krebyy 0:cbba28a205fa 160 #define BMX055_MAG_ZOUT_LSB 0x46
krebyy 0:cbba28a205fa 161 #define BMX055_MAG_ZOUT_MSB 0x47
krebyy 0:cbba28a205fa 162 #define BMX055_MAG_ROUT_LSB 0x48
krebyy 0:cbba28a205fa 163 #define BMX055_MAG_ROUT_MSB 0x49
krebyy 0:cbba28a205fa 164 #define BMX055_MAG_INT_STATUS 0x4A
krebyy 0:cbba28a205fa 165 #define BMX055_MAG_PWR_CNTL1 0x4B
krebyy 0:cbba28a205fa 166 #define BMX055_MAG_PWR_CNTL2 0x4C
krebyy 0:cbba28a205fa 167 #define BMX055_MAG_INT_EN_1 0x4D
krebyy 0:cbba28a205fa 168 #define BMX055_MAG_INT_EN_2 0x4E
krebyy 0:cbba28a205fa 169 #define BMX055_MAG_LOW_THS 0x4F
krebyy 0:cbba28a205fa 170 #define BMX055_MAG_HIGH_THS 0x50
krebyy 0:cbba28a205fa 171 #define BMX055_MAG_REP_XY 0x51
krebyy 0:cbba28a205fa 172 #define BMX055_MAG_REP_Z 0x52
krebyy 0:cbba28a205fa 173 /* Trim Extended Registers */
krebyy 0:cbba28a205fa 174 #define BMM050_DIG_X1 0x5D // needed for magnetic field calculation
krebyy 0:cbba28a205fa 175 #define BMM050_DIG_Y1 0x5E
krebyy 0:cbba28a205fa 176 #define BMM050_DIG_Z4_LSB 0x62
krebyy 0:cbba28a205fa 177 #define BMM050_DIG_Z4_MSB 0x63
krebyy 0:cbba28a205fa 178 #define BMM050_DIG_X2 0x64
krebyy 0:cbba28a205fa 179 #define BMM050_DIG_Y2 0x65
krebyy 0:cbba28a205fa 180 #define BMM050_DIG_Z2_LSB 0x68
krebyy 0:cbba28a205fa 181 #define BMM050_DIG_Z2_MSB 0x69
krebyy 0:cbba28a205fa 182 #define BMM050_DIG_Z1_LSB 0x6A
krebyy 0:cbba28a205fa 183 #define BMM050_DIG_Z1_MSB 0x6B
krebyy 0:cbba28a205fa 184 #define BMM050_DIG_XYZ1_LSB 0x6C
krebyy 0:cbba28a205fa 185 #define BMM050_DIG_XYZ1_MSB 0x6D
krebyy 0:cbba28a205fa 186 #define BMM050_DIG_Z3_LSB 0x6E
krebyy 0:cbba28a205fa 187 #define BMM050_DIG_Z3_MSB 0x6F
krebyy 0:cbba28a205fa 188 #define BMM050_DIG_XY2 0x70
krebyy 0:cbba28a205fa 189 #define BMM050_DIG_XY1 0x71
krebyy 0:cbba28a205fa 190
krebyy 0:cbba28a205fa 191 // Using SDO1 = SDO2 = CSB3 = GND as designed
krebyy 0:cbba28a205fa 192 // Seven-bit device addresses are ACC = 0x18, GYRO = 0x68, MAG = 0x10
krebyy 0:cbba28a205fa 193 #define BMX055_ACC_ADDRESS 0x18 << 1 // Address of BMX055 accelerometer
krebyy 0:cbba28a205fa 194 #define BMX055_GYRO_ADDRESS 0x68 << 1 // Address of BMX055 gyroscope
krebyy 0:cbba28a205fa 195 #define BMX055_MAG_ADDRESS 0x10 << 1 // Address of BMX055 magnetometer
krebyy 0:cbba28a205fa 196
krebyy 0:cbba28a205fa 197 // Set initial input parameters
krebyy 0:cbba28a205fa 198 // define BMX055 ACC full scale options
krebyy 0:cbba28a205fa 199 #define AFS_2G 0x03
krebyy 0:cbba28a205fa 200 #define AFS_4G 0x05
krebyy 0:cbba28a205fa 201 #define AFS_8G 0x08
krebyy 0:cbba28a205fa 202 #define AFS_16G 0x0C
krebyy 0:cbba28a205fa 203
krebyy 0:cbba28a205fa 204 enum ACCBW { // define BMX055 accelerometer bandwidths
krebyy 0:cbba28a205fa 205 ABW_8Hz, // 7.81 Hz, 64 ms update time
krebyy 0:cbba28a205fa 206 ABW_16Hz, // 15.63 Hz, 32 ms update time
krebyy 0:cbba28a205fa 207 ABW_31Hz, // 31.25 Hz, 16 ms update time
krebyy 0:cbba28a205fa 208 ABW_63Hz, // 62.5 Hz, 8 ms update time
krebyy 0:cbba28a205fa 209 ABW_125Hz, // 125 Hz, 4 ms update time
krebyy 0:cbba28a205fa 210 ABW_250Hz, // 250 Hz, 2 ms update time
krebyy 0:cbba28a205fa 211 ABW_500Hz, // 500 Hz, 1 ms update time
krebyy 0:cbba28a205fa 212 ABW_100Hz // 1000 Hz, 0.5 ms update time
krebyy 0:cbba28a205fa 213 };
krebyy 0:cbba28a205fa 214
krebyy 0:cbba28a205fa 215 enum Gscale {
krebyy 0:cbba28a205fa 216 GFS_2000DPS = 0,
krebyy 0:cbba28a205fa 217 GFS_1000DPS,
krebyy 0:cbba28a205fa 218 GFS_500DPS,
krebyy 0:cbba28a205fa 219 GFS_250DPS,
krebyy 0:cbba28a205fa 220 GFS_125DPS
krebyy 0:cbba28a205fa 221 };
krebyy 0:cbba28a205fa 222
krebyy 0:cbba28a205fa 223 enum GODRBW {
krebyy 0:cbba28a205fa 224 G_2000Hz523Hz = 0, // 2000 Hz ODR and unfiltered (bandwidth 523Hz)
krebyy 0:cbba28a205fa 225 G_2000Hz230Hz,
krebyy 0:cbba28a205fa 226 G_1000Hz116Hz,
krebyy 0:cbba28a205fa 227 G_400Hz47Hz,
krebyy 0:cbba28a205fa 228 G_200Hz23Hz,
krebyy 0:cbba28a205fa 229 G_100Hz12Hz,
krebyy 0:cbba28a205fa 230 G_200Hz64Hz,
krebyy 0:cbba28a205fa 231 G_100Hz32Hz // 100 Hz ODR and 32 Hz bandwidth
krebyy 0:cbba28a205fa 232 };
krebyy 0:cbba28a205fa 233
krebyy 0:cbba28a205fa 234 enum MODR {
krebyy 0:cbba28a205fa 235 MODR_10Hz = 0, // 10 Hz ODR
krebyy 0:cbba28a205fa 236 MODR_2Hz , // 2 Hz ODR
krebyy 0:cbba28a205fa 237 MODR_6Hz , // 6 Hz ODR
krebyy 0:cbba28a205fa 238 MODR_8Hz , // 8 Hz ODR
krebyy 0:cbba28a205fa 239 MODR_15Hz , // 15 Hz ODR
krebyy 0:cbba28a205fa 240 MODR_20Hz , // 20 Hz ODR
krebyy 0:cbba28a205fa 241 MODR_25Hz , // 25 Hz ODR
krebyy 0:cbba28a205fa 242 MODR_30Hz // 30 Hz ODR
krebyy 0:cbba28a205fa 243 };
krebyy 0:cbba28a205fa 244
krebyy 0:cbba28a205fa 245 enum Mmode {
krebyy 0:cbba28a205fa 246 lowPower = 0, // rms noise ~1.0 microTesla, 0.17 mA power
krebyy 0:cbba28a205fa 247 Regular , // rms noise ~0.6 microTesla, 0.5 mA power
krebyy 0:cbba28a205fa 248 enhancedRegular , // rms noise ~0.5 microTesla, 0.8 mA power
krebyy 0:cbba28a205fa 249 highAccuracy // rms noise ~0.3 microTesla, 4.9 mA power
krebyy 0:cbba28a205fa 250 };
krebyy 0:cbba28a205fa 251
krebyy 0:cbba28a205fa 252 // Set up I2C, (SDA,SCL)
krebyy 0:cbba28a205fa 253 I2C i2c2(PB_11, PB_10);
krebyy 0:cbba28a205fa 254
krebyy 0:cbba28a205fa 255 // Specify sensor full scale
krebyy 0:cbba28a205fa 256 uint8_t Ascale = AFS_2G;
krebyy 0:cbba28a205fa 257 uint8_t Gscale = GFS_125DPS;
krebyy 0:cbba28a205fa 258 float aRes, gRes, mRes; // scale resolutions per LSB for the sensors
krebyy 0:cbba28a205fa 259
krebyy 0:cbba28a205fa 260 // Parameters to hold BMX055 trim values
krebyy 0:cbba28a205fa 261 int8_t dig_x1;
krebyy 0:cbba28a205fa 262 int8_t dig_y1;
krebyy 0:cbba28a205fa 263 int8_t dig_x2;
krebyy 0:cbba28a205fa 264 int8_t dig_y2;
krebyy 0:cbba28a205fa 265 uint16_t dig_z1;
krebyy 0:cbba28a205fa 266 int16_t dig_z2;
krebyy 0:cbba28a205fa 267 int16_t dig_z3;
krebyy 0:cbba28a205fa 268 int16_t dig_z4;
krebyy 0:cbba28a205fa 269 uint8_t dig_xy1;
krebyy 0:cbba28a205fa 270 int8_t dig_xy2;
krebyy 0:cbba28a205fa 271 uint16_t dig_xyz1;
krebyy 0:cbba28a205fa 272
krebyy 0:cbba28a205fa 273 // BMX055 variables
krebyy 0:cbba28a205fa 274 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
krebyy 0:cbba28a205fa 275 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
krebyy 0:cbba28a205fa 276 int16_t magCount[3]; // Stores the 13/15-bit signed magnetometer sensor output
krebyy 0:cbba28a205fa 277 float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}, magBias[3] = {0, 0, 0}; // Bias corrections for gyro, accelerometer, mag
krebyy 0:cbba28a205fa 278 float SelfTest[6]; // holds results of gyro and accelerometer self test
krebyy 0:cbba28a205fa 279
krebyy 0:cbba28a205fa 280 // global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
krebyy 0:cbba28a205fa 281 float GyroMeasError = M_PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)
krebyy 0:cbba28a205fa 282 float GyroMeasDrift = M_PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
krebyy 0:cbba28a205fa 283 // There is a tradeoff in the beta parameter between accuracy and response speed.
krebyy 0:cbba28a205fa 284 // In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
krebyy 0:cbba28a205fa 285 // However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
krebyy 0:cbba28a205fa 286 // Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
krebyy 0:cbba28a205fa 287 // By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
krebyy 0:cbba28a205fa 288 // I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
krebyy 0:cbba28a205fa 289 // the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
krebyy 0:cbba28a205fa 290 // In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
krebyy 0:cbba28a205fa 291 float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
krebyy 0:cbba28a205fa 292 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
krebyy 0:cbba28a205fa 293 #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
krebyy 0:cbba28a205fa 294 #define Ki 0.0f
krebyy 0:cbba28a205fa 295
krebyy 0:cbba28a205fa 296 // Declination at Sao Paulo, Brazil is -21 degrees 7 minutes on 2016-03-27
krebyy 0:cbba28a205fa 297 #define LOCAL_DECLINATION -21.1f
krebyy 0:cbba28a205fa 298
krebyy 0:cbba28a205fa 299 float deltat = 0.0f; // integration interval for both filter schemes
krebyy 0:cbba28a205fa 300
krebyy 0:cbba28a205fa 301 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
krebyy 0:cbba28a205fa 302 float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method
krebyy 0:cbba28a205fa 303
krebyy 0:cbba28a205fa 304
krebyy 0:cbba28a205fa 305 class BMX055 {
krebyy 0:cbba28a205fa 306 private:
krebyy 0:cbba28a205fa 307 float pitch, yaw, roll;
krebyy 0:cbba28a205fa 308
krebyy 0:cbba28a205fa 309 void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
krebyy 0:cbba28a205fa 310 {
krebyy 0:cbba28a205fa 311 char data_write[2];
krebyy 0:cbba28a205fa 312 data_write[0] = subAddress;
krebyy 0:cbba28a205fa 313 data_write[1] = data;
krebyy 0:cbba28a205fa 314 i2c2.write(address, data_write, 2, 0);
krebyy 0:cbba28a205fa 315 }
krebyy 0:cbba28a205fa 316
krebyy 0:cbba28a205fa 317 char readByte(uint8_t address, uint8_t subAddress)
krebyy 0:cbba28a205fa 318 {
krebyy 0:cbba28a205fa 319 char data[1]; // `data` will store the register data
krebyy 0:cbba28a205fa 320 char data_write[1];
krebyy 0:cbba28a205fa 321 data_write[0] = subAddress;
krebyy 0:cbba28a205fa 322 i2c2.write(address, data_write, 1, 1); // no stop
krebyy 0:cbba28a205fa 323 i2c2.read(address, data, 1, 0);
krebyy 0:cbba28a205fa 324 return data[0];
krebyy 0:cbba28a205fa 325 }
krebyy 0:cbba28a205fa 326
krebyy 0:cbba28a205fa 327 void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
krebyy 0:cbba28a205fa 328 {
krebyy 0:cbba28a205fa 329 char data[14];
krebyy 0:cbba28a205fa 330 char data_write[1];
krebyy 0:cbba28a205fa 331 data_write[0] = subAddress;
krebyy 0:cbba28a205fa 332 i2c2.write(address, data_write, 1, 1); // no stop
krebyy 0:cbba28a205fa 333 i2c2.read(address, data, count, 0);
krebyy 0:cbba28a205fa 334 for(int ii = 0; ii < count; ii++)
krebyy 0:cbba28a205fa 335 {
krebyy 0:cbba28a205fa 336 dest[ii] = data[ii];
krebyy 0:cbba28a205fa 337 }
krebyy 0:cbba28a205fa 338 }
krebyy 0:cbba28a205fa 339
krebyy 0:cbba28a205fa 340 public:
krebyy 0:cbba28a205fa 341 BMX055()
krebyy 0:cbba28a205fa 342 {
krebyy 0:cbba28a205fa 343 //Set up I2C
krebyy 0:cbba28a205fa 344 i2c2.frequency(400000); // use fast (400 kHz) I2C
krebyy 0:cbba28a205fa 345 }
krebyy 0:cbba28a205fa 346
krebyy 0:cbba28a205fa 347 float getAres(void)
krebyy 0:cbba28a205fa 348 {
krebyy 0:cbba28a205fa 349 switch (Ascale)
krebyy 0:cbba28a205fa 350 {
krebyy 0:cbba28a205fa 351 // Possible accelerometer scales (and their register bit settings) are:
krebyy 0:cbba28a205fa 352 // 2 Gs (0011), 4 Gs (0101), 8 Gs (1000), and 16 Gs (1100).
krebyy 0:cbba28a205fa 353 // BMX055 ACC data is signed 12 bit
krebyy 0:cbba28a205fa 354 case AFS_2G:
krebyy 0:cbba28a205fa 355 aRes = 2.0/2048.0;
krebyy 0:cbba28a205fa 356 break;
krebyy 0:cbba28a205fa 357 case AFS_4G:
krebyy 0:cbba28a205fa 358 aRes = 4.0/2048.0;
krebyy 0:cbba28a205fa 359 break;
krebyy 0:cbba28a205fa 360 case AFS_8G:
krebyy 0:cbba28a205fa 361 aRes = 8.0/2048.0;
krebyy 0:cbba28a205fa 362 break;
krebyy 0:cbba28a205fa 363 case AFS_16G:
krebyy 0:cbba28a205fa 364 aRes = 16.0/2048.0;
krebyy 0:cbba28a205fa 365 break;
krebyy 0:cbba28a205fa 366 }
krebyy 0:cbba28a205fa 367
krebyy 0:cbba28a205fa 368 return aRes;
krebyy 0:cbba28a205fa 369 }
krebyy 0:cbba28a205fa 370
krebyy 0:cbba28a205fa 371 float getGres(void)
krebyy 0:cbba28a205fa 372 {
krebyy 0:cbba28a205fa 373 switch (Gscale)
krebyy 0:cbba28a205fa 374 {
krebyy 0:cbba28a205fa 375 // Possible gyro scales (and their register bit settings) are:
krebyy 0:cbba28a205fa 376 // 125 DPS (100), 250 DPS (011), 500 DPS (010), 1000 DPS (001), and 2000 DPS (000).
krebyy 0:cbba28a205fa 377 case GFS_125DPS:
krebyy 0:cbba28a205fa 378 gRes = 124.87/32768.0; // per data sheet, not exactly 125!?
krebyy 0:cbba28a205fa 379 break;
krebyy 0:cbba28a205fa 380 case GFS_250DPS:
krebyy 0:cbba28a205fa 381 gRes = 249.75/32768.0;
krebyy 0:cbba28a205fa 382 break;
krebyy 0:cbba28a205fa 383 case GFS_500DPS:
krebyy 0:cbba28a205fa 384 gRes = 499.5/32768.0;
krebyy 0:cbba28a205fa 385 break;
krebyy 0:cbba28a205fa 386 case GFS_1000DPS:
krebyy 0:cbba28a205fa 387 gRes = 999.0/32768.0;
krebyy 0:cbba28a205fa 388 break;
krebyy 0:cbba28a205fa 389 case GFS_2000DPS:
krebyy 0:cbba28a205fa 390 gRes = 1998.0/32768.0;
krebyy 0:cbba28a205fa 391 break;
krebyy 0:cbba28a205fa 392 }
krebyy 0:cbba28a205fa 393
krebyy 0:cbba28a205fa 394 return gRes;
krebyy 0:cbba28a205fa 395 }
krebyy 0:cbba28a205fa 396
krebyy 0:cbba28a205fa 397 float getPitch(void)
krebyy 0:cbba28a205fa 398 {
krebyy 0:cbba28a205fa 399 return pitch;
krebyy 0:cbba28a205fa 400 }
krebyy 0:cbba28a205fa 401
krebyy 0:cbba28a205fa 402 float getRoll(void)
krebyy 0:cbba28a205fa 403 {
krebyy 0:cbba28a205fa 404 return roll;
krebyy 0:cbba28a205fa 405 }
krebyy 0:cbba28a205fa 406
krebyy 0:cbba28a205fa 407 float getYaw(void)
krebyy 0:cbba28a205fa 408 {
krebyy 0:cbba28a205fa 409 return yaw;
krebyy 0:cbba28a205fa 410 }
krebyy 0:cbba28a205fa 411
krebyy 0:cbba28a205fa 412 void readAccelData(int16_t * destination)
krebyy 0:cbba28a205fa 413 {
krebyy 0:cbba28a205fa 414 uint8_t rawData[6]; // x/y/z accel register data stored here
krebyy 0:cbba28a205fa 415
krebyy 0:cbba28a205fa 416 // Read the six raw data registers into data array
krebyy 0:cbba28a205fa 417 readBytes(BMX055_ACC_ADDRESS, BMX055_ACC_D_X_LSB, 6, &rawData[0]);
krebyy 0:cbba28a205fa 418 if((rawData[0] & 0x01) && (rawData[2] & 0x01) && (rawData[4] & 0x01))
krebyy 0:cbba28a205fa 419 { // Check that all 3 axes have new data
krebyy 0:cbba28a205fa 420 // Turn the MSB and LSB into a signed 12-bit value
krebyy 0:cbba28a205fa 421 destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]) >> 4;
krebyy 0:cbba28a205fa 422 destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]) >> 4;
krebyy 0:cbba28a205fa 423 destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]) >> 4;
krebyy 0:cbba28a205fa 424 }
krebyy 0:cbba28a205fa 425 }
krebyy 0:cbba28a205fa 426
krebyy 0:cbba28a205fa 427 void readGyroData(int16_t * destination)
krebyy 0:cbba28a205fa 428 {
krebyy 0:cbba28a205fa 429 uint8_t rawData[6]; // x/y/z gyro register data stored here
krebyy 0:cbba28a205fa 430
krebyy 0:cbba28a205fa 431 // Read the six raw data registers sequentially into data array
krebyy 0:cbba28a205fa 432 readBytes(BMX055_GYRO_ADDRESS, BMX055_GYRO_RATE_X_LSB, 6, &rawData[0]);
krebyy 0:cbba28a205fa 433 // Turn the MSB and LSB into a signed 16-bit value
krebyy 0:cbba28a205fa 434 destination[0] = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 435 destination[1] = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]);
krebyy 0:cbba28a205fa 436 destination[2] = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]);
krebyy 0:cbba28a205fa 437 }
krebyy 0:cbba28a205fa 438
krebyy 0:cbba28a205fa 439 void readMagData(int16_t * magData)
krebyy 0:cbba28a205fa 440 {
krebyy 0:cbba28a205fa 441 int16_t mdata_x = 0, mdata_y = 0, mdata_z = 0, temp = 0;
krebyy 0:cbba28a205fa 442 uint16_t data_r = 0;
krebyy 0:cbba28a205fa 443 uint8_t rawData[8]; // x/y/z hall magnetic field data, and Hall resistance data
krebyy 0:cbba28a205fa 444 readBytes(BMX055_MAG_ADDRESS, BMX055_MAG_XOUT_LSB, 8, &rawData[0]); // Read the eight raw data registers sequentially into data array
krebyy 0:cbba28a205fa 445 if(rawData[6] & 0x01) { // Check if data ready status bit is set
krebyy 0:cbba28a205fa 446 mdata_x = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]) >> 3; // 13-bit signed integer for x-axis field
krebyy 0:cbba28a205fa 447 mdata_y = (int16_t) (((int16_t)rawData[3] << 8) | rawData[2]) >> 3; // 13-bit signed integer for y-axis field
krebyy 0:cbba28a205fa 448 mdata_z = (int16_t) (((int16_t)rawData[5] << 8) | rawData[4]) >> 1; // 15-bit signed integer for z-axis field
krebyy 0:cbba28a205fa 449 data_r = (uint16_t) (((uint16_t)rawData[7] << 8) | rawData[6]) >> 2; // 14-bit unsigned integer for Hall resistance
krebyy 0:cbba28a205fa 450
krebyy 0:cbba28a205fa 451 // calculate temperature compensated 16-bit magnetic fields
krebyy 0:cbba28a205fa 452 temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
krebyy 0:cbba28a205fa 453 magData[0] = ((int16_t)((((int32_t)mdata_x) *
krebyy 0:cbba28a205fa 454 ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
krebyy 0:cbba28a205fa 455 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
krebyy 0:cbba28a205fa 456 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_x2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
krebyy 0:cbba28a205fa 457 (((int16_t)dig_x1) << 3);
krebyy 0:cbba28a205fa 458
krebyy 0:cbba28a205fa 459 temp = ((int16_t)(((uint16_t)((((int32_t)dig_xyz1) << 14)/(data_r != 0 ? data_r : dig_xyz1))) - ((uint16_t)0x4000)));
krebyy 0:cbba28a205fa 460 magData[1] = ((int16_t)((((int32_t)mdata_y) *
krebyy 0:cbba28a205fa 461 ((((((((int32_t)dig_xy2) * ((((int32_t)temp) * ((int32_t)temp)) >> 7)) +
krebyy 0:cbba28a205fa 462 (((int32_t)temp) * ((int32_t)(((int16_t)dig_xy1) << 7)))) >> 9) +
krebyy 0:cbba28a205fa 463 ((int32_t)0x100000)) * ((int32_t)(((int16_t)dig_y2) + ((int16_t)0xA0)))) >> 12)) >> 13)) +
krebyy 0:cbba28a205fa 464 (((int16_t)dig_y1) << 3);
krebyy 0:cbba28a205fa 465 magData[2] = (((((int32_t)(mdata_z - dig_z4)) << 15) - ((((int32_t)dig_z3) * ((int32_t)(((int16_t)data_r) -
krebyy 0:cbba28a205fa 466 ((int16_t)dig_xyz1))))>>2))/(dig_z2 + ((int16_t)(((((int32_t)dig_z1) * ((((int16_t)data_r) << 1)))+(1<<15))>>16))));
krebyy 0:cbba28a205fa 467 }
krebyy 0:cbba28a205fa 468 }
krebyy 0:cbba28a205fa 469
krebyy 0:cbba28a205fa 470 float getTemperature()
krebyy 0:cbba28a205fa 471 {
krebyy 0:cbba28a205fa 472 uint8_t c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_D_TEMP); // Read the raw data register
krebyy 0:cbba28a205fa 473 int16_t tempCount = ((int16_t)((int16_t)c << 8)) >> 8 ; // Turn the byte into a signed 8-bit integer
krebyy 0:cbba28a205fa 474
krebyy 2:b3c3bf0b9101 475 return ((((float)tempCount) * 0.5f) + 23.0f); // temperature in degrees Centigrade
krebyy 0:cbba28a205fa 476 }
krebyy 0:cbba28a205fa 477
krebyy 0:cbba28a205fa 478 void fastcompaccelBMX055(float * dest1)
krebyy 0:cbba28a205fa 479 {
krebyy 0:cbba28a205fa 480 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x80); // set all accel offset compensation registers to zero
krebyy 0:cbba28a205fa 481 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_SETTING, 0x20); // set offset targets to 0, 0, and +1 g for x, y, z axes
krebyy 0:cbba28a205fa 482 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x20); // calculate x-axis offset
krebyy 0:cbba28a205fa 483
krebyy 0:cbba28a205fa 484 uint8_t c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL);
krebyy 0:cbba28a205fa 485 while(!(c & 0x10))
krebyy 0:cbba28a205fa 486 { // check if fast calibration complete
krebyy 0:cbba28a205fa 487 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL);
krebyy 0:cbba28a205fa 488 wait_ms(10);
krebyy 0:cbba28a205fa 489 }
krebyy 0:cbba28a205fa 490 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x40); // calculate y-axis offset
krebyy 0:cbba28a205fa 491
krebyy 0:cbba28a205fa 492 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL);
krebyy 0:cbba28a205fa 493 while(!(c & 0x10))
krebyy 0:cbba28a205fa 494 { // check if fast calibration complete
krebyy 0:cbba28a205fa 495 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL);
krebyy 0:cbba28a205fa 496 wait_ms(10);
krebyy 0:cbba28a205fa 497 }
krebyy 0:cbba28a205fa 498 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL, 0x60); // calculate z-axis offset
krebyy 0:cbba28a205fa 499
krebyy 0:cbba28a205fa 500 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL);
krebyy 0:cbba28a205fa 501 while(!(c & 0x10))
krebyy 0:cbba28a205fa 502 { // check if fast calibration complete
krebyy 0:cbba28a205fa 503 c = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_CTRL);
krebyy 0:cbba28a205fa 504 wait_ms(10);
krebyy 0:cbba28a205fa 505 }
krebyy 0:cbba28a205fa 506
krebyy 0:cbba28a205fa 507 int8_t compx = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_OFFSET_X);
krebyy 0:cbba28a205fa 508 int8_t compy = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_OFFSET_Y);
krebyy 0:cbba28a205fa 509 int8_t compz = readByte(BMX055_ACC_ADDRESS, BMX055_ACC_OFC_OFFSET_Z);
krebyy 0:cbba28a205fa 510
krebyy 2:b3c3bf0b9101 511 dest1[0] = (float) compx/128.0f; // accleration bias in g
krebyy 2:b3c3bf0b9101 512 dest1[1] = (float) compy/128.0f; // accleration bias in g
krebyy 2:b3c3bf0b9101 513 dest1[2] = (float) compz/128.0f; // accleration bias in g
krebyy 0:cbba28a205fa 514 }
krebyy 0:cbba28a205fa 515
krebyy 0:cbba28a205fa 516 void magcalBMX055(float * dest1)
krebyy 0:cbba28a205fa 517 {
krebyy 0:cbba28a205fa 518 uint16_t ii = 0, sample_count = 0;
krebyy 0:cbba28a205fa 519 int32_t mag_bias[3] = {0, 0, 0};
krebyy 0:cbba28a205fa 520 int16_t mag_max[3] = {0, 0, 0}, mag_min[3] = {0, 0, 0};
krebyy 0:cbba28a205fa 521
krebyy 0:cbba28a205fa 522 // ## "Mag Calibration: Wave device in a figure eight until done!" ##
krebyy 0:cbba28a205fa 523 //wait(4);
krebyy 0:cbba28a205fa 524
krebyy 0:cbba28a205fa 525 sample_count = 48;
krebyy 0:cbba28a205fa 526 for(ii = 0; ii < sample_count; ii++)
krebyy 0:cbba28a205fa 527 {
krebyy 0:cbba28a205fa 528 int16_t mag_temp[3] = {0, 0, 0};
krebyy 0:cbba28a205fa 529 readMagData(mag_temp);
krebyy 0:cbba28a205fa 530 for (int jj = 0; jj < 3; jj++)
krebyy 0:cbba28a205fa 531 {
krebyy 0:cbba28a205fa 532 if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
krebyy 0:cbba28a205fa 533 if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
krebyy 0:cbba28a205fa 534 }
krebyy 0:cbba28a205fa 535 wait_ms(105); // at 10 Hz ODR, new mag data is available every 100 ms
krebyy 0:cbba28a205fa 536 }
krebyy 0:cbba28a205fa 537
krebyy 0:cbba28a205fa 538 mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
krebyy 0:cbba28a205fa 539 mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
krebyy 0:cbba28a205fa 540 mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
krebyy 0:cbba28a205fa 541
krebyy 0:cbba28a205fa 542 // save mag biases in G for main program
krebyy 0:cbba28a205fa 543 dest1[0] = (float) mag_bias[0]*mRes;
krebyy 0:cbba28a205fa 544 dest1[1] = (float) mag_bias[1]*mRes;
krebyy 0:cbba28a205fa 545 dest1[2] = (float) mag_bias[2]*mRes;
krebyy 0:cbba28a205fa 546
krebyy 0:cbba28a205fa 547 // ## "Mag Calibration done!" ##
krebyy 0:cbba28a205fa 548 }
krebyy 0:cbba28a205fa 549
krebyy 0:cbba28a205fa 550 // Get trim values for magnetometer sensitivity
krebyy 0:cbba28a205fa 551 void trim(void)
krebyy 0:cbba28a205fa 552 {
krebyy 0:cbba28a205fa 553 uint8_t rawData[2]; //placeholder for 2-byte trim data
krebyy 0:cbba28a205fa 554
krebyy 0:cbba28a205fa 555 dig_x1 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_X1);
krebyy 0:cbba28a205fa 556 dig_x2 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_X2);
krebyy 0:cbba28a205fa 557 dig_y1 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_Y1);
krebyy 0:cbba28a205fa 558 dig_y2 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_Y2);
krebyy 0:cbba28a205fa 559 dig_xy1 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_XY1);
krebyy 0:cbba28a205fa 560 dig_xy2 = readByte(BMX055_ACC_ADDRESS, BMM050_DIG_XY2);
krebyy 0:cbba28a205fa 561 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z1_LSB, 2, &rawData[0]);
krebyy 0:cbba28a205fa 562 dig_z1 = (uint16_t) (((uint16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 563 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z2_LSB, 2, &rawData[0]);
krebyy 0:cbba28a205fa 564 dig_z2 = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 565 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z3_LSB, 2, &rawData[0]);
krebyy 0:cbba28a205fa 566 dig_z3 = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 567 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_Z4_LSB, 2, &rawData[0]);
krebyy 0:cbba28a205fa 568 dig_z4 = (int16_t) (((int16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 569 readBytes(BMX055_MAG_ADDRESS, BMM050_DIG_XYZ1_LSB, 2, &rawData[0]);
krebyy 0:cbba28a205fa 570 dig_xyz1 = (uint16_t) (((uint16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 571 }
krebyy 0:cbba28a205fa 572
krebyy 0:cbba28a205fa 573 /** Initialize device for active mode
krebyy 0:cbba28a205fa 574 * @param Ascale set accel full scale
krebyy 0:cbba28a205fa 575 * @param ACCBW set bandwidth for accelerometer
krebyy 0:cbba28a205fa 576 * @param Gscale set gyro full scale
krebyy 0:cbba28a205fa 577 * @param GODRBW set gyro ODR and bandwidth
krebyy 0:cbba28a205fa 578 * @param Mmode set magnetometer operation mode
krebyy 0:cbba28a205fa 579 * @param MODR set magnetometer data rate
krebyy 0:cbba28a205fa 580 * @see ACCBW, GODRBW and MODR enums
krebyy 0:cbba28a205fa 581 */
krebyy 0:cbba28a205fa 582 void init(uint8_t mAscale = AFS_2G, uint8_t ACCBW = ABW_16Hz,
krebyy 0:cbba28a205fa 583 uint8_t mGscale = GFS_125DPS, uint8_t GODRBW = G_200Hz23Hz,
krebyy 0:cbba28a205fa 584 uint8_t Mmode = Regular, uint8_t MODR = MODR_10Hz)
krebyy 0:cbba28a205fa 585 {
krebyy 0:cbba28a205fa 586 Ascale = mAscale;
krebyy 0:cbba28a205fa 587 Gscale = mGscale;
krebyy 0:cbba28a205fa 588
krebyy 0:cbba28a205fa 589 // Configure accelerometer
krebyy 0:cbba28a205fa 590 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_PMU_RANGE, Ascale & 0x0F); // Set accelerometer full scale
krebyy 0:cbba28a205fa 591 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_PMU_BW, (0x08 | ACCBW) & 0x0F); // Set accelerometer bandwidth
krebyy 0:cbba28a205fa 592 writeByte(BMX055_ACC_ADDRESS, BMX055_ACC_D_HBW, 0x00); // Use filtered data
krebyy 0:cbba28a205fa 593
krebyy 0:cbba28a205fa 594 // Configure Gyro
krebyy 0:cbba28a205fa 595 writeByte(BMX055_GYRO_ADDRESS, BMX055_GYRO_RANGE, Gscale); // set GYRO FS range
krebyy 0:cbba28a205fa 596 writeByte(BMX055_GYRO_ADDRESS, BMX055_GYRO_BW, GODRBW); // set GYRO ODR and Bandwidth
krebyy 0:cbba28a205fa 597
krebyy 0:cbba28a205fa 598 // Configure magnetometer
krebyy 0:cbba28a205fa 599 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_PWR_CNTL1, 0x82); // Softreset magnetometer, ends up in sleep mode
krebyy 0:cbba28a205fa 600 wait_ms(100);
krebyy 0:cbba28a205fa 601 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_PWR_CNTL1, 0x01); // Wake up magnetometer
krebyy 0:cbba28a205fa 602 wait_ms(100);
krebyy 0:cbba28a205fa 603 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_PWR_CNTL2, MODR << 3); // Normal mode
krebyy 0:cbba28a205fa 604
krebyy 0:cbba28a205fa 605 // Set up four standard configurations for the magnetometer
krebyy 0:cbba28a205fa 606 switch (Mmode)
krebyy 0:cbba28a205fa 607 {
krebyy 0:cbba28a205fa 608 case lowPower:
krebyy 0:cbba28a205fa 609 // Low-power
krebyy 0:cbba28a205fa 610 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x01); // 3 repetitions (oversampling)
krebyy 0:cbba28a205fa 611 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x02); // 3 repetitions (oversampling)
krebyy 0:cbba28a205fa 612 break;
krebyy 0:cbba28a205fa 613 case Regular:
krebyy 0:cbba28a205fa 614 // Regular
krebyy 0:cbba28a205fa 615 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x04); // 9 repetitions (oversampling)
krebyy 0:cbba28a205fa 616 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x16); // 15 repetitions (oversampling)
krebyy 0:cbba28a205fa 617 break;
krebyy 0:cbba28a205fa 618 case enhancedRegular:
krebyy 0:cbba28a205fa 619 // Enhanced Regular
krebyy 0:cbba28a205fa 620 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x07); // 15 repetitions (oversampling)
krebyy 0:cbba28a205fa 621 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x22); // 27 repetitions (oversampling)
krebyy 0:cbba28a205fa 622 break;
krebyy 0:cbba28a205fa 623 case highAccuracy:
krebyy 0:cbba28a205fa 624 // High Accuracy
krebyy 0:cbba28a205fa 625 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_XY, 0x17); // 47 repetitions (oversampling)
krebyy 0:cbba28a205fa 626 writeByte(BMX055_MAG_ADDRESS, BMX055_MAG_REP_Z, 0x51); // 83 repetitions (oversampling)
krebyy 0:cbba28a205fa 627 break;
krebyy 0:cbba28a205fa 628 }
krebyy 0:cbba28a205fa 629
krebyy 0:cbba28a205fa 630 // get sensor resolutions, only need to do this once
krebyy 0:cbba28a205fa 631 getAres();
krebyy 0:cbba28a205fa 632 getGres();
krebyy 0:cbba28a205fa 633 // magnetometer resolution is 1 microTesla/16 counts or 1/1.6 milliGauss/count
krebyy 0:cbba28a205fa 634 mRes = 1./1.6;
krebyy 0:cbba28a205fa 635
krebyy 0:cbba28a205fa 636 trim(); // read the magnetometer calibration data
krebyy 0:cbba28a205fa 637
krebyy 0:cbba28a205fa 638 fastcompaccelBMX055(accelBias);
krebyy 0:cbba28a205fa 639 magcalBMX055(magBias);
krebyy 0:cbba28a205fa 640 // TODO: see magcalBMX055(): 128 samples * 105ms = 13.44s
krebyy 0:cbba28a205fa 641 // So far, magnetometer bias is calculated and subtracted here manually, should construct an algorithm to do it automatically
krebyy 0:cbba28a205fa 642 // like the gyro and accelerometer biases
krebyy 0:cbba28a205fa 643 // magBias[0] = -5.; // User environmental x-axis correction in milliGauss
krebyy 0:cbba28a205fa 644 // magBias[1] = -95.; // User environmental y-axis correction in milliGauss
krebyy 0:cbba28a205fa 645 // magBias[2] = -260.; // User environmental z-axis correction in milliGauss
krebyy 0:cbba28a205fa 646
krebyy 0:cbba28a205fa 647 }
krebyy 0:cbba28a205fa 648
krebyy 0:cbba28a205fa 649 // Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
krebyy 0:cbba28a205fa 650 // (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
krebyy 0:cbba28a205fa 651 // which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
krebyy 0:cbba28a205fa 652 // device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
krebyy 0:cbba28a205fa 653 // The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
krebyy 0:cbba28a205fa 654 // but is much less computationally intensive
krebyy 0:cbba28a205fa 655 void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
krebyy 0:cbba28a205fa 656 {
krebyy 0:cbba28a205fa 657 float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
krebyy 0:cbba28a205fa 658 float norm;
krebyy 0:cbba28a205fa 659 float hx, hy, _2bx, _2bz;
krebyy 0:cbba28a205fa 660 float s1, s2, s3, s4;
krebyy 0:cbba28a205fa 661 float qDot1, qDot2, qDot3, qDot4;
krebyy 0:cbba28a205fa 662
krebyy 0:cbba28a205fa 663 // Auxiliary variables to avoid repeated arithmetic
krebyy 0:cbba28a205fa 664 float _2q1mx;
krebyy 0:cbba28a205fa 665 float _2q1my;
krebyy 0:cbba28a205fa 666 float _2q1mz;
krebyy 0:cbba28a205fa 667 float _2q2mx;
krebyy 0:cbba28a205fa 668 float _4bx;
krebyy 0:cbba28a205fa 669 float _4bz;
krebyy 0:cbba28a205fa 670 float _2q1 = 2.0f * q1;
krebyy 0:cbba28a205fa 671 float _2q2 = 2.0f * q2;
krebyy 0:cbba28a205fa 672 float _2q3 = 2.0f * q3;
krebyy 0:cbba28a205fa 673 float _2q4 = 2.0f * q4;
krebyy 0:cbba28a205fa 674 float _2q1q3 = 2.0f * q1 * q3;
krebyy 0:cbba28a205fa 675 float _2q3q4 = 2.0f * q3 * q4;
krebyy 0:cbba28a205fa 676 float q1q1 = q1 * q1;
krebyy 0:cbba28a205fa 677 float q1q2 = q1 * q2;
krebyy 0:cbba28a205fa 678 float q1q3 = q1 * q3;
krebyy 0:cbba28a205fa 679 float q1q4 = q1 * q4;
krebyy 0:cbba28a205fa 680 float q2q2 = q2 * q2;
krebyy 0:cbba28a205fa 681 float q2q3 = q2 * q3;
krebyy 0:cbba28a205fa 682 float q2q4 = q2 * q4;
krebyy 0:cbba28a205fa 683 float q3q3 = q3 * q3;
krebyy 0:cbba28a205fa 684 float q3q4 = q3 * q4;
krebyy 0:cbba28a205fa 685 float q4q4 = q4 * q4;
krebyy 0:cbba28a205fa 686
krebyy 0:cbba28a205fa 687 // Normalise accelerometer measurement
krebyy 0:cbba28a205fa 688 norm = sqrt(ax * ax + ay * ay + az * az);
krebyy 0:cbba28a205fa 689 if (norm == 0.0f) return; // handle NaN
krebyy 0:cbba28a205fa 690 norm = 1.0f/norm;
krebyy 0:cbba28a205fa 691 ax *= norm;
krebyy 0:cbba28a205fa 692 ay *= norm;
krebyy 0:cbba28a205fa 693 az *= norm;
krebyy 0:cbba28a205fa 694
krebyy 0:cbba28a205fa 695 // Normalise magnetometer measurement
krebyy 0:cbba28a205fa 696 norm = sqrt(mx * mx + my * my + mz * mz);
krebyy 0:cbba28a205fa 697 if (norm == 0.0f) return; // handle NaN
krebyy 0:cbba28a205fa 698 norm = 1.0f/norm;
krebyy 0:cbba28a205fa 699 mx *= norm;
krebyy 0:cbba28a205fa 700 my *= norm;
krebyy 0:cbba28a205fa 701 mz *= norm;
krebyy 0:cbba28a205fa 702
krebyy 0:cbba28a205fa 703 // Reference direction of Earth's magnetic field
krebyy 0:cbba28a205fa 704 _2q1mx = 2.0f * q1 * mx;
krebyy 0:cbba28a205fa 705 _2q1my = 2.0f * q1 * my;
krebyy 0:cbba28a205fa 706 _2q1mz = 2.0f * q1 * mz;
krebyy 0:cbba28a205fa 707 _2q2mx = 2.0f * q2 * mx;
krebyy 0:cbba28a205fa 708 hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
krebyy 0:cbba28a205fa 709 hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
krebyy 0:cbba28a205fa 710 _2bx = sqrt(hx * hx + hy * hy);
krebyy 0:cbba28a205fa 711 _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
krebyy 0:cbba28a205fa 712 _4bx = 2.0f * _2bx;
krebyy 0:cbba28a205fa 713 _4bz = 2.0f * _2bz;
krebyy 0:cbba28a205fa 714
krebyy 0:cbba28a205fa 715 // Gradient decent algorithm corrective step
krebyy 0:cbba28a205fa 716 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);
krebyy 0:cbba28a205fa 717 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);
krebyy 0:cbba28a205fa 718 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);
krebyy 0:cbba28a205fa 719 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);
krebyy 0:cbba28a205fa 720 norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
krebyy 0:cbba28a205fa 721 norm = 1.0f/norm;
krebyy 0:cbba28a205fa 722 s1 *= norm;
krebyy 0:cbba28a205fa 723 s2 *= norm;
krebyy 0:cbba28a205fa 724 s3 *= norm;
krebyy 0:cbba28a205fa 725 s4 *= norm;
krebyy 0:cbba28a205fa 726
krebyy 0:cbba28a205fa 727 // Compute rate of change of quaternion
krebyy 0:cbba28a205fa 728 qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
krebyy 0:cbba28a205fa 729 qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
krebyy 0:cbba28a205fa 730 qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
krebyy 0:cbba28a205fa 731 qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
krebyy 0:cbba28a205fa 732
krebyy 0:cbba28a205fa 733 // Integrate to yield quaternion
krebyy 0:cbba28a205fa 734 q1 += qDot1 * deltat;
krebyy 0:cbba28a205fa 735 q2 += qDot2 * deltat;
krebyy 0:cbba28a205fa 736 q3 += qDot3 * deltat;
krebyy 0:cbba28a205fa 737 q4 += qDot4 * deltat;
krebyy 0:cbba28a205fa 738 norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
krebyy 0:cbba28a205fa 739 norm = 1.0f/norm;
krebyy 0:cbba28a205fa 740 q[0] = q1 * norm;
krebyy 0:cbba28a205fa 741 q[1] = q2 * norm;
krebyy 0:cbba28a205fa 742 q[2] = q3 * norm;
krebyy 0:cbba28a205fa 743 q[3] = q4 * norm;
krebyy 0:cbba28a205fa 744
krebyy 0:cbba28a205fa 745 }
krebyy 0:cbba28a205fa 746
krebyy 0:cbba28a205fa 747 /** Get raw 9-axis motion sensor readings (accel/gyro/compass).
krebyy 0:cbba28a205fa 748 * @param ax 12-bit signed integer container for accelerometer X-axis value
krebyy 0:cbba28a205fa 749 * @param ay 12-bit signed integer container for accelerometer Y-axis value
krebyy 0:cbba28a205fa 750 * @param az 12-bit signed integer container for accelerometer Z-axis value
krebyy 0:cbba28a205fa 751 * @param gx 16-bit signed integer container for gyroscope X-axis value
krebyy 0:cbba28a205fa 752 * @param gy 16-bit signed integer container for gyroscope Y-axis value
krebyy 0:cbba28a205fa 753 * @param gz 16-bit signed integer container for gyroscope Z-axis value
krebyy 0:cbba28a205fa 754 * @param mx 13-bit signed integer container for magnetometer X-axis value
krebyy 0:cbba28a205fa 755 * @param my 13-bit signed integer container for magnetometer Y-axis value
krebyy 0:cbba28a205fa 756 * @param mz 15-bit signed integer container for magnetometer Z-axis value
krebyy 0:cbba28a205fa 757 * @see getAcceleration()
krebyy 0:cbba28a205fa 758 * @see getRotation()
krebyy 0:cbba28a205fa 759 * @see getMag()
krebyy 0:cbba28a205fa 760 */
krebyy 0:cbba28a205fa 761 void getRaw9( int16_t* ax, int16_t* ay, int16_t* az,
krebyy 0:cbba28a205fa 762 int16_t* gx, int16_t* gy, int16_t* gz,
krebyy 0:cbba28a205fa 763 int16_t* mx, int16_t* my, int16_t* mz)
krebyy 0:cbba28a205fa 764 {
krebyy 0:cbba28a205fa 765 uint8_t rawData[8]; // x/y/z MSB and LSB registers raw data stored here
krebyy 0:cbba28a205fa 766
krebyy 0:cbba28a205fa 767 // Read the six raw data registers into data array
krebyy 0:cbba28a205fa 768 // Turn the MSB and LSB into a signed 12-bit value
krebyy 0:cbba28a205fa 769 readBytes(BMX055_ACC_ADDRESS, BMX055_ACC_D_X_LSB, 6, rawData);
krebyy 0:cbba28a205fa 770 *ax = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]) >> 4;
krebyy 0:cbba28a205fa 771 *ay = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) >> 4;
krebyy 0:cbba28a205fa 772 *az = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) >> 4;
krebyy 0:cbba28a205fa 773
krebyy 0:cbba28a205fa 774 // Read the six raw data registers sequentially into data array
krebyy 0:cbba28a205fa 775 // Turn the MSB and LSB into a signed 16-bit value
krebyy 0:cbba28a205fa 776 readBytes(BMX055_GYRO_ADDRESS, BMX055_GYRO_RATE_X_LSB, 6, rawData);
krebyy 0:cbba28a205fa 777 *gx = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);
krebyy 0:cbba28a205fa 778 *gy = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]);
krebyy 0:cbba28a205fa 779 *gz = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]);
krebyy 0:cbba28a205fa 780
krebyy 0:cbba28a205fa 781 // Read the six raw data registers into data array
krebyy 0:cbba28a205fa 782 // 13-bit signed integer for x-axis and y-axis field
krebyy 0:cbba28a205fa 783 // 15-bit signed integer for z-axis field
krebyy 0:cbba28a205fa 784 readBytes(BMX055_MAG_ADDRESS, BMX055_MAG_XOUT_LSB, 8, rawData);
krebyy 0:cbba28a205fa 785 if(rawData[6] & 0x01) // Check if data ready status bit is set
krebyy 0:cbba28a205fa 786 {
krebyy 0:cbba28a205fa 787 *mx = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]) >> 3;
krebyy 0:cbba28a205fa 788 *my = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) >> 3;
krebyy 0:cbba28a205fa 789 *mz = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) >> 1;
krebyy 0:cbba28a205fa 790 }
krebyy 0:cbba28a205fa 791 }
krebyy 0:cbba28a205fa 792
krebyy 0:cbba28a205fa 793 /** Get raw 9-axis motion sensor readings (accel/gyro/compass).
krebyy 0:cbba28a205fa 794 * @param ax accelerometer X-axis value (g's)
krebyy 0:cbba28a205fa 795 * @param ay accelerometer Y-axis value (g's)
krebyy 0:cbba28a205fa 796 * @param az accelerometer Z-axis value (g's)
krebyy 0:cbba28a205fa 797 * @param gx gyroscope X-axis value (degrees per second)
krebyy 0:cbba28a205fa 798 * @param gy gyroscope Y-axis value (degrees per second)
krebyy 0:cbba28a205fa 799 * @param gz gyroscope Z-axis value (degrees per second)
krebyy 0:cbba28a205fa 800 * @param mx magnetometer X-axis value (milliGauss)
krebyy 0:cbba28a205fa 801 * @param my magnetometer Y-axis value (milliGauss)
krebyy 0:cbba28a205fa 802 * @param mz magnetometer Z-axis value (milliGauss)
krebyy 0:cbba28a205fa 803 * @see getAcceleration()
krebyy 0:cbba28a205fa 804 * @see getRotation()
krebyy 0:cbba28a205fa 805 * @see getMag()
krebyy 0:cbba28a205fa 806 */
krebyy 0:cbba28a205fa 807 void getMotion9( float* ax, float* ay, float* az,
krebyy 0:cbba28a205fa 808 float* gx, float* gy, float* gz,
krebyy 0:cbba28a205fa 809 float* mx, float* my, float* mz)
krebyy 0:cbba28a205fa 810 {
krebyy 0:cbba28a205fa 811 int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
krebyy 0:cbba28a205fa 812 int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
krebyy 0:cbba28a205fa 813 int16_t magCount[3]; // Stores the 13/15-bit signed magnetometer sensor output
krebyy 0:cbba28a205fa 814
krebyy 0:cbba28a205fa 815 // Read the x/y/z raw values
krebyy 0:cbba28a205fa 816 readAccelData(accelCount);
krebyy 0:cbba28a205fa 817
krebyy 0:cbba28a205fa 818 // Calculate the accleration value into actual g's
krebyy 0:cbba28a205fa 819 // get actual g value, this depends on scale being set
krebyy 0:cbba28a205fa 820 *ax = (float)accelCount[0]*aRes; // + accelBias[0];
krebyy 0:cbba28a205fa 821 *ay = (float)accelCount[1]*aRes; // + accelBias[1];
krebyy 0:cbba28a205fa 822 *az = (float)accelCount[2]*aRes; // + accelBias[2];
krebyy 0:cbba28a205fa 823
krebyy 0:cbba28a205fa 824
krebyy 0:cbba28a205fa 825 // Read the x/y/z raw values
krebyy 0:cbba28a205fa 826 readGyroData(gyroCount);
krebyy 0:cbba28a205fa 827
krebyy 0:cbba28a205fa 828 // Calculate the gyro value into actual degrees per second
krebyy 0:cbba28a205fa 829 // get actual gyro value, this depends on scale being set
krebyy 0:cbba28a205fa 830 *gx = (float)gyroCount[0]*gRes;
krebyy 0:cbba28a205fa 831 *gy = (float)gyroCount[1]*gRes;
krebyy 0:cbba28a205fa 832 *gz = (float)gyroCount[2]*gRes;
krebyy 0:cbba28a205fa 833
krebyy 0:cbba28a205fa 834
krebyy 0:cbba28a205fa 835 // Read the x/y/z raw values
krebyy 0:cbba28a205fa 836 readMagData(magCount);
krebyy 0:cbba28a205fa 837
krebyy 0:cbba28a205fa 838 // Calculate the magnetometer values in milliGauss
krebyy 0:cbba28a205fa 839 // Temperature-compensated magnetic field is in 16 LSB/microTesla
krebyy 0:cbba28a205fa 840 // get actual magnetometer value, this depends on scale being set
krebyy 0:cbba28a205fa 841 *mx = (float)magCount[0]*mRes - magBias[0];
krebyy 0:cbba28a205fa 842 *my = (float)magCount[1]*mRes - magBias[1];
krebyy 0:cbba28a205fa 843 *mz = (float)magCount[2]*mRes - magBias[2];
krebyy 0:cbba28a205fa 844 }
krebyy 0:cbba28a205fa 845
krebyy 0:cbba28a205fa 846 void runAHRS(float mdeltat, float local_declination = LOCAL_DECLINATION)
krebyy 0:cbba28a205fa 847 {
krebyy 0:cbba28a205fa 848 float ax, ay, az, gx, gy, gz, mx, my, mz;
krebyy 0:cbba28a205fa 849
krebyy 0:cbba28a205fa 850 getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
krebyy 0:cbba28a205fa 851 deltat = mdeltat;
krebyy 0:cbba28a205fa 852
krebyy 0:cbba28a205fa 853 // Sensors x (y)-axis of the accelerometer is aligned with the -y (x)-axis of the magnetometer;
krebyy 0:cbba28a205fa 854 // the magnetometer z-axis (+ up) is aligned with z-axis (+ up) of accelerometer and gyro!
krebyy 0:cbba28a205fa 855 // We have to make some allowance for this orientation mismatch in feeding the output to the quaternion filter.
krebyy 0:cbba28a205fa 856 // For the BMX-055, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like
krebyy 0:cbba28a205fa 857 // in the MPU9250 sensor. This rotation can be modified to allow any convenient orientation convention.
krebyy 0:cbba28a205fa 858 // This is ok by aircraft orientation standards!
krebyy 0:cbba28a205fa 859 // Pass gyro rate as rad/s
krebyy 0:cbba28a205fa 860 //MadgwickQuaternionUpdate(ax, ay, az, gx*M_PI/180.0f, gy*M_PI/180.0f, gz*M_PI/180.0f, -my, mx, mz);
krebyy 0:cbba28a205fa 861 MadgwickQuaternionUpdate(-ay, ax, az, -gy*M_PI/180.0f, gx*M_PI/180.0f, gz*M_PI/180.0f, mx, my, mz);
krebyy 0:cbba28a205fa 862
krebyy 0:cbba28a205fa 863
krebyy 0:cbba28a205fa 864 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
krebyy 0:cbba28a205fa 865 // In this coordinate system, the positive z-axis is down toward Earth.
krebyy 0:cbba28a205fa 866 // 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.
krebyy 0:cbba28a205fa 867 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
krebyy 0:cbba28a205fa 868 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
krebyy 0:cbba28a205fa 869 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
krebyy 0:cbba28a205fa 870 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
krebyy 0:cbba28a205fa 871 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
krebyy 0:cbba28a205fa 872 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
krebyy 0:cbba28a205fa 873 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]);
krebyy 0:cbba28a205fa 874 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
krebyy 0:cbba28a205fa 875 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]);
krebyy 0:cbba28a205fa 876 pitch *= 180.0f / M_PI;
krebyy 0:cbba28a205fa 877 yaw *= 180.0f / M_PI;
krebyy 0:cbba28a205fa 878 yaw -= local_declination;
krebyy 0:cbba28a205fa 879 roll *= 180.0f / M_PI;
krebyy 0:cbba28a205fa 880 }
krebyy 0:cbba28a205fa 881 };
krebyy 0:cbba28a205fa 882
krebyy 0:cbba28a205fa 883 #endif /* BMX055_H_ */