Some improvements on mrcrsch's BNO055_fusion library
Fork of BNO055_fusion by
BNO055.h@5:6a08a4c5b1e1, 2017-01-16 (annotated)
- Committer:
- mrcrsch
- Date:
- Mon Jan 16 16:51:39 2017 +0000
- Revision:
- 5:6a08a4c5b1e1
- Parent:
- 4:9e6fead1e93e
- Child:
- 7:7df0c13f0ef9
added extra functions;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenjiArai | 0:86a17116e8be | 1 | /* |
kenjiArai | 0:86a17116e8be | 2 | * mbed library program |
kenjiArai | 0:86a17116e8be | 3 | * BNO055 Intelligent 9-axis absolute orientation sensor |
kenjiArai | 0:86a17116e8be | 4 | * by Bosch Sensortec |
kenjiArai | 0:86a17116e8be | 5 | * |
kenjiArai | 0:86a17116e8be | 6 | * Copyright (c) 2015 Kenji Arai / JH1PJL |
kenjiArai | 0:86a17116e8be | 7 | * http://www.page.sannet.ne.jp/kenjia/index.html |
kenjiArai | 0:86a17116e8be | 8 | * http://mbed.org/users/kenjiArai/ |
kenjiArai | 0:86a17116e8be | 9 | * Created: March 30th, 2015 |
kenjiArai | 4:9e6fead1e93e | 10 | * Revised: April 16th, 2015 |
kenjiArai | 0:86a17116e8be | 11 | * |
kenjiArai | 0:86a17116e8be | 12 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
kenjiArai | 0:86a17116e8be | 13 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE |
kenjiArai | 0:86a17116e8be | 14 | * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, |
kenjiArai | 0:86a17116e8be | 15 | * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
kenjiArai | 0:86a17116e8be | 16 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
kenjiArai | 0:86a17116e8be | 17 | */ |
kenjiArai | 0:86a17116e8be | 18 | /* |
kenjiArai | 0:86a17116e8be | 19 | *---------------- REFERENCE ---------------------------------------------------------------------- |
kenjiArai | 0:86a17116e8be | 20 | * Original Information |
kenjiArai | 0:86a17116e8be | 21 | * https://www.bosch-sensortec.com/en/homepage/products_3/sensor_hubs/iot_solutions/bno055_1/bno055_4 |
kenjiArai | 0:86a17116e8be | 22 | * Intelligent 9-axis absolute orientation sensor / Data Sheet BST_BNO055_DS000_12 Nov. 2014 rev.1.2 |
kenjiArai | 0:86a17116e8be | 23 | * Sample software https://github.com/BoschSensortec/BNO055_driver |
kenjiArai | 0:86a17116e8be | 24 | * Sensor board |
kenjiArai | 0:86a17116e8be | 25 | * https://www.rutronik24.com/product/bosch+se/bno055+shuttle+board+mems/6431291.html |
kenjiArai | 0:86a17116e8be | 26 | * http://microcontrollershop.com/product_info.php?products_id=7140&osCsid=10645k86db2crld4tfi0vol5g5 |
kenjiArai | 0:86a17116e8be | 27 | */ |
kenjiArai | 0:86a17116e8be | 28 | |
kenjiArai | 0:86a17116e8be | 29 | #ifndef BNO055_H |
kenjiArai | 0:86a17116e8be | 30 | #define BNO055_H |
kenjiArai | 0:86a17116e8be | 31 | |
kenjiArai | 0:86a17116e8be | 32 | #include "mbed.h" |
kenjiArai | 0:86a17116e8be | 33 | |
kenjiArai | 0:86a17116e8be | 34 | // BNO055 |
kenjiArai | 0:86a17116e8be | 35 | // 7bit address = 0b010100x(0x28 or 0x29 depends on COM3) |
kenjiArai | 0:86a17116e8be | 36 | #define BNO055_G_CHIP_ADDR (0x28 << 1) // COM3 = GND |
kenjiArai | 0:86a17116e8be | 37 | #define BNO055_V_CHIP_ADDR (0x29 << 1) // COM3 = Vdd |
kenjiArai | 0:86a17116e8be | 38 | |
kenjiArai | 0:86a17116e8be | 39 | // Fusion mode |
kenjiArai | 0:86a17116e8be | 40 | #define CONFIGMODE 0x00 |
kenjiArai | 0:86a17116e8be | 41 | #define MODE_IMU 0x08 |
kenjiArai | 0:86a17116e8be | 42 | #define MODE_COMPASS 0x09 |
kenjiArai | 0:86a17116e8be | 43 | #define MODE_M4G 0x0a |
kenjiArai | 0:86a17116e8be | 44 | #define MODE_NDOF_FMC_OFF 0x0b |
kenjiArai | 0:86a17116e8be | 45 | #define MODE_NDOF 0x0c |
kenjiArai | 0:86a17116e8be | 46 | |
kenjiArai | 0:86a17116e8be | 47 | // UNIT |
kenjiArai | 0:86a17116e8be | 48 | #define UNIT_ACC_MSS 0x00 // acc m/s2 |
kenjiArai | 0:86a17116e8be | 49 | #define UNIT_ACC_MG 0x01 // acc mg |
kenjiArai | 0:86a17116e8be | 50 | #define UNIT_GYR_DPS 0x00 // gyro Dps |
kenjiArai | 0:86a17116e8be | 51 | #define UNIT_GYR_RPS 0x02 // gyro Rps |
kenjiArai | 0:86a17116e8be | 52 | #define UNIT_EULER_DEG 0x00 // euler Degrees |
kenjiArai | 0:86a17116e8be | 53 | #define UNIT_EULER_RAD 0x04 // euler Radians |
kenjiArai | 0:86a17116e8be | 54 | #define UNIT_TEMP_C 0x00 // temperature degC |
kenjiArai | 0:86a17116e8be | 55 | #define UNIT_TEMP_F 0x10 // temperature degF |
kenjiArai | 0:86a17116e8be | 56 | #define UNIT_ORI_WIN 0x00 // Windows orientation |
kenjiArai | 0:86a17116e8be | 57 | #define UNIT_ORI_ANDROID 0x80 // Android orientation |
kenjiArai | 0:86a17116e8be | 58 | |
kenjiArai | 0:86a17116e8be | 59 | // ID's |
kenjiArai | 0:86a17116e8be | 60 | #define I_AM_BNO055_CHIP 0xa0 // CHIP ID |
kenjiArai | 0:86a17116e8be | 61 | #define I_AM_BNO055_ACC 0xfb // ACC ID |
kenjiArai | 0:86a17116e8be | 62 | #define I_AM_BNO055_MAG 0x32 // MAG ID |
kenjiArai | 0:86a17116e8be | 63 | #define I_AM_BNO055_GYR 0x0f // GYR ID |
kenjiArai | 0:86a17116e8be | 64 | |
mrcrsch | 5:6a08a4c5b1e1 | 65 | |
mrcrsch | 5:6a08a4c5b1e1 | 66 | |
mrcrsch | 5:6a08a4c5b1e1 | 67 | |
kenjiArai | 0:86a17116e8be | 68 | ////////////// DATA TYPE DEFINITION /////////////////////// |
kenjiArai | 0:86a17116e8be | 69 | typedef struct { |
kenjiArai | 0:86a17116e8be | 70 | uint8_t chip_id; |
kenjiArai | 0:86a17116e8be | 71 | uint8_t acc_id; |
kenjiArai | 0:86a17116e8be | 72 | uint8_t mag_id; |
kenjiArai | 0:86a17116e8be | 73 | uint8_t gyr_id; |
kenjiArai | 0:86a17116e8be | 74 | uint8_t bootldr_rev_id; |
kenjiArai | 0:86a17116e8be | 75 | uint16_t sw_rev_id; |
kenjiArai | 0:86a17116e8be | 76 | } BNO055_ID_INF_TypeDef; |
kenjiArai | 0:86a17116e8be | 77 | |
kenjiArai | 0:86a17116e8be | 78 | typedef struct { |
kenjiArai | 0:86a17116e8be | 79 | double h; |
kenjiArai | 0:86a17116e8be | 80 | double r; |
kenjiArai | 0:86a17116e8be | 81 | double p; |
kenjiArai | 0:86a17116e8be | 82 | } BNO055_EULER_TypeDef; |
kenjiArai | 0:86a17116e8be | 83 | |
kenjiArai | 0:86a17116e8be | 84 | typedef struct { |
kenjiArai | 0:86a17116e8be | 85 | int16_t x; |
kenjiArai | 0:86a17116e8be | 86 | int16_t y; |
kenjiArai | 0:86a17116e8be | 87 | int16_t z; |
kenjiArai | 0:86a17116e8be | 88 | int16_t w; |
kenjiArai | 0:86a17116e8be | 89 | } BNO055_QUATERNION_TypeDef; |
kenjiArai | 0:86a17116e8be | 90 | |
kenjiArai | 0:86a17116e8be | 91 | typedef struct { |
kenjiArai | 0:86a17116e8be | 92 | double x; |
kenjiArai | 0:86a17116e8be | 93 | double y; |
kenjiArai | 0:86a17116e8be | 94 | double z; |
kenjiArai | 0:86a17116e8be | 95 | } BNO055_LIN_ACC_TypeDef; |
kenjiArai | 0:86a17116e8be | 96 | |
kenjiArai | 0:86a17116e8be | 97 | typedef struct { |
kenjiArai | 0:86a17116e8be | 98 | double x; |
kenjiArai | 0:86a17116e8be | 99 | double y; |
kenjiArai | 0:86a17116e8be | 100 | double z; |
kenjiArai | 0:86a17116e8be | 101 | } BNO055_GRAVITY_TypeDef; |
kenjiArai | 0:86a17116e8be | 102 | |
kenjiArai | 0:86a17116e8be | 103 | typedef struct { |
kenjiArai | 0:86a17116e8be | 104 | int8_t acc_chip; |
kenjiArai | 0:86a17116e8be | 105 | int8_t gyr_chip; |
kenjiArai | 0:86a17116e8be | 106 | } BNO055_TEMPERATURE_TypeDef; |
kenjiArai | 0:86a17116e8be | 107 | |
mrcrsch | 5:6a08a4c5b1e1 | 108 | typedef struct { |
mrcrsch | 5:6a08a4c5b1e1 | 109 | uint8_t Gscale; |
mrcrsch | 5:6a08a4c5b1e1 | 110 | uint8_t Gbw; |
mrcrsch | 5:6a08a4c5b1e1 | 111 | uint8_t GPwrMode; |
mrcrsch | 5:6a08a4c5b1e1 | 112 | |
mrcrsch | 5:6a08a4c5b1e1 | 113 | uint8_t Ascale; |
mrcrsch | 5:6a08a4c5b1e1 | 114 | uint8_t Abw; |
mrcrsch | 5:6a08a4c5b1e1 | 115 | uint8_t APwrMode; |
mrcrsch | 5:6a08a4c5b1e1 | 116 | |
mrcrsch | 5:6a08a4c5b1e1 | 117 | uint8_t MOpMode; |
mrcrsch | 5:6a08a4c5b1e1 | 118 | uint8_t MPwrMode; |
mrcrsch | 5:6a08a4c5b1e1 | 119 | uint8_t Modr; |
mrcrsch | 5:6a08a4c5b1e1 | 120 | |
mrcrsch | 5:6a08a4c5b1e1 | 121 | } BNO055_CONFIG_TypeDef; |
mrcrsch | 5:6a08a4c5b1e1 | 122 | |
kenjiArai | 0:86a17116e8be | 123 | enum {MT_P0 = 0, MT_P1, MT_P2, MT_P3, MT_P4, MT_P5, MT_P6, MT_P7}; |
kenjiArai | 0:86a17116e8be | 124 | |
kenjiArai | 0:86a17116e8be | 125 | /** Interface for Bosch Sensortec Intelligent 9-axis absolute orientation sensor |
kenjiArai | 0:86a17116e8be | 126 | * Chip: BNO055 |
kenjiArai | 0:86a17116e8be | 127 | * |
kenjiArai | 0:86a17116e8be | 128 | * @code |
kenjiArai | 0:86a17116e8be | 129 | * #include "mbed.h" |
kenjiArai | 0:86a17116e8be | 130 | * #include "BNO055.h" |
kenjiArai | 0:86a17116e8be | 131 | * |
kenjiArai | 0:86a17116e8be | 132 | * Serial pc(USBTX,USBRX); |
kenjiArai | 0:86a17116e8be | 133 | * I2C i2c(PB_9, PB_8); // SDA, SCL |
kenjiArai | 1:cb7e19c0a702 | 134 | * BNO055 imu(i2c, PA_8); // Reset |
kenjiArai | 0:86a17116e8be | 135 | * |
kenjiArai | 0:86a17116e8be | 136 | * BNO055_ID_INF_TypeDef bno055_id_inf; |
kenjiArai | 0:86a17116e8be | 137 | * BNO055_EULER_TypeDef euler_angles; |
kenjiArai | 0:86a17116e8be | 138 | * |
kenjiArai | 0:86a17116e8be | 139 | * int main() { |
kenjiArai | 0:86a17116e8be | 140 | * pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
kenjiArai | 0:86a17116e8be | 141 | * if (imu.chip_ready() == 0){ |
kenjiArai | 0:86a17116e8be | 142 | * pc.printf("Bosch BNO055 is NOT avirable!!\r\n"); |
kenjiArai | 0:86a17116e8be | 143 | * } |
kenjiArai | 0:86a17116e8be | 144 | * imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 0:86a17116e8be | 145 | * pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", |
kenjiArai | 0:86a17116e8be | 146 | * bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, |
kenjiArai | 0:86a17116e8be | 147 | * bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
kenjiArai | 0:86a17116e8be | 148 | * while(1) { |
kenjiArai | 0:86a17116e8be | 149 | * imu.get_Euler_Angles(&euler_angles); |
kenjiArai | 0:86a17116e8be | 150 | * pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg]\r\n", |
kenjiArai | 0:86a17116e8be | 151 | * euler_angles.h, euler_angles.r, euler_angles.p); |
kenjiArai | 0:86a17116e8be | 152 | * wait(0.5); |
kenjiArai | 0:86a17116e8be | 153 | * } |
kenjiArai | 0:86a17116e8be | 154 | * } |
kenjiArai | 0:86a17116e8be | 155 | * @endcode |
kenjiArai | 0:86a17116e8be | 156 | */ |
kenjiArai | 0:86a17116e8be | 157 | |
kenjiArai | 0:86a17116e8be | 158 | class BNO055 |
kenjiArai | 0:86a17116e8be | 159 | { |
kenjiArai | 0:86a17116e8be | 160 | public: |
kenjiArai | 0:86a17116e8be | 161 | /** Configure data pin |
kenjiArai | 0:86a17116e8be | 162 | * @param data SDA and SCL pins |
kenjiArai | 0:86a17116e8be | 163 | * @param device address |
kenjiArai | 0:86a17116e8be | 164 | */ |
kenjiArai | 1:cb7e19c0a702 | 165 | BNO055(PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode); |
kenjiArai | 0:86a17116e8be | 166 | |
kenjiArai | 0:86a17116e8be | 167 | /** Configure data pin |
kenjiArai | 0:86a17116e8be | 168 | * @param data SDA and SCL pins |
kenjiArai | 0:86a17116e8be | 169 | * @param Other parameters are set default data |
kenjiArai | 0:86a17116e8be | 170 | */ |
kenjiArai | 1:cb7e19c0a702 | 171 | BNO055(PinName p_sda, PinName p_scl, PinName p_reset); |
kenjiArai | 0:86a17116e8be | 172 | |
kenjiArai | 0:86a17116e8be | 173 | /** Configure data pin (with other devices on I2C line) |
kenjiArai | 0:86a17116e8be | 174 | * @param I2C previous definition |
kenjiArai | 0:86a17116e8be | 175 | * @param device address |
kenjiArai | 0:86a17116e8be | 176 | */ |
kenjiArai | 1:cb7e19c0a702 | 177 | BNO055(I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode); |
kenjiArai | 0:86a17116e8be | 178 | |
kenjiArai | 0:86a17116e8be | 179 | /** Configure data pin (with other devices on I2C line) |
kenjiArai | 0:86a17116e8be | 180 | * @param I2C previous definition |
kenjiArai | 0:86a17116e8be | 181 | * @param Other parameters are set default data |
kenjiArai | 0:86a17116e8be | 182 | */ |
kenjiArai | 1:cb7e19c0a702 | 183 | BNO055(I2C& p_i2c, PinName p_reset); |
kenjiArai | 0:86a17116e8be | 184 | |
kenjiArai | 0:86a17116e8be | 185 | /** Get Euler Angles |
kenjiArai | 0:86a17116e8be | 186 | * @param double type of 3D data address |
kenjiArai | 0:86a17116e8be | 187 | */ |
kenjiArai | 0:86a17116e8be | 188 | void get_Euler_Angles(BNO055_EULER_TypeDef *el); |
kenjiArai | 0:86a17116e8be | 189 | |
kenjiArai | 0:86a17116e8be | 190 | /** Get Quaternion XYZ&W |
kenjiArai | 0:86a17116e8be | 191 | * @param int16_t type of 4D data address |
kenjiArai | 0:86a17116e8be | 192 | */ |
kenjiArai | 0:86a17116e8be | 193 | void get_quaternion(BNO055_QUATERNION_TypeDef *qua); |
kenjiArai | 0:86a17116e8be | 194 | |
kenjiArai | 0:86a17116e8be | 195 | /** Get Linear accel data |
kenjiArai | 0:86a17116e8be | 196 | * @param double type of 3D data address |
kenjiArai | 0:86a17116e8be | 197 | */ |
kenjiArai | 0:86a17116e8be | 198 | void get_linear_accel(BNO055_LIN_ACC_TypeDef *la); |
kenjiArai | 0:86a17116e8be | 199 | |
kenjiArai | 0:86a17116e8be | 200 | /** Get Gravity data |
kenjiArai | 0:86a17116e8be | 201 | * @param double type of 3D data address |
kenjiArai | 0:86a17116e8be | 202 | */ |
kenjiArai | 0:86a17116e8be | 203 | void get_gravity(BNO055_GRAVITY_TypeDef *gr); |
kenjiArai | 0:86a17116e8be | 204 | |
kenjiArai | 0:86a17116e8be | 205 | /** Get Chip temperature data both Acc & Gyro |
kenjiArai | 0:86a17116e8be | 206 | * @param int8_t type of data address |
kenjiArai | 0:86a17116e8be | 207 | */ |
kenjiArai | 0:86a17116e8be | 208 | void get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp); |
kenjiArai | 0:86a17116e8be | 209 | |
kenjiArai | 0:86a17116e8be | 210 | /** Change fusion mode |
kenjiArai | 0:86a17116e8be | 211 | * @param fusion mode |
kenjiArai | 0:86a17116e8be | 212 | * @return none |
kenjiArai | 0:86a17116e8be | 213 | */ |
kenjiArai | 0:86a17116e8be | 214 | void change_fusion_mode(uint8_t mode); |
kenjiArai | 0:86a17116e8be | 215 | |
kenjiArai | 0:86a17116e8be | 216 | /** Set Mouting position |
kenjiArai | 0:86a17116e8be | 217 | * Please make sure your mounting direction of BNO055 chip |
kenjiArai | 0:86a17116e8be | 218 | * refrence: BNO055 data sheet BST-BNO055-DS000-12 3.4 Axis remap |
kenjiArai | 0:86a17116e8be | 219 | * @param Set P0 to P7 mounting position data |
kenjiArai | 0:86a17116e8be | 220 | * @return none |
kenjiArai | 0:86a17116e8be | 221 | */ |
kenjiArai | 0:86a17116e8be | 222 | void set_mounting_position(uint8_t position); |
kenjiArai | 0:86a17116e8be | 223 | |
kenjiArai | 0:86a17116e8be | 224 | /** Read BNO055 ID information |
kenjiArai | 0:86a17116e8be | 225 | * @param ID information address |
kenjiArai | 0:86a17116e8be | 226 | * @return none |
kenjiArai | 0:86a17116e8be | 227 | */ |
kenjiArai | 0:86a17116e8be | 228 | void read_id_inf(BNO055_ID_INF_TypeDef *id); |
kenjiArai | 0:86a17116e8be | 229 | |
kenjiArai | 0:86a17116e8be | 230 | /** Check chip is avairable or not |
kenjiArai | 0:86a17116e8be | 231 | * @param none |
kenjiArai | 0:86a17116e8be | 232 | * @return OK = 1, NG = 0; |
kenjiArai | 0:86a17116e8be | 233 | */ |
kenjiArai | 0:86a17116e8be | 234 | uint8_t chip_ready(void); |
kenjiArai | 0:86a17116e8be | 235 | |
kenjiArai | 2:0f225b686cd5 | 236 | /** Read calibration status |
kenjiArai | 2:0f225b686cd5 | 237 | * @param none |
kenjiArai | 2:0f225b686cd5 | 238 | * @return SYS(7:6),GYR(5:4),ACC(3:2),MAG(1:0) 3 = Calibrated, 0= not yet |
kenjiArai | 2:0f225b686cd5 | 239 | */ |
kenjiArai | 2:0f225b686cd5 | 240 | uint8_t read_calib_status(void); |
kenjiArai | 2:0f225b686cd5 | 241 | |
kenjiArai | 1:cb7e19c0a702 | 242 | /** Reset |
kenjiArai | 1:cb7e19c0a702 | 243 | * @param none |
kenjiArai | 3:0ad6f85b178f | 244 | * @return 0 = sucess, 1 = Not available chip |
kenjiArai | 1:cb7e19c0a702 | 245 | */ |
kenjiArai | 1:cb7e19c0a702 | 246 | uint8_t reset(void); |
kenjiArai | 1:cb7e19c0a702 | 247 | |
kenjiArai | 0:86a17116e8be | 248 | /** Set I2C clock frequency |
kenjiArai | 0:86a17116e8be | 249 | * @param freq. |
kenjiArai | 0:86a17116e8be | 250 | * @return none |
kenjiArai | 0:86a17116e8be | 251 | */ |
kenjiArai | 0:86a17116e8be | 252 | void frequency(int hz); |
kenjiArai | 0:86a17116e8be | 253 | |
kenjiArai | 0:86a17116e8be | 254 | /** Read page 0 register |
kenjiArai | 0:86a17116e8be | 255 | * @param register's address |
kenjiArai | 0:86a17116e8be | 256 | * @return register data |
kenjiArai | 0:86a17116e8be | 257 | */ |
kenjiArai | 0:86a17116e8be | 258 | uint8_t read_reg0(uint8_t addr); |
kenjiArai | 0:86a17116e8be | 259 | |
kenjiArai | 0:86a17116e8be | 260 | /** Write page 0 register |
kenjiArai | 0:86a17116e8be | 261 | * @param register's address |
kenjiArai | 0:86a17116e8be | 262 | * @param data |
kenjiArai | 0:86a17116e8be | 263 | * @return register data |
kenjiArai | 0:86a17116e8be | 264 | */ |
kenjiArai | 0:86a17116e8be | 265 | uint8_t write_reg0(uint8_t addr, uint8_t data); |
kenjiArai | 0:86a17116e8be | 266 | |
kenjiArai | 0:86a17116e8be | 267 | /** Read page 1 register |
kenjiArai | 0:86a17116e8be | 268 | * @param register's address |
kenjiArai | 0:86a17116e8be | 269 | * @return register data |
kenjiArai | 0:86a17116e8be | 270 | */ |
kenjiArai | 0:86a17116e8be | 271 | uint8_t read_reg1(uint8_t addr); |
kenjiArai | 0:86a17116e8be | 272 | |
kenjiArai | 0:86a17116e8be | 273 | /** Write page 1 register |
kenjiArai | 0:86a17116e8be | 274 | * @param register's address |
kenjiArai | 0:86a17116e8be | 275 | * @param data |
kenjiArai | 0:86a17116e8be | 276 | * @return register data |
kenjiArai | 0:86a17116e8be | 277 | */ |
kenjiArai | 0:86a17116e8be | 278 | uint8_t write_reg1(uint8_t addr, uint8_t data); |
mrcrsch | 5:6a08a4c5b1e1 | 279 | |
mrcrsch | 5:6a08a4c5b1e1 | 280 | /** Calculate acceleration in Earth frame |
mrcrsch | 5:6a08a4c5b1e1 | 281 | * @param Linear acceleration structure |
mrcrsch | 5:6a08a4c5b1e1 | 282 | */ |
mrcrsch | 5:6a08a4c5b1e1 | 283 | void get_abs_accel(BNO055_LIN_ACC_TypeDef *la); |
mrcrsch | 5:6a08a4c5b1e1 | 284 | |
mrcrsch | 5:6a08a4c5b1e1 | 285 | void get_quaternion_float(float *Q); |
mrcrsch | 5:6a08a4c5b1e1 | 286 | |
mrcrsch | 5:6a08a4c5b1e1 | 287 | void configure_accelerometer_range(uint8_t range); |
mrcrsch | 5:6a08a4c5b1e1 | 288 | |
kenjiArai | 0:86a17116e8be | 289 | protected: |
kenjiArai | 0:86a17116e8be | 290 | void initialize(void); |
kenjiArai | 0:86a17116e8be | 291 | void check_id(void); |
kenjiArai | 0:86a17116e8be | 292 | void set_initial_dt_to_regs(void); |
kenjiArai | 0:86a17116e8be | 293 | void unit_selection(void); |
kenjiArai | 0:86a17116e8be | 294 | uint8_t check_operating_mode(void); |
kenjiArai | 0:86a17116e8be | 295 | uint8_t select_page(uint8_t page); |
kenjiArai | 0:86a17116e8be | 296 | |
kenjiArai | 0:86a17116e8be | 297 | I2C _i2c; |
kenjiArai | 1:cb7e19c0a702 | 298 | DigitalOut _res; |
kenjiArai | 0:86a17116e8be | 299 | |
kenjiArai | 0:86a17116e8be | 300 | private: |
kenjiArai | 0:86a17116e8be | 301 | char dt[10]; // working buffer |
kenjiArai | 0:86a17116e8be | 302 | uint8_t chip_addr; |
kenjiArai | 0:86a17116e8be | 303 | uint8_t chip_mode; |
kenjiArai | 3:0ad6f85b178f | 304 | uint8_t ready_flag; |
kenjiArai | 3:0ad6f85b178f | 305 | uint8_t page_flag; |
kenjiArai | 0:86a17116e8be | 306 | |
kenjiArai | 0:86a17116e8be | 307 | uint8_t chip_id; |
kenjiArai | 0:86a17116e8be | 308 | uint8_t acc_id; |
kenjiArai | 0:86a17116e8be | 309 | uint8_t mag_id; |
kenjiArai | 0:86a17116e8be | 310 | uint8_t gyr_id; |
kenjiArai | 0:86a17116e8be | 311 | uint8_t bootldr_rev_id; |
kenjiArai | 0:86a17116e8be | 312 | uint16_t sw_rev_id; |
mrcrsch | 5:6a08a4c5b1e1 | 313 | |
mrcrsch | 5:6a08a4c5b1e1 | 314 | |
kenjiArai | 0:86a17116e8be | 315 | |
kenjiArai | 0:86a17116e8be | 316 | }; |
kenjiArai | 0:86a17116e8be | 317 | |
kenjiArai | 0:86a17116e8be | 318 | //--------------------------------------------------------- |
kenjiArai | 0:86a17116e8be | 319 | //----- Register's definition ----------------------------- |
kenjiArai | 0:86a17116e8be | 320 | //--------------------------------------------------------- |
kenjiArai | 0:86a17116e8be | 321 | // Page id register definition |
kenjiArai | 0:86a17116e8be | 322 | #define BNO055_PAGE_ID 0x07 |
kenjiArai | 0:86a17116e8be | 323 | |
kenjiArai | 0:86a17116e8be | 324 | //----- page0 --------------------------------------------- |
kenjiArai | 0:86a17116e8be | 325 | #define BNO055_CHIP_ID 0x00 |
kenjiArai | 0:86a17116e8be | 326 | #define BNO055_ACCEL_REV_ID 0x01 |
kenjiArai | 0:86a17116e8be | 327 | #define BNO055_MAG_REV_ID 0x02 |
kenjiArai | 0:86a17116e8be | 328 | #define BNO055_GYRO_REV_ID 0x03 |
kenjiArai | 0:86a17116e8be | 329 | #define BNO055_SW_REV_ID_LSB 0x04 |
kenjiArai | 0:86a17116e8be | 330 | #define BNO055_SW_REV_ID_MSB 0x05 |
kenjiArai | 0:86a17116e8be | 331 | #define BNO055_BL_REV_ID 0x06 |
kenjiArai | 0:86a17116e8be | 332 | |
kenjiArai | 0:86a17116e8be | 333 | // Accel data register*/ |
kenjiArai | 0:86a17116e8be | 334 | #define BNO055_ACC_X_LSB 0x08 |
kenjiArai | 0:86a17116e8be | 335 | #define BNO055_ACC_X_MSB 0x09 |
kenjiArai | 0:86a17116e8be | 336 | #define BNO055_ACC_Y_LSB 0x0a |
kenjiArai | 0:86a17116e8be | 337 | #define BNO055_ACC_Y_MSB 0x0b |
kenjiArai | 0:86a17116e8be | 338 | #define BNO055_ACC_Z_LSB 0x0c |
kenjiArai | 0:86a17116e8be | 339 | #define BNO055_ACC_Z_MSB 0x0d |
kenjiArai | 0:86a17116e8be | 340 | |
kenjiArai | 0:86a17116e8be | 341 | // Mag data register |
kenjiArai | 0:86a17116e8be | 342 | #define BNO055_MAG_X_LSB 0x0e |
kenjiArai | 0:86a17116e8be | 343 | #define BNO055_MAG_X_MSB 0x0f |
kenjiArai | 0:86a17116e8be | 344 | #define BNO055_MAG_Y_LSB 0x10 |
kenjiArai | 0:86a17116e8be | 345 | #define BNO055_MAG_Y_MSB 0x11 |
kenjiArai | 0:86a17116e8be | 346 | #define BNO055_MAG_Z_LSB 0x12 |
kenjiArai | 0:86a17116e8be | 347 | #define BNO055_MAG_Z_MSB 0x13 |
kenjiArai | 0:86a17116e8be | 348 | |
kenjiArai | 0:86a17116e8be | 349 | // Gyro data registers |
kenjiArai | 0:86a17116e8be | 350 | #define BNO055_GYR_X_LSB 0x14 |
kenjiArai | 0:86a17116e8be | 351 | #define BNO055_GYR_X_MSB 0x15 |
kenjiArai | 0:86a17116e8be | 352 | #define BNO055_GYR_Y_LSB 0x16 |
kenjiArai | 0:86a17116e8be | 353 | #define BNO055_GYR_Y_MSB 0x17 |
kenjiArai | 0:86a17116e8be | 354 | #define BNO055_GYR_Z_LSB 0x18 |
kenjiArai | 0:86a17116e8be | 355 | #define BNO055_GYR_Z_MSB 0x19 |
kenjiArai | 0:86a17116e8be | 356 | |
kenjiArai | 0:86a17116e8be | 357 | // Euler data registers |
kenjiArai | 0:86a17116e8be | 358 | #define BNO055_EULER_H_LSB 0x1a |
kenjiArai | 0:86a17116e8be | 359 | #define BNO055_EULER_H_MSB 0x1b |
kenjiArai | 0:86a17116e8be | 360 | |
kenjiArai | 0:86a17116e8be | 361 | #define BNO055_EULER_R_LSB 0x1c |
kenjiArai | 0:86a17116e8be | 362 | #define BNO055_EULER_R_MSB 0x1d |
kenjiArai | 0:86a17116e8be | 363 | |
kenjiArai | 0:86a17116e8be | 364 | #define BNO055_EULER_P_LSB 0x1e |
kenjiArai | 0:86a17116e8be | 365 | #define BNO055_EULER_P_MSB 0x1f |
kenjiArai | 0:86a17116e8be | 366 | |
kenjiArai | 0:86a17116e8be | 367 | // Quaternion data registers |
kenjiArai | 0:86a17116e8be | 368 | #define BNO055_QUATERNION_W_LSB 0x20 |
kenjiArai | 0:86a17116e8be | 369 | #define BNO055_QUATERNION_W_MSB 0x21 |
kenjiArai | 0:86a17116e8be | 370 | #define BNO055_QUATERNION_X_LSB 0x22 |
kenjiArai | 0:86a17116e8be | 371 | #define BNO055_QUATERNION_X_MSB 0x23 |
kenjiArai | 0:86a17116e8be | 372 | #define BNO055_QUATERNION_Y_LSB 0x24 |
kenjiArai | 0:86a17116e8be | 373 | #define BNO055_QUATERNION_Y_MSB 0x25 |
kenjiArai | 0:86a17116e8be | 374 | #define BNO055_QUATERNION_Z_LSB 0x26 |
kenjiArai | 0:86a17116e8be | 375 | #define BNO055_QUATERNION_Z_MSB 0x27 |
kenjiArai | 0:86a17116e8be | 376 | |
kenjiArai | 0:86a17116e8be | 377 | // Linear acceleration data registers |
kenjiArai | 0:86a17116e8be | 378 | #define BNO055_LINEAR_ACC_X_LSB 0x28 |
kenjiArai | 0:86a17116e8be | 379 | #define BNO055_LINEAR_ACC_X_MSB 0x29 |
kenjiArai | 0:86a17116e8be | 380 | #define BNO055_LINEAR_ACC_Y_LSB 0x2a |
kenjiArai | 0:86a17116e8be | 381 | #define BNO055_LINEAR_ACC_Y_MSB 0x2b |
kenjiArai | 0:86a17116e8be | 382 | #define BNO055_LINEAR_ACC_Z_LSB 0x2c |
kenjiArai | 0:86a17116e8be | 383 | #define BNO055_LINEAR_ACC_Z_MSB 0x2d |
kenjiArai | 0:86a17116e8be | 384 | |
kenjiArai | 0:86a17116e8be | 385 | // Gravity data registers |
kenjiArai | 0:86a17116e8be | 386 | #define BNO055_GRAVITY_X_LSB 0x2e |
kenjiArai | 0:86a17116e8be | 387 | #define BNO055_GRAVITY_X_MSB 0x2f |
kenjiArai | 0:86a17116e8be | 388 | #define BNO055_GRAVITY_Y_LSB 0x30 |
kenjiArai | 0:86a17116e8be | 389 | #define BNO055_GRAVITY_Y_MSB 0x31 |
kenjiArai | 0:86a17116e8be | 390 | #define BNO055_GRAVITY_Z_LSB 0x32 |
kenjiArai | 0:86a17116e8be | 391 | #define BNO055_GRAVITY_Z_MSB 0x33 |
kenjiArai | 0:86a17116e8be | 392 | |
kenjiArai | 0:86a17116e8be | 393 | // Temperature data register |
kenjiArai | 0:86a17116e8be | 394 | #define BNO055_TEMP 0x34 |
kenjiArai | 0:86a17116e8be | 395 | |
kenjiArai | 0:86a17116e8be | 396 | // Status registers |
kenjiArai | 0:86a17116e8be | 397 | #define BNO055_CALIB_STAT 0x35 |
kenjiArai | 0:86a17116e8be | 398 | #define BNO055_SELFTEST_RESULT 0x36 |
kenjiArai | 0:86a17116e8be | 399 | #define BNO055_INTR_STAT 0x37 |
kenjiArai | 0:86a17116e8be | 400 | #define BNO055_SYS_CLK_STAT 0x38 |
kenjiArai | 0:86a17116e8be | 401 | #define BNO055_SYS_STAT 0x39 |
kenjiArai | 0:86a17116e8be | 402 | #define BNO055_SYS_ERR 0x3a |
kenjiArai | 0:86a17116e8be | 403 | |
kenjiArai | 0:86a17116e8be | 404 | // Unit selection register |
kenjiArai | 0:86a17116e8be | 405 | #define BNO055_UNIT_SEL 0x3b |
kenjiArai | 0:86a17116e8be | 406 | #define BNO055_DATA_SELECT 0x3c |
kenjiArai | 0:86a17116e8be | 407 | |
kenjiArai | 0:86a17116e8be | 408 | // Mode registers |
kenjiArai | 0:86a17116e8be | 409 | #define BNO055_OPR_MODE 0x3d |
kenjiArai | 0:86a17116e8be | 410 | #define BNO055_PWR_MODE 0x3e |
kenjiArai | 0:86a17116e8be | 411 | #define BNO055_SYS_TRIGGER 0x3f |
kenjiArai | 0:86a17116e8be | 412 | #define BNO055_TEMP_SOURCE 0x40 |
kenjiArai | 0:86a17116e8be | 413 | |
kenjiArai | 0:86a17116e8be | 414 | // Axis remap registers |
kenjiArai | 0:86a17116e8be | 415 | #define BNO055_AXIS_MAP_CONFIG 0x41 |
kenjiArai | 0:86a17116e8be | 416 | #define BNO055_AXIS_MAP_SIGN 0x42 |
kenjiArai | 0:86a17116e8be | 417 | |
kenjiArai | 0:86a17116e8be | 418 | // SIC registers |
kenjiArai | 0:86a17116e8be | 419 | #define BNO055_SIC_MTRX_0_LSB 0x43 |
kenjiArai | 0:86a17116e8be | 420 | #define BNO055_SIC_MTRX_0_MSB 0x44 |
kenjiArai | 0:86a17116e8be | 421 | #define BNO055_SIC_MTRX_1_LSB 0x45 |
kenjiArai | 0:86a17116e8be | 422 | #define BNO055_SIC_MTRX_1_MSB 0x46 |
kenjiArai | 0:86a17116e8be | 423 | #define BNO055_SIC_MTRX_2_LSB 0x47 |
kenjiArai | 0:86a17116e8be | 424 | #define BNO055_SIC_MTRX_2_MSB 0x48 |
kenjiArai | 0:86a17116e8be | 425 | #define BNO055_SIC_MTRX_3_LSB 0x49 |
kenjiArai | 0:86a17116e8be | 426 | #define BNO055_SIC_MTRX_3_MSB 0x4a |
kenjiArai | 0:86a17116e8be | 427 | #define BNO055_SIC_MTRX_4_LSB 0x4b |
kenjiArai | 0:86a17116e8be | 428 | #define BNO055_SIC_MTRX_4_MSB 0x4c |
kenjiArai | 0:86a17116e8be | 429 | #define BNO055_SIC_MTRX_5_LSB 0x4d |
kenjiArai | 0:86a17116e8be | 430 | #define BNO055_SIC_MTRX_5_MSB 0x4e |
kenjiArai | 0:86a17116e8be | 431 | #define BNO055_SIC_MTRX_6_LSB 0x4f |
kenjiArai | 0:86a17116e8be | 432 | #define BNO055_SIC_MTRX_6_MSB 0x50 |
kenjiArai | 0:86a17116e8be | 433 | #define BNO055_SIC_MTRX_7_LSB 0x51 |
kenjiArai | 0:86a17116e8be | 434 | #define BNO055_SIC_MTRX_7_MSB 0x52 |
kenjiArai | 0:86a17116e8be | 435 | #define BNO055_SIC_MTRX_8_LSB 0x53 |
kenjiArai | 0:86a17116e8be | 436 | #define BNO055_SIC_MTRX_8_MSB 0x54 |
kenjiArai | 0:86a17116e8be | 437 | |
kenjiArai | 0:86a17116e8be | 438 | // Accelerometer Offset registers |
kenjiArai | 0:86a17116e8be | 439 | #define ACCEL_OFFSET_X_LSB 0x55 |
kenjiArai | 0:86a17116e8be | 440 | #define ACCEL_OFFSET_X_MSB 0x56 |
kenjiArai | 0:86a17116e8be | 441 | #define ACCEL_OFFSET_Y_LSB 0x57 |
kenjiArai | 0:86a17116e8be | 442 | #define ACCEL_OFFSET_Y_MSB 0x58 |
kenjiArai | 0:86a17116e8be | 443 | #define ACCEL_OFFSET_Z_LSB 0x59 |
kenjiArai | 0:86a17116e8be | 444 | #define ACCEL_OFFSET_Z_MSB 0x5a |
kenjiArai | 0:86a17116e8be | 445 | |
kenjiArai | 0:86a17116e8be | 446 | // Magnetometer Offset registers |
kenjiArai | 0:86a17116e8be | 447 | #define MAG_OFFSET_X_LSB 0x5b |
kenjiArai | 0:86a17116e8be | 448 | #define MAG_OFFSET_X_MSB 0x5c |
kenjiArai | 0:86a17116e8be | 449 | #define MAG_OFFSET_Y_LSB 0x5d |
kenjiArai | 0:86a17116e8be | 450 | #define MAG_OFFSET_Y_MSB 0x5e |
kenjiArai | 0:86a17116e8be | 451 | #define MAG_OFFSET_Z_LSB 0x5f |
kenjiArai | 0:86a17116e8be | 452 | #define MAG_OFFSET_Z_MSB 0x60 |
kenjiArai | 0:86a17116e8be | 453 | |
kenjiArai | 0:86a17116e8be | 454 | // Gyroscope Offset registers |
kenjiArai | 0:86a17116e8be | 455 | #define GYRO_OFFSET_X_LSB 0x61 |
kenjiArai | 0:86a17116e8be | 456 | #define GYRO_OFFSET_X_MSB 0x62 |
kenjiArai | 0:86a17116e8be | 457 | #define GYRO_OFFSET_Y_LSB 0x63 |
kenjiArai | 0:86a17116e8be | 458 | #define GYRO_OFFSET_Y_MSB 0x64 |
kenjiArai | 0:86a17116e8be | 459 | #define GYRO_OFFSET_Z_LSB 0x65 |
kenjiArai | 0:86a17116e8be | 460 | #define GYRO_OFFSET_Z_MSB 0x66 |
kenjiArai | 0:86a17116e8be | 461 | |
kenjiArai | 0:86a17116e8be | 462 | // Radius registers |
kenjiArai | 0:86a17116e8be | 463 | #define ACCEL_RADIUS_LSB 0x67 |
kenjiArai | 0:86a17116e8be | 464 | #define ACCEL_RADIUS_MSB 0x68 |
kenjiArai | 0:86a17116e8be | 465 | #define MAG_RADIUS_LSB 0x69 |
kenjiArai | 0:86a17116e8be | 466 | #define MAG_RADIUS_MSB 0x6a |
kenjiArai | 0:86a17116e8be | 467 | |
kenjiArai | 0:86a17116e8be | 468 | //----- page1 --------------------------------------------- |
kenjiArai | 0:86a17116e8be | 469 | // Configuration registers |
kenjiArai | 0:86a17116e8be | 470 | #define ACCEL_CONFIG 0x08 |
kenjiArai | 0:86a17116e8be | 471 | #define MAG_CONFIG 0x09 |
kenjiArai | 0:86a17116e8be | 472 | #define GYRO_CONFIG 0x0a |
kenjiArai | 0:86a17116e8be | 473 | #define GYRO_MODE_CONFIG 0x0b |
kenjiArai | 0:86a17116e8be | 474 | #define ACCEL_SLEEP_CONFIG 0x0c |
kenjiArai | 0:86a17116e8be | 475 | #define GYRO_SLEEP_CONFIG 0x0d |
kenjiArai | 0:86a17116e8be | 476 | #define MAG_SLEEP_CONFIG 0x0e |
kenjiArai | 0:86a17116e8be | 477 | |
kenjiArai | 0:86a17116e8be | 478 | // Interrupt registers |
kenjiArai | 0:86a17116e8be | 479 | #define INT_MASK 0x0f |
kenjiArai | 0:86a17116e8be | 480 | #define INT 0x10 |
kenjiArai | 0:86a17116e8be | 481 | #define ACCEL_ANY_MOTION_THRES 0x11 |
kenjiArai | 0:86a17116e8be | 482 | #define ACCEL_INTR_SETTINGS 0x12 |
kenjiArai | 0:86a17116e8be | 483 | #define ACCEL_HIGH_G_DURN 0x13 |
kenjiArai | 0:86a17116e8be | 484 | #define ACCEL_HIGH_G_THRES 0x14 |
kenjiArai | 0:86a17116e8be | 485 | #define ACCEL_NO_MOTION_THRES 0x15 |
kenjiArai | 0:86a17116e8be | 486 | #define ACCEL_NO_MOTION_SET 0x16 |
kenjiArai | 0:86a17116e8be | 487 | #define GYRO_INTR_SETING 0x17 |
kenjiArai | 0:86a17116e8be | 488 | #define GYRO_HIGHRATE_X_SET 0x18 |
kenjiArai | 0:86a17116e8be | 489 | #define GYRO_DURN_X 0x19 |
kenjiArai | 0:86a17116e8be | 490 | #define GYRO_HIGHRATE_Y_SET 0x1a |
kenjiArai | 0:86a17116e8be | 491 | #define GYRO_DURN_Y 0x1b |
kenjiArai | 0:86a17116e8be | 492 | #define GYRO_HIGHRATE_Z_SET 0x1c |
kenjiArai | 0:86a17116e8be | 493 | #define GYRO_DURN_Z 0x1d |
kenjiArai | 0:86a17116e8be | 494 | #define GYRO_ANY_MOTION_THRES 0x1e |
kenjiArai | 0:86a17116e8be | 495 | #define GYRO_ANY_MOTION_SET 0x1f |
kenjiArai | 0:86a17116e8be | 496 | |
mrcrsch | 5:6a08a4c5b1e1 | 497 | |
mrcrsch | 5:6a08a4c5b1e1 | 498 | |
mrcrsch | 5:6a08a4c5b1e1 | 499 | |
kenjiArai | 3:0ad6f85b178f | 500 | //--------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 501 | //----- Calibration example ------------------------------- |
kenjiArai | 3:0ad6f85b178f | 502 | //--------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 503 | #if 0 |
kenjiArai | 3:0ad6f85b178f | 504 | // Calibration |
kenjiArai | 3:0ad6f85b178f | 505 | // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data |
kenjiArai | 3:0ad6f85b178f | 506 | void bno055_calbration(void){ |
kenjiArai | 3:0ad6f85b178f | 507 | uint8_t d; |
kenjiArai | 3:0ad6f85b178f | 508 | |
kenjiArai | 3:0ad6f85b178f | 509 | pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n"); |
kenjiArai | 3:0ad6f85b178f | 510 | //---------- Gyroscope Caliblation ------------------------------------------------------------ |
kenjiArai | 3:0ad6f85b178f | 511 | // (a) Place the device in a single stable position for a period of few seconds to allow the |
kenjiArai | 3:0ad6f85b178f | 512 | // gyroscope to calibrate |
kenjiArai | 3:0ad6f85b178f | 513 | pc.printf("Step1) Please wait few seconds\r\n"); |
kenjiArai | 3:0ad6f85b178f | 514 | t.start(); |
kenjiArai | 3:0ad6f85b178f | 515 | while (t.read() < 10){ |
kenjiArai | 3:0ad6f85b178f | 516 | d = imu.read_calib_status(); |
kenjiArai | 3:0ad6f85b178f | 517 | pc.printf("Calb dat = 0x%x target = 0x30(at least)\r\n", d); |
kenjiArai | 3:0ad6f85b178f | 518 | if ((d & 0x30) == 0x30){ |
kenjiArai | 3:0ad6f85b178f | 519 | break; |
kenjiArai | 3:0ad6f85b178f | 520 | } |
kenjiArai | 3:0ad6f85b178f | 521 | wait(1.0); |
kenjiArai | 3:0ad6f85b178f | 522 | } |
kenjiArai | 3:0ad6f85b178f | 523 | pc.printf("-> Step1) is done\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 524 | //---------- Magnetometer Caliblation --------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 525 | // (a) Make some random movements (for example: writing the number ‘8’ on air) until the |
kenjiArai | 3:0ad6f85b178f | 526 | // CALIB_STAT register indicates fully calibrated. |
kenjiArai | 3:0ad6f85b178f | 527 | // (b) It takes more calibration movements to get the magnetometer calibrated than in the |
kenjiArai | 3:0ad6f85b178f | 528 | // NDOF mode. |
kenjiArai | 3:0ad6f85b178f | 529 | pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n"); |
kenjiArai | 3:0ad6f85b178f | 530 | t.start(); |
kenjiArai | 3:0ad6f85b178f | 531 | while (t.read() < 30){ |
kenjiArai | 3:0ad6f85b178f | 532 | d = imu.read_calib_status(); |
kenjiArai | 3:0ad6f85b178f | 533 | pc.printf("Calb dat = 0x%x target = 0x33(at least)\r\n", d); |
kenjiArai | 3:0ad6f85b178f | 534 | if ((d & 0x03) == 0x03){ |
kenjiArai | 3:0ad6f85b178f | 535 | break; |
kenjiArai | 3:0ad6f85b178f | 536 | } |
kenjiArai | 3:0ad6f85b178f | 537 | wait(1.0); |
kenjiArai | 3:0ad6f85b178f | 538 | } |
kenjiArai | 3:0ad6f85b178f | 539 | pc.printf("-> Step2) is done\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 540 | //---------- Magnetometer Caliblation --------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 541 | // a) Place the device in 6 different stable positions for a period of few seconds |
kenjiArai | 3:0ad6f85b178f | 542 | // to allow the accelerometer to calibrate. |
kenjiArai | 3:0ad6f85b178f | 543 | // b) Make sure that there is slow movement between 2 stable positions |
kenjiArai | 3:0ad6f85b178f | 544 | // The 6 stable positions could be in any direction, but make sure that the device is |
kenjiArai | 3:0ad6f85b178f | 545 | // lying at least once perpendicular to the x, y and z axis. |
kenjiArai | 3:0ad6f85b178f | 546 | pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!"); |
kenjiArai | 3:0ad6f85b178f | 547 | pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n"); |
kenjiArai | 3:0ad6f85b178f | 548 | pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,"); |
kenjiArai | 3:0ad6f85b178f | 549 | pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n"); |
kenjiArai | 3:0ad6f85b178f | 550 | pc.printf(" If you will give up, hit any key.\r\n", d); |
kenjiArai | 3:0ad6f85b178f | 551 | t.stop(); |
kenjiArai | 3:0ad6f85b178f | 552 | while (true){ |
kenjiArai | 3:0ad6f85b178f | 553 | d = imu.read_calib_status(); |
kenjiArai | 3:0ad6f85b178f | 554 | imu.get_gravity(&gravity); |
kenjiArai | 3:0ad6f85b178f | 555 | pc.printf("Calb dat = 0x%x target = 0xff ACC:X %3.0f, Y %3.0f, Z %3.0f\r\n", |
kenjiArai | 3:0ad6f85b178f | 556 | d, gravity.x, gravity.y, gravity.z); |
kenjiArai | 3:0ad6f85b178f | 557 | if (d == 0xff){ break;} |
kenjiArai | 3:0ad6f85b178f | 558 | if (pc.readable()){ break;} |
kenjiArai | 3:0ad6f85b178f | 559 | wait(1.0); |
kenjiArai | 3:0ad6f85b178f | 560 | } |
kenjiArai | 3:0ad6f85b178f | 561 | if (imu.read_calib_status() == 0xff){ |
kenjiArai | 3:0ad6f85b178f | 562 | pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 563 | } else { |
kenjiArai | 3:0ad6f85b178f | 564 | pc.printf("-> Calibration steps are suspended!\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 565 | } |
kenjiArai | 3:0ad6f85b178f | 566 | t.stop(); |
kenjiArai | 3:0ad6f85b178f | 567 | } |
kenjiArai | 3:0ad6f85b178f | 568 | #endif |
kenjiArai | 3:0ad6f85b178f | 569 | |
kenjiArai | 0:86a17116e8be | 570 | #endif // BNO055_H |