Some improvements on mrcrsch's BNO055_fusion library
Fork of BNO055_fusion by
BNO055.h@4:9e6fead1e93e, 2015-04-16 (annotated)
- Committer:
- kenjiArai
- Date:
- Thu Apr 16 10:47:40 2015 +0000
- Revision:
- 4:9e6fead1e93e
- Parent:
- 3:0ad6f85b178f
- Child:
- 5:6a08a4c5b1e1
bug fixed, normalization
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 | |
kenjiArai | 0:86a17116e8be | 65 | ////////////// DATA TYPE DEFINITION /////////////////////// |
kenjiArai | 0:86a17116e8be | 66 | typedef struct { |
kenjiArai | 0:86a17116e8be | 67 | uint8_t chip_id; |
kenjiArai | 0:86a17116e8be | 68 | uint8_t acc_id; |
kenjiArai | 0:86a17116e8be | 69 | uint8_t mag_id; |
kenjiArai | 0:86a17116e8be | 70 | uint8_t gyr_id; |
kenjiArai | 0:86a17116e8be | 71 | uint8_t bootldr_rev_id; |
kenjiArai | 0:86a17116e8be | 72 | uint16_t sw_rev_id; |
kenjiArai | 0:86a17116e8be | 73 | } BNO055_ID_INF_TypeDef; |
kenjiArai | 0:86a17116e8be | 74 | |
kenjiArai | 0:86a17116e8be | 75 | typedef struct { |
kenjiArai | 0:86a17116e8be | 76 | double h; |
kenjiArai | 0:86a17116e8be | 77 | double r; |
kenjiArai | 0:86a17116e8be | 78 | double p; |
kenjiArai | 0:86a17116e8be | 79 | } BNO055_EULER_TypeDef; |
kenjiArai | 0:86a17116e8be | 80 | |
kenjiArai | 0:86a17116e8be | 81 | typedef struct { |
kenjiArai | 0:86a17116e8be | 82 | int16_t x; |
kenjiArai | 0:86a17116e8be | 83 | int16_t y; |
kenjiArai | 0:86a17116e8be | 84 | int16_t z; |
kenjiArai | 0:86a17116e8be | 85 | int16_t w; |
kenjiArai | 0:86a17116e8be | 86 | } BNO055_QUATERNION_TypeDef; |
kenjiArai | 0:86a17116e8be | 87 | |
kenjiArai | 0:86a17116e8be | 88 | typedef struct { |
kenjiArai | 0:86a17116e8be | 89 | double x; |
kenjiArai | 0:86a17116e8be | 90 | double y; |
kenjiArai | 0:86a17116e8be | 91 | double z; |
kenjiArai | 0:86a17116e8be | 92 | } BNO055_LIN_ACC_TypeDef; |
kenjiArai | 0:86a17116e8be | 93 | |
kenjiArai | 0:86a17116e8be | 94 | typedef struct { |
kenjiArai | 0:86a17116e8be | 95 | double x; |
kenjiArai | 0:86a17116e8be | 96 | double y; |
kenjiArai | 0:86a17116e8be | 97 | double z; |
kenjiArai | 0:86a17116e8be | 98 | } BNO055_GRAVITY_TypeDef; |
kenjiArai | 0:86a17116e8be | 99 | |
kenjiArai | 0:86a17116e8be | 100 | typedef struct { |
kenjiArai | 0:86a17116e8be | 101 | int8_t acc_chip; |
kenjiArai | 0:86a17116e8be | 102 | int8_t gyr_chip; |
kenjiArai | 0:86a17116e8be | 103 | } BNO055_TEMPERATURE_TypeDef; |
kenjiArai | 0:86a17116e8be | 104 | |
kenjiArai | 0:86a17116e8be | 105 | enum {MT_P0 = 0, MT_P1, MT_P2, MT_P3, MT_P4, MT_P5, MT_P6, MT_P7}; |
kenjiArai | 0:86a17116e8be | 106 | |
kenjiArai | 0:86a17116e8be | 107 | /** Interface for Bosch Sensortec Intelligent 9-axis absolute orientation sensor |
kenjiArai | 0:86a17116e8be | 108 | * Chip: BNO055 |
kenjiArai | 0:86a17116e8be | 109 | * |
kenjiArai | 0:86a17116e8be | 110 | * @code |
kenjiArai | 0:86a17116e8be | 111 | * #include "mbed.h" |
kenjiArai | 0:86a17116e8be | 112 | * #include "BNO055.h" |
kenjiArai | 0:86a17116e8be | 113 | * |
kenjiArai | 0:86a17116e8be | 114 | * Serial pc(USBTX,USBRX); |
kenjiArai | 0:86a17116e8be | 115 | * I2C i2c(PB_9, PB_8); // SDA, SCL |
kenjiArai | 1:cb7e19c0a702 | 116 | * BNO055 imu(i2c, PA_8); // Reset |
kenjiArai | 0:86a17116e8be | 117 | * |
kenjiArai | 0:86a17116e8be | 118 | * BNO055_ID_INF_TypeDef bno055_id_inf; |
kenjiArai | 0:86a17116e8be | 119 | * BNO055_EULER_TypeDef euler_angles; |
kenjiArai | 0:86a17116e8be | 120 | * |
kenjiArai | 0:86a17116e8be | 121 | * int main() { |
kenjiArai | 0:86a17116e8be | 122 | * pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
kenjiArai | 0:86a17116e8be | 123 | * if (imu.chip_ready() == 0){ |
kenjiArai | 0:86a17116e8be | 124 | * pc.printf("Bosch BNO055 is NOT avirable!!\r\n"); |
kenjiArai | 0:86a17116e8be | 125 | * } |
kenjiArai | 0:86a17116e8be | 126 | * imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 0:86a17116e8be | 127 | * 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 | 128 | * bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, |
kenjiArai | 0:86a17116e8be | 129 | * bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
kenjiArai | 0:86a17116e8be | 130 | * while(1) { |
kenjiArai | 0:86a17116e8be | 131 | * imu.get_Euler_Angles(&euler_angles); |
kenjiArai | 0:86a17116e8be | 132 | * pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg]\r\n", |
kenjiArai | 0:86a17116e8be | 133 | * euler_angles.h, euler_angles.r, euler_angles.p); |
kenjiArai | 0:86a17116e8be | 134 | * wait(0.5); |
kenjiArai | 0:86a17116e8be | 135 | * } |
kenjiArai | 0:86a17116e8be | 136 | * } |
kenjiArai | 0:86a17116e8be | 137 | * @endcode |
kenjiArai | 0:86a17116e8be | 138 | */ |
kenjiArai | 0:86a17116e8be | 139 | |
kenjiArai | 0:86a17116e8be | 140 | class BNO055 |
kenjiArai | 0:86a17116e8be | 141 | { |
kenjiArai | 0:86a17116e8be | 142 | public: |
kenjiArai | 0:86a17116e8be | 143 | /** Configure data pin |
kenjiArai | 0:86a17116e8be | 144 | * @param data SDA and SCL pins |
kenjiArai | 0:86a17116e8be | 145 | * @param device address |
kenjiArai | 0:86a17116e8be | 146 | */ |
kenjiArai | 1:cb7e19c0a702 | 147 | BNO055(PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode); |
kenjiArai | 0:86a17116e8be | 148 | |
kenjiArai | 0:86a17116e8be | 149 | /** Configure data pin |
kenjiArai | 0:86a17116e8be | 150 | * @param data SDA and SCL pins |
kenjiArai | 0:86a17116e8be | 151 | * @param Other parameters are set default data |
kenjiArai | 0:86a17116e8be | 152 | */ |
kenjiArai | 1:cb7e19c0a702 | 153 | BNO055(PinName p_sda, PinName p_scl, PinName p_reset); |
kenjiArai | 0:86a17116e8be | 154 | |
kenjiArai | 0:86a17116e8be | 155 | /** Configure data pin (with other devices on I2C line) |
kenjiArai | 0:86a17116e8be | 156 | * @param I2C previous definition |
kenjiArai | 0:86a17116e8be | 157 | * @param device address |
kenjiArai | 0:86a17116e8be | 158 | */ |
kenjiArai | 1:cb7e19c0a702 | 159 | BNO055(I2C& p_i2c, PinName p_reset, uint8_t addr, uint8_t mode); |
kenjiArai | 0:86a17116e8be | 160 | |
kenjiArai | 0:86a17116e8be | 161 | /** Configure data pin (with other devices on I2C line) |
kenjiArai | 0:86a17116e8be | 162 | * @param I2C previous definition |
kenjiArai | 0:86a17116e8be | 163 | * @param Other parameters are set default data |
kenjiArai | 0:86a17116e8be | 164 | */ |
kenjiArai | 1:cb7e19c0a702 | 165 | BNO055(I2C& p_i2c, PinName p_reset); |
kenjiArai | 0:86a17116e8be | 166 | |
kenjiArai | 0:86a17116e8be | 167 | /** Get Euler Angles |
kenjiArai | 0:86a17116e8be | 168 | * @param double type of 3D data address |
kenjiArai | 0:86a17116e8be | 169 | */ |
kenjiArai | 0:86a17116e8be | 170 | void get_Euler_Angles(BNO055_EULER_TypeDef *el); |
kenjiArai | 0:86a17116e8be | 171 | |
kenjiArai | 0:86a17116e8be | 172 | /** Get Quaternion XYZ&W |
kenjiArai | 0:86a17116e8be | 173 | * @param int16_t type of 4D data address |
kenjiArai | 0:86a17116e8be | 174 | */ |
kenjiArai | 0:86a17116e8be | 175 | void get_quaternion(BNO055_QUATERNION_TypeDef *qua); |
kenjiArai | 0:86a17116e8be | 176 | |
kenjiArai | 0:86a17116e8be | 177 | /** Get Linear accel data |
kenjiArai | 0:86a17116e8be | 178 | * @param double type of 3D data address |
kenjiArai | 0:86a17116e8be | 179 | */ |
kenjiArai | 0:86a17116e8be | 180 | void get_linear_accel(BNO055_LIN_ACC_TypeDef *la); |
kenjiArai | 0:86a17116e8be | 181 | |
kenjiArai | 0:86a17116e8be | 182 | /** Get Gravity data |
kenjiArai | 0:86a17116e8be | 183 | * @param double type of 3D data address |
kenjiArai | 0:86a17116e8be | 184 | */ |
kenjiArai | 0:86a17116e8be | 185 | void get_gravity(BNO055_GRAVITY_TypeDef *gr); |
kenjiArai | 0:86a17116e8be | 186 | |
kenjiArai | 0:86a17116e8be | 187 | /** Get Chip temperature data both Acc & Gyro |
kenjiArai | 0:86a17116e8be | 188 | * @param int8_t type of data address |
kenjiArai | 0:86a17116e8be | 189 | */ |
kenjiArai | 0:86a17116e8be | 190 | void get_chip_temperature(BNO055_TEMPERATURE_TypeDef *tmp); |
kenjiArai | 0:86a17116e8be | 191 | |
kenjiArai | 0:86a17116e8be | 192 | /** Change fusion mode |
kenjiArai | 0:86a17116e8be | 193 | * @param fusion mode |
kenjiArai | 0:86a17116e8be | 194 | * @return none |
kenjiArai | 0:86a17116e8be | 195 | */ |
kenjiArai | 0:86a17116e8be | 196 | void change_fusion_mode(uint8_t mode); |
kenjiArai | 0:86a17116e8be | 197 | |
kenjiArai | 0:86a17116e8be | 198 | /** Set Mouting position |
kenjiArai | 0:86a17116e8be | 199 | * Please make sure your mounting direction of BNO055 chip |
kenjiArai | 0:86a17116e8be | 200 | * refrence: BNO055 data sheet BST-BNO055-DS000-12 3.4 Axis remap |
kenjiArai | 0:86a17116e8be | 201 | * @param Set P0 to P7 mounting position data |
kenjiArai | 0:86a17116e8be | 202 | * @return none |
kenjiArai | 0:86a17116e8be | 203 | */ |
kenjiArai | 0:86a17116e8be | 204 | void set_mounting_position(uint8_t position); |
kenjiArai | 0:86a17116e8be | 205 | |
kenjiArai | 0:86a17116e8be | 206 | /** Read BNO055 ID information |
kenjiArai | 0:86a17116e8be | 207 | * @param ID information address |
kenjiArai | 0:86a17116e8be | 208 | * @return none |
kenjiArai | 0:86a17116e8be | 209 | */ |
kenjiArai | 0:86a17116e8be | 210 | void read_id_inf(BNO055_ID_INF_TypeDef *id); |
kenjiArai | 0:86a17116e8be | 211 | |
kenjiArai | 0:86a17116e8be | 212 | /** Check chip is avairable or not |
kenjiArai | 0:86a17116e8be | 213 | * @param none |
kenjiArai | 0:86a17116e8be | 214 | * @return OK = 1, NG = 0; |
kenjiArai | 0:86a17116e8be | 215 | */ |
kenjiArai | 0:86a17116e8be | 216 | uint8_t chip_ready(void); |
kenjiArai | 0:86a17116e8be | 217 | |
kenjiArai | 2:0f225b686cd5 | 218 | /** Read calibration status |
kenjiArai | 2:0f225b686cd5 | 219 | * @param none |
kenjiArai | 2:0f225b686cd5 | 220 | * @return SYS(7:6),GYR(5:4),ACC(3:2),MAG(1:0) 3 = Calibrated, 0= not yet |
kenjiArai | 2:0f225b686cd5 | 221 | */ |
kenjiArai | 2:0f225b686cd5 | 222 | uint8_t read_calib_status(void); |
kenjiArai | 2:0f225b686cd5 | 223 | |
kenjiArai | 1:cb7e19c0a702 | 224 | /** Reset |
kenjiArai | 1:cb7e19c0a702 | 225 | * @param none |
kenjiArai | 3:0ad6f85b178f | 226 | * @return 0 = sucess, 1 = Not available chip |
kenjiArai | 1:cb7e19c0a702 | 227 | */ |
kenjiArai | 1:cb7e19c0a702 | 228 | uint8_t reset(void); |
kenjiArai | 1:cb7e19c0a702 | 229 | |
kenjiArai | 0:86a17116e8be | 230 | /** Set I2C clock frequency |
kenjiArai | 0:86a17116e8be | 231 | * @param freq. |
kenjiArai | 0:86a17116e8be | 232 | * @return none |
kenjiArai | 0:86a17116e8be | 233 | */ |
kenjiArai | 0:86a17116e8be | 234 | void frequency(int hz); |
kenjiArai | 0:86a17116e8be | 235 | |
kenjiArai | 0:86a17116e8be | 236 | /** Read page 0 register |
kenjiArai | 0:86a17116e8be | 237 | * @param register's address |
kenjiArai | 0:86a17116e8be | 238 | * @return register data |
kenjiArai | 0:86a17116e8be | 239 | */ |
kenjiArai | 0:86a17116e8be | 240 | uint8_t read_reg0(uint8_t addr); |
kenjiArai | 0:86a17116e8be | 241 | |
kenjiArai | 0:86a17116e8be | 242 | /** Write page 0 register |
kenjiArai | 0:86a17116e8be | 243 | * @param register's address |
kenjiArai | 0:86a17116e8be | 244 | * @param data |
kenjiArai | 0:86a17116e8be | 245 | * @return register data |
kenjiArai | 0:86a17116e8be | 246 | */ |
kenjiArai | 0:86a17116e8be | 247 | uint8_t write_reg0(uint8_t addr, uint8_t data); |
kenjiArai | 0:86a17116e8be | 248 | |
kenjiArai | 0:86a17116e8be | 249 | /** Read page 1 register |
kenjiArai | 0:86a17116e8be | 250 | * @param register's address |
kenjiArai | 0:86a17116e8be | 251 | * @return register data |
kenjiArai | 0:86a17116e8be | 252 | */ |
kenjiArai | 0:86a17116e8be | 253 | uint8_t read_reg1(uint8_t addr); |
kenjiArai | 0:86a17116e8be | 254 | |
kenjiArai | 0:86a17116e8be | 255 | /** Write page 1 register |
kenjiArai | 0:86a17116e8be | 256 | * @param register's address |
kenjiArai | 0:86a17116e8be | 257 | * @param data |
kenjiArai | 0:86a17116e8be | 258 | * @return register data |
kenjiArai | 0:86a17116e8be | 259 | */ |
kenjiArai | 0:86a17116e8be | 260 | uint8_t write_reg1(uint8_t addr, uint8_t data); |
kenjiArai | 0:86a17116e8be | 261 | |
kenjiArai | 0:86a17116e8be | 262 | protected: |
kenjiArai | 0:86a17116e8be | 263 | void initialize(void); |
kenjiArai | 0:86a17116e8be | 264 | void check_id(void); |
kenjiArai | 0:86a17116e8be | 265 | void set_initial_dt_to_regs(void); |
kenjiArai | 0:86a17116e8be | 266 | void unit_selection(void); |
kenjiArai | 0:86a17116e8be | 267 | uint8_t check_operating_mode(void); |
kenjiArai | 0:86a17116e8be | 268 | uint8_t select_page(uint8_t page); |
kenjiArai | 0:86a17116e8be | 269 | |
kenjiArai | 0:86a17116e8be | 270 | I2C _i2c; |
kenjiArai | 1:cb7e19c0a702 | 271 | DigitalOut _res; |
kenjiArai | 0:86a17116e8be | 272 | |
kenjiArai | 0:86a17116e8be | 273 | private: |
kenjiArai | 0:86a17116e8be | 274 | char dt[10]; // working buffer |
kenjiArai | 0:86a17116e8be | 275 | uint8_t chip_addr; |
kenjiArai | 0:86a17116e8be | 276 | uint8_t chip_mode; |
kenjiArai | 3:0ad6f85b178f | 277 | uint8_t ready_flag; |
kenjiArai | 3:0ad6f85b178f | 278 | uint8_t page_flag; |
kenjiArai | 0:86a17116e8be | 279 | |
kenjiArai | 0:86a17116e8be | 280 | uint8_t chip_id; |
kenjiArai | 0:86a17116e8be | 281 | uint8_t acc_id; |
kenjiArai | 0:86a17116e8be | 282 | uint8_t mag_id; |
kenjiArai | 0:86a17116e8be | 283 | uint8_t gyr_id; |
kenjiArai | 0:86a17116e8be | 284 | uint8_t bootldr_rev_id; |
kenjiArai | 0:86a17116e8be | 285 | uint16_t sw_rev_id; |
kenjiArai | 0:86a17116e8be | 286 | |
kenjiArai | 0:86a17116e8be | 287 | }; |
kenjiArai | 0:86a17116e8be | 288 | |
kenjiArai | 0:86a17116e8be | 289 | //--------------------------------------------------------- |
kenjiArai | 0:86a17116e8be | 290 | //----- Register's definition ----------------------------- |
kenjiArai | 0:86a17116e8be | 291 | //--------------------------------------------------------- |
kenjiArai | 0:86a17116e8be | 292 | // Page id register definition |
kenjiArai | 0:86a17116e8be | 293 | #define BNO055_PAGE_ID 0x07 |
kenjiArai | 0:86a17116e8be | 294 | |
kenjiArai | 0:86a17116e8be | 295 | //----- page0 --------------------------------------------- |
kenjiArai | 0:86a17116e8be | 296 | #define BNO055_CHIP_ID 0x00 |
kenjiArai | 0:86a17116e8be | 297 | #define BNO055_ACCEL_REV_ID 0x01 |
kenjiArai | 0:86a17116e8be | 298 | #define BNO055_MAG_REV_ID 0x02 |
kenjiArai | 0:86a17116e8be | 299 | #define BNO055_GYRO_REV_ID 0x03 |
kenjiArai | 0:86a17116e8be | 300 | #define BNO055_SW_REV_ID_LSB 0x04 |
kenjiArai | 0:86a17116e8be | 301 | #define BNO055_SW_REV_ID_MSB 0x05 |
kenjiArai | 0:86a17116e8be | 302 | #define BNO055_BL_REV_ID 0x06 |
kenjiArai | 0:86a17116e8be | 303 | |
kenjiArai | 0:86a17116e8be | 304 | // Accel data register*/ |
kenjiArai | 0:86a17116e8be | 305 | #define BNO055_ACC_X_LSB 0x08 |
kenjiArai | 0:86a17116e8be | 306 | #define BNO055_ACC_X_MSB 0x09 |
kenjiArai | 0:86a17116e8be | 307 | #define BNO055_ACC_Y_LSB 0x0a |
kenjiArai | 0:86a17116e8be | 308 | #define BNO055_ACC_Y_MSB 0x0b |
kenjiArai | 0:86a17116e8be | 309 | #define BNO055_ACC_Z_LSB 0x0c |
kenjiArai | 0:86a17116e8be | 310 | #define BNO055_ACC_Z_MSB 0x0d |
kenjiArai | 0:86a17116e8be | 311 | |
kenjiArai | 0:86a17116e8be | 312 | // Mag data register |
kenjiArai | 0:86a17116e8be | 313 | #define BNO055_MAG_X_LSB 0x0e |
kenjiArai | 0:86a17116e8be | 314 | #define BNO055_MAG_X_MSB 0x0f |
kenjiArai | 0:86a17116e8be | 315 | #define BNO055_MAG_Y_LSB 0x10 |
kenjiArai | 0:86a17116e8be | 316 | #define BNO055_MAG_Y_MSB 0x11 |
kenjiArai | 0:86a17116e8be | 317 | #define BNO055_MAG_Z_LSB 0x12 |
kenjiArai | 0:86a17116e8be | 318 | #define BNO055_MAG_Z_MSB 0x13 |
kenjiArai | 0:86a17116e8be | 319 | |
kenjiArai | 0:86a17116e8be | 320 | // Gyro data registers |
kenjiArai | 0:86a17116e8be | 321 | #define BNO055_GYR_X_LSB 0x14 |
kenjiArai | 0:86a17116e8be | 322 | #define BNO055_GYR_X_MSB 0x15 |
kenjiArai | 0:86a17116e8be | 323 | #define BNO055_GYR_Y_LSB 0x16 |
kenjiArai | 0:86a17116e8be | 324 | #define BNO055_GYR_Y_MSB 0x17 |
kenjiArai | 0:86a17116e8be | 325 | #define BNO055_GYR_Z_LSB 0x18 |
kenjiArai | 0:86a17116e8be | 326 | #define BNO055_GYR_Z_MSB 0x19 |
kenjiArai | 0:86a17116e8be | 327 | |
kenjiArai | 0:86a17116e8be | 328 | // Euler data registers |
kenjiArai | 0:86a17116e8be | 329 | #define BNO055_EULER_H_LSB 0x1a |
kenjiArai | 0:86a17116e8be | 330 | #define BNO055_EULER_H_MSB 0x1b |
kenjiArai | 0:86a17116e8be | 331 | |
kenjiArai | 0:86a17116e8be | 332 | #define BNO055_EULER_R_LSB 0x1c |
kenjiArai | 0:86a17116e8be | 333 | #define BNO055_EULER_R_MSB 0x1d |
kenjiArai | 0:86a17116e8be | 334 | |
kenjiArai | 0:86a17116e8be | 335 | #define BNO055_EULER_P_LSB 0x1e |
kenjiArai | 0:86a17116e8be | 336 | #define BNO055_EULER_P_MSB 0x1f |
kenjiArai | 0:86a17116e8be | 337 | |
kenjiArai | 0:86a17116e8be | 338 | // Quaternion data registers |
kenjiArai | 0:86a17116e8be | 339 | #define BNO055_QUATERNION_W_LSB 0x20 |
kenjiArai | 0:86a17116e8be | 340 | #define BNO055_QUATERNION_W_MSB 0x21 |
kenjiArai | 0:86a17116e8be | 341 | #define BNO055_QUATERNION_X_LSB 0x22 |
kenjiArai | 0:86a17116e8be | 342 | #define BNO055_QUATERNION_X_MSB 0x23 |
kenjiArai | 0:86a17116e8be | 343 | #define BNO055_QUATERNION_Y_LSB 0x24 |
kenjiArai | 0:86a17116e8be | 344 | #define BNO055_QUATERNION_Y_MSB 0x25 |
kenjiArai | 0:86a17116e8be | 345 | #define BNO055_QUATERNION_Z_LSB 0x26 |
kenjiArai | 0:86a17116e8be | 346 | #define BNO055_QUATERNION_Z_MSB 0x27 |
kenjiArai | 0:86a17116e8be | 347 | |
kenjiArai | 0:86a17116e8be | 348 | // Linear acceleration data registers |
kenjiArai | 0:86a17116e8be | 349 | #define BNO055_LINEAR_ACC_X_LSB 0x28 |
kenjiArai | 0:86a17116e8be | 350 | #define BNO055_LINEAR_ACC_X_MSB 0x29 |
kenjiArai | 0:86a17116e8be | 351 | #define BNO055_LINEAR_ACC_Y_LSB 0x2a |
kenjiArai | 0:86a17116e8be | 352 | #define BNO055_LINEAR_ACC_Y_MSB 0x2b |
kenjiArai | 0:86a17116e8be | 353 | #define BNO055_LINEAR_ACC_Z_LSB 0x2c |
kenjiArai | 0:86a17116e8be | 354 | #define BNO055_LINEAR_ACC_Z_MSB 0x2d |
kenjiArai | 0:86a17116e8be | 355 | |
kenjiArai | 0:86a17116e8be | 356 | // Gravity data registers |
kenjiArai | 0:86a17116e8be | 357 | #define BNO055_GRAVITY_X_LSB 0x2e |
kenjiArai | 0:86a17116e8be | 358 | #define BNO055_GRAVITY_X_MSB 0x2f |
kenjiArai | 0:86a17116e8be | 359 | #define BNO055_GRAVITY_Y_LSB 0x30 |
kenjiArai | 0:86a17116e8be | 360 | #define BNO055_GRAVITY_Y_MSB 0x31 |
kenjiArai | 0:86a17116e8be | 361 | #define BNO055_GRAVITY_Z_LSB 0x32 |
kenjiArai | 0:86a17116e8be | 362 | #define BNO055_GRAVITY_Z_MSB 0x33 |
kenjiArai | 0:86a17116e8be | 363 | |
kenjiArai | 0:86a17116e8be | 364 | // Temperature data register |
kenjiArai | 0:86a17116e8be | 365 | #define BNO055_TEMP 0x34 |
kenjiArai | 0:86a17116e8be | 366 | |
kenjiArai | 0:86a17116e8be | 367 | // Status registers |
kenjiArai | 0:86a17116e8be | 368 | #define BNO055_CALIB_STAT 0x35 |
kenjiArai | 0:86a17116e8be | 369 | #define BNO055_SELFTEST_RESULT 0x36 |
kenjiArai | 0:86a17116e8be | 370 | #define BNO055_INTR_STAT 0x37 |
kenjiArai | 0:86a17116e8be | 371 | #define BNO055_SYS_CLK_STAT 0x38 |
kenjiArai | 0:86a17116e8be | 372 | #define BNO055_SYS_STAT 0x39 |
kenjiArai | 0:86a17116e8be | 373 | #define BNO055_SYS_ERR 0x3a |
kenjiArai | 0:86a17116e8be | 374 | |
kenjiArai | 0:86a17116e8be | 375 | // Unit selection register |
kenjiArai | 0:86a17116e8be | 376 | #define BNO055_UNIT_SEL 0x3b |
kenjiArai | 0:86a17116e8be | 377 | #define BNO055_DATA_SELECT 0x3c |
kenjiArai | 0:86a17116e8be | 378 | |
kenjiArai | 0:86a17116e8be | 379 | // Mode registers |
kenjiArai | 0:86a17116e8be | 380 | #define BNO055_OPR_MODE 0x3d |
kenjiArai | 0:86a17116e8be | 381 | #define BNO055_PWR_MODE 0x3e |
kenjiArai | 0:86a17116e8be | 382 | #define BNO055_SYS_TRIGGER 0x3f |
kenjiArai | 0:86a17116e8be | 383 | #define BNO055_TEMP_SOURCE 0x40 |
kenjiArai | 0:86a17116e8be | 384 | |
kenjiArai | 0:86a17116e8be | 385 | // Axis remap registers |
kenjiArai | 0:86a17116e8be | 386 | #define BNO055_AXIS_MAP_CONFIG 0x41 |
kenjiArai | 0:86a17116e8be | 387 | #define BNO055_AXIS_MAP_SIGN 0x42 |
kenjiArai | 0:86a17116e8be | 388 | |
kenjiArai | 0:86a17116e8be | 389 | // SIC registers |
kenjiArai | 0:86a17116e8be | 390 | #define BNO055_SIC_MTRX_0_LSB 0x43 |
kenjiArai | 0:86a17116e8be | 391 | #define BNO055_SIC_MTRX_0_MSB 0x44 |
kenjiArai | 0:86a17116e8be | 392 | #define BNO055_SIC_MTRX_1_LSB 0x45 |
kenjiArai | 0:86a17116e8be | 393 | #define BNO055_SIC_MTRX_1_MSB 0x46 |
kenjiArai | 0:86a17116e8be | 394 | #define BNO055_SIC_MTRX_2_LSB 0x47 |
kenjiArai | 0:86a17116e8be | 395 | #define BNO055_SIC_MTRX_2_MSB 0x48 |
kenjiArai | 0:86a17116e8be | 396 | #define BNO055_SIC_MTRX_3_LSB 0x49 |
kenjiArai | 0:86a17116e8be | 397 | #define BNO055_SIC_MTRX_3_MSB 0x4a |
kenjiArai | 0:86a17116e8be | 398 | #define BNO055_SIC_MTRX_4_LSB 0x4b |
kenjiArai | 0:86a17116e8be | 399 | #define BNO055_SIC_MTRX_4_MSB 0x4c |
kenjiArai | 0:86a17116e8be | 400 | #define BNO055_SIC_MTRX_5_LSB 0x4d |
kenjiArai | 0:86a17116e8be | 401 | #define BNO055_SIC_MTRX_5_MSB 0x4e |
kenjiArai | 0:86a17116e8be | 402 | #define BNO055_SIC_MTRX_6_LSB 0x4f |
kenjiArai | 0:86a17116e8be | 403 | #define BNO055_SIC_MTRX_6_MSB 0x50 |
kenjiArai | 0:86a17116e8be | 404 | #define BNO055_SIC_MTRX_7_LSB 0x51 |
kenjiArai | 0:86a17116e8be | 405 | #define BNO055_SIC_MTRX_7_MSB 0x52 |
kenjiArai | 0:86a17116e8be | 406 | #define BNO055_SIC_MTRX_8_LSB 0x53 |
kenjiArai | 0:86a17116e8be | 407 | #define BNO055_SIC_MTRX_8_MSB 0x54 |
kenjiArai | 0:86a17116e8be | 408 | |
kenjiArai | 0:86a17116e8be | 409 | // Accelerometer Offset registers |
kenjiArai | 0:86a17116e8be | 410 | #define ACCEL_OFFSET_X_LSB 0x55 |
kenjiArai | 0:86a17116e8be | 411 | #define ACCEL_OFFSET_X_MSB 0x56 |
kenjiArai | 0:86a17116e8be | 412 | #define ACCEL_OFFSET_Y_LSB 0x57 |
kenjiArai | 0:86a17116e8be | 413 | #define ACCEL_OFFSET_Y_MSB 0x58 |
kenjiArai | 0:86a17116e8be | 414 | #define ACCEL_OFFSET_Z_LSB 0x59 |
kenjiArai | 0:86a17116e8be | 415 | #define ACCEL_OFFSET_Z_MSB 0x5a |
kenjiArai | 0:86a17116e8be | 416 | |
kenjiArai | 0:86a17116e8be | 417 | // Magnetometer Offset registers |
kenjiArai | 0:86a17116e8be | 418 | #define MAG_OFFSET_X_LSB 0x5b |
kenjiArai | 0:86a17116e8be | 419 | #define MAG_OFFSET_X_MSB 0x5c |
kenjiArai | 0:86a17116e8be | 420 | #define MAG_OFFSET_Y_LSB 0x5d |
kenjiArai | 0:86a17116e8be | 421 | #define MAG_OFFSET_Y_MSB 0x5e |
kenjiArai | 0:86a17116e8be | 422 | #define MAG_OFFSET_Z_LSB 0x5f |
kenjiArai | 0:86a17116e8be | 423 | #define MAG_OFFSET_Z_MSB 0x60 |
kenjiArai | 0:86a17116e8be | 424 | |
kenjiArai | 0:86a17116e8be | 425 | // Gyroscope Offset registers |
kenjiArai | 0:86a17116e8be | 426 | #define GYRO_OFFSET_X_LSB 0x61 |
kenjiArai | 0:86a17116e8be | 427 | #define GYRO_OFFSET_X_MSB 0x62 |
kenjiArai | 0:86a17116e8be | 428 | #define GYRO_OFFSET_Y_LSB 0x63 |
kenjiArai | 0:86a17116e8be | 429 | #define GYRO_OFFSET_Y_MSB 0x64 |
kenjiArai | 0:86a17116e8be | 430 | #define GYRO_OFFSET_Z_LSB 0x65 |
kenjiArai | 0:86a17116e8be | 431 | #define GYRO_OFFSET_Z_MSB 0x66 |
kenjiArai | 0:86a17116e8be | 432 | |
kenjiArai | 0:86a17116e8be | 433 | // Radius registers |
kenjiArai | 0:86a17116e8be | 434 | #define ACCEL_RADIUS_LSB 0x67 |
kenjiArai | 0:86a17116e8be | 435 | #define ACCEL_RADIUS_MSB 0x68 |
kenjiArai | 0:86a17116e8be | 436 | #define MAG_RADIUS_LSB 0x69 |
kenjiArai | 0:86a17116e8be | 437 | #define MAG_RADIUS_MSB 0x6a |
kenjiArai | 0:86a17116e8be | 438 | |
kenjiArai | 0:86a17116e8be | 439 | //----- page1 --------------------------------------------- |
kenjiArai | 0:86a17116e8be | 440 | // Configuration registers |
kenjiArai | 0:86a17116e8be | 441 | #define ACCEL_CONFIG 0x08 |
kenjiArai | 0:86a17116e8be | 442 | #define MAG_CONFIG 0x09 |
kenjiArai | 0:86a17116e8be | 443 | #define GYRO_CONFIG 0x0a |
kenjiArai | 0:86a17116e8be | 444 | #define GYRO_MODE_CONFIG 0x0b |
kenjiArai | 0:86a17116e8be | 445 | #define ACCEL_SLEEP_CONFIG 0x0c |
kenjiArai | 0:86a17116e8be | 446 | #define GYRO_SLEEP_CONFIG 0x0d |
kenjiArai | 0:86a17116e8be | 447 | #define MAG_SLEEP_CONFIG 0x0e |
kenjiArai | 0:86a17116e8be | 448 | |
kenjiArai | 0:86a17116e8be | 449 | // Interrupt registers |
kenjiArai | 0:86a17116e8be | 450 | #define INT_MASK 0x0f |
kenjiArai | 0:86a17116e8be | 451 | #define INT 0x10 |
kenjiArai | 0:86a17116e8be | 452 | #define ACCEL_ANY_MOTION_THRES 0x11 |
kenjiArai | 0:86a17116e8be | 453 | #define ACCEL_INTR_SETTINGS 0x12 |
kenjiArai | 0:86a17116e8be | 454 | #define ACCEL_HIGH_G_DURN 0x13 |
kenjiArai | 0:86a17116e8be | 455 | #define ACCEL_HIGH_G_THRES 0x14 |
kenjiArai | 0:86a17116e8be | 456 | #define ACCEL_NO_MOTION_THRES 0x15 |
kenjiArai | 0:86a17116e8be | 457 | #define ACCEL_NO_MOTION_SET 0x16 |
kenjiArai | 0:86a17116e8be | 458 | #define GYRO_INTR_SETING 0x17 |
kenjiArai | 0:86a17116e8be | 459 | #define GYRO_HIGHRATE_X_SET 0x18 |
kenjiArai | 0:86a17116e8be | 460 | #define GYRO_DURN_X 0x19 |
kenjiArai | 0:86a17116e8be | 461 | #define GYRO_HIGHRATE_Y_SET 0x1a |
kenjiArai | 0:86a17116e8be | 462 | #define GYRO_DURN_Y 0x1b |
kenjiArai | 0:86a17116e8be | 463 | #define GYRO_HIGHRATE_Z_SET 0x1c |
kenjiArai | 0:86a17116e8be | 464 | #define GYRO_DURN_Z 0x1d |
kenjiArai | 0:86a17116e8be | 465 | #define GYRO_ANY_MOTION_THRES 0x1e |
kenjiArai | 0:86a17116e8be | 466 | #define GYRO_ANY_MOTION_SET 0x1f |
kenjiArai | 0:86a17116e8be | 467 | |
kenjiArai | 3:0ad6f85b178f | 468 | //--------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 469 | //----- Calibration example ------------------------------- |
kenjiArai | 3:0ad6f85b178f | 470 | //--------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 471 | #if 0 |
kenjiArai | 3:0ad6f85b178f | 472 | // Calibration |
kenjiArai | 3:0ad6f85b178f | 473 | // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data |
kenjiArai | 3:0ad6f85b178f | 474 | void bno055_calbration(void){ |
kenjiArai | 3:0ad6f85b178f | 475 | uint8_t d; |
kenjiArai | 3:0ad6f85b178f | 476 | |
kenjiArai | 3:0ad6f85b178f | 477 | pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n"); |
kenjiArai | 3:0ad6f85b178f | 478 | //---------- Gyroscope Caliblation ------------------------------------------------------------ |
kenjiArai | 3:0ad6f85b178f | 479 | // (a) Place the device in a single stable position for a period of few seconds to allow the |
kenjiArai | 3:0ad6f85b178f | 480 | // gyroscope to calibrate |
kenjiArai | 3:0ad6f85b178f | 481 | pc.printf("Step1) Please wait few seconds\r\n"); |
kenjiArai | 3:0ad6f85b178f | 482 | t.start(); |
kenjiArai | 3:0ad6f85b178f | 483 | while (t.read() < 10){ |
kenjiArai | 3:0ad6f85b178f | 484 | d = imu.read_calib_status(); |
kenjiArai | 3:0ad6f85b178f | 485 | pc.printf("Calb dat = 0x%x target = 0x30(at least)\r\n", d); |
kenjiArai | 3:0ad6f85b178f | 486 | if ((d & 0x30) == 0x30){ |
kenjiArai | 3:0ad6f85b178f | 487 | break; |
kenjiArai | 3:0ad6f85b178f | 488 | } |
kenjiArai | 3:0ad6f85b178f | 489 | wait(1.0); |
kenjiArai | 3:0ad6f85b178f | 490 | } |
kenjiArai | 3:0ad6f85b178f | 491 | pc.printf("-> Step1) is done\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 492 | //---------- Magnetometer Caliblation --------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 493 | // (a) Make some random movements (for example: writing the number ‘8’ on air) until the |
kenjiArai | 3:0ad6f85b178f | 494 | // CALIB_STAT register indicates fully calibrated. |
kenjiArai | 3:0ad6f85b178f | 495 | // (b) It takes more calibration movements to get the magnetometer calibrated than in the |
kenjiArai | 3:0ad6f85b178f | 496 | // NDOF mode. |
kenjiArai | 3:0ad6f85b178f | 497 | pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n"); |
kenjiArai | 3:0ad6f85b178f | 498 | t.start(); |
kenjiArai | 3:0ad6f85b178f | 499 | while (t.read() < 30){ |
kenjiArai | 3:0ad6f85b178f | 500 | d = imu.read_calib_status(); |
kenjiArai | 3:0ad6f85b178f | 501 | pc.printf("Calb dat = 0x%x target = 0x33(at least)\r\n", d); |
kenjiArai | 3:0ad6f85b178f | 502 | if ((d & 0x03) == 0x03){ |
kenjiArai | 3:0ad6f85b178f | 503 | break; |
kenjiArai | 3:0ad6f85b178f | 504 | } |
kenjiArai | 3:0ad6f85b178f | 505 | wait(1.0); |
kenjiArai | 3:0ad6f85b178f | 506 | } |
kenjiArai | 3:0ad6f85b178f | 507 | pc.printf("-> Step2) is done\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 508 | //---------- Magnetometer Caliblation --------------------------------------------------------- |
kenjiArai | 3:0ad6f85b178f | 509 | // a) Place the device in 6 different stable positions for a period of few seconds |
kenjiArai | 3:0ad6f85b178f | 510 | // to allow the accelerometer to calibrate. |
kenjiArai | 3:0ad6f85b178f | 511 | // b) Make sure that there is slow movement between 2 stable positions |
kenjiArai | 3:0ad6f85b178f | 512 | // The 6 stable positions could be in any direction, but make sure that the device is |
kenjiArai | 3:0ad6f85b178f | 513 | // lying at least once perpendicular to the x, y and z axis. |
kenjiArai | 3:0ad6f85b178f | 514 | pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!"); |
kenjiArai | 3:0ad6f85b178f | 515 | pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n"); |
kenjiArai | 3:0ad6f85b178f | 516 | pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,"); |
kenjiArai | 3:0ad6f85b178f | 517 | pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n"); |
kenjiArai | 3:0ad6f85b178f | 518 | pc.printf(" If you will give up, hit any key.\r\n", d); |
kenjiArai | 3:0ad6f85b178f | 519 | t.stop(); |
kenjiArai | 3:0ad6f85b178f | 520 | while (true){ |
kenjiArai | 3:0ad6f85b178f | 521 | d = imu.read_calib_status(); |
kenjiArai | 3:0ad6f85b178f | 522 | imu.get_gravity(&gravity); |
kenjiArai | 3:0ad6f85b178f | 523 | pc.printf("Calb dat = 0x%x target = 0xff ACC:X %3.0f, Y %3.0f, Z %3.0f\r\n", |
kenjiArai | 3:0ad6f85b178f | 524 | d, gravity.x, gravity.y, gravity.z); |
kenjiArai | 3:0ad6f85b178f | 525 | if (d == 0xff){ break;} |
kenjiArai | 3:0ad6f85b178f | 526 | if (pc.readable()){ break;} |
kenjiArai | 3:0ad6f85b178f | 527 | wait(1.0); |
kenjiArai | 3:0ad6f85b178f | 528 | } |
kenjiArai | 3:0ad6f85b178f | 529 | if (imu.read_calib_status() == 0xff){ |
kenjiArai | 3:0ad6f85b178f | 530 | pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 531 | } else { |
kenjiArai | 3:0ad6f85b178f | 532 | pc.printf("-> Calibration steps are suspended!\r\n\r\n"); |
kenjiArai | 3:0ad6f85b178f | 533 | } |
kenjiArai | 3:0ad6f85b178f | 534 | t.stop(); |
kenjiArai | 3:0ad6f85b178f | 535 | } |
kenjiArai | 3:0ad6f85b178f | 536 | #endif |
kenjiArai | 3:0ad6f85b178f | 537 | |
kenjiArai | 0:86a17116e8be | 538 | #endif // BNO055_H |