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