1 /* 2 * Copyright (c) 2006-2018, RT-Thread Development Team 3 * 4 * SPDX-License-Identifier: Apache-2.0 5 * 6 * Change Logs: 7 * Date Author Notes 8 * 2015-1-11 RT_learning the first version 9 */ 10 11 #include <string.h> 12 #include <stdio.h> 13 #include <rtdevice.h> 14 #include "bmi055_sensor.h" 15 16 17 const static sensor_t _BMI055_sensor[] = 18 { 19 { 20 .name = "Accelerometer", 21 .vendor = "Bosch", 22 .version = sizeof(sensor_t), 23 .handle = 0, 24 .type = SENSOR_TYPE_ACCELEROMETER, 25 .maxRange = SENSOR_ACCEL_RANGE_16G, 26 .resolution = 1.0f, 27 .power = 0.5f, 28 .minDelay = 10000, 29 .fifoReservedEventCount = 0, 30 .fifoMaxEventCount = 64, 31 }, 32 { 33 .name = "Gyroscope", 34 .vendor = "Bosch", 35 .version = sizeof(sensor_t), 36 .handle = 0, 37 .type = SENSOR_TYPE_GYROSCOPE, 38 .maxRange = SENSOR_GYRO_RANGE_2000DPS, 39 .resolution = 1.0f, 40 .power = 0.5f, 41 .minDelay = 10000, 42 .fifoReservedEventCount = 0, 43 .fifoMaxEventCount = 64, 44 } 45 }; 46 47 BMI055::BMI055(int sensor_type, const char* iic_bus, int addr) 48 : SensorBase(sensor_type) 49 { 50 this->i2c_bus = (struct rt_i2c_bus_device *)rt_device_find(iic_bus); 51 if (this->i2c_bus == NULL) 52 { 53 printf("BMI055: No IIC device:%s\n", iic_bus); 54 return; 55 } 56 57 this->i2c_addr = addr; 58 59 /* register to sensor manager */ 60 SensorManager::registerSensor(this); 61 } 62 63 int BMI055::read_reg(rt_uint8_t reg, rt_uint8_t *value) 64 { 65 struct rt_i2c_msg msgs[2]; 66 67 msgs[0].addr = this->i2c_addr; 68 msgs[0].flags = RT_I2C_WR; 69 msgs[0].buf = ® 70 msgs[0].len = 1; 71 72 msgs[1].addr = this->i2c_addr; 73 msgs[1].flags = RT_I2C_RD; /* Read from slave */ 74 msgs[1].buf = (rt_uint8_t *)value; 75 msgs[1].len = 1; 76 77 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) 78 return RT_EOK; 79 80 return -RT_ERROR; 81 } 82 83 int BMI055::read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size) 84 { 85 struct rt_i2c_msg msgs[2]; 86 87 msgs[0].addr = this->i2c_addr; 88 msgs[0].flags = RT_I2C_WR; 89 msgs[0].buf = ® 90 msgs[0].len = 1; 91 92 msgs[1].addr = this->i2c_addr; 93 msgs[1].flags = RT_I2C_RD; /* Read from slave */ 94 msgs[1].buf = (rt_uint8_t *)value; 95 msgs[1].len = size; 96 97 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) 98 return RT_EOK; 99 100 return -RT_ERROR; 101 } 102 103 int BMI055::write_reg(rt_uint8_t reg, rt_uint8_t value) 104 { 105 struct rt_i2c_msg msgs[2]; 106 107 msgs[0].addr = this->i2c_addr; 108 msgs[0].flags = RT_I2C_WR; 109 msgs[0].buf = ® 110 msgs[0].len = 1; 111 112 msgs[1].addr = this->i2c_addr; 113 msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START; 114 msgs[1].buf = (rt_uint8_t *)&value; 115 msgs[1].len = 1; 116 117 if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2) 118 return RT_EOK; 119 120 return -RT_ERROR; 121 } 122 123 124 BMI055_Accelerometer::BMI055_Accelerometer(const char* iic_name, int addr) 125 : BMI055(SENSOR_TYPE_ACCELEROMETER, iic_name, addr) 126 { 127 int index; 128 uint8_t id; 129 rt_uint8_t value[6] = {0}; 130 rt_int32_t x, y, z; 131 SensorConfig config = {SENSOR_MODE_NORMAL, SENSOR_DATARATE_400HZ, SENSOR_ACCEL_RANGE_2G}; 132 133 write_reg(BMI055_BGW_SOFTRESET, 0xB6); /* reset of the sensor P57 */ 134 write_reg(BMI055_PMU_LPW, 0x00); /* PMU_LPW NORMAL mode P55 */ 135 write_reg(BMI055_PMU_BW, 0x0A); /* 01010b 31.25 Hz P55 */ 136 write_reg(BMI055_PMU_RANGE, 0x05); /* 0101b 卤4g range PMU_RANGE set acc +-4g/s P54 */ 137 138 139 x_offset = y_offset = z_offset = 0; 140 x = y = z = 0; 141 142 /* read BMI055 id */ 143 read_buffer(BMI055_ACC_BGW_CHIPID, &id, 1); /* BGW_CHIPID P47*/ 144 if (id != BMI055_ACC_BGW_CHIPID_VALUE) 145 { 146 printf("Warning: not found BMI055 id: %02x\n", id); 147 } 148 149 /* get offset */ 150 for (index = 0; index < 200; index ++) 151 { 152 read_buffer(BMI055_ACCD_X_LSB, value, 6); /*ACCD_X_LSB P47 */ 153 154 x += (((rt_int16_t)value[1] << 8) | value[0]); 155 y += (((rt_int16_t)value[3] << 8) | value[2]); 156 z += (((rt_int16_t)value[5] << 8) | value[4]); 157 } 158 x_offset = x / 200; 159 y_offset = y / 200; 160 z_offset = z / 200; 161 162 this->enable = RT_FALSE; 163 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; 164 this->config = config; 165 } 166 167 int 168 BMI055_Accelerometer::configure(SensorConfig *config) 169 { 170 int range; 171 uint8_t value; 172 173 if (config == RT_NULL) return -1; 174 175 /* TODO: set datarate */ 176 177 /* get range and calc the sensitivity */ 178 range = config->range.accel_range; 179 switch (range) 180 { 181 case SENSOR_ACCEL_RANGE_2G: 182 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G; 183 range = 0x03; //0011b 184 break; 185 case SENSOR_ACCEL_RANGE_4G: 186 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G; 187 range = 0x05; //0101b 188 break; 189 case SENSOR_ACCEL_RANGE_8G: 190 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G; 191 range = 0x01 << 3; //1000b 192 break; 193 case SENSOR_ACCEL_RANGE_16G: 194 this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G; 195 range = 0x03 << 2; //1100b 196 break; 197 default: 198 return -1; 199 } 200 201 /* set range to sensor */ 202 read_reg(BMI055_PMU_RANGE, &value); /* PMU_RANGE P54 */ 203 value &= 0xF0; 204 value |= range; 205 write_reg(BMI055_PMU_RANGE, value); 206 207 return 0; 208 } 209 210 int 211 BMI055_Accelerometer::activate(int enable) 212 { 213 uint8_t value; 214 215 if (enable && this->enable == RT_FALSE) 216 { 217 /* enable accelerometer */ 218 read_reg(BMI055_PMU_LPW, &value); /* P55 */ 219 value &= ~(0x07 << 7); 220 write_reg(BMI055_PMU_LPW, value); 221 } 222 223 if (!enable && this->enable == RT_TRUE) 224 { 225 /* disable accelerometer */ 226 read_reg(BMI055_PMU_LPW, &value); 227 value &= ~(0x07 << 7); 228 value |= (0x01 << 7); 229 write_reg(BMI055_PMU_LPW, value); 230 } 231 232 if (enable) this->enable = RT_TRUE; 233 else this->enable = RT_FALSE; 234 235 return 0; 236 } 237 238 int 239 BMI055_Accelerometer::poll(sensors_event_t *event) 240 { 241 rt_uint8_t value[6]; 242 rt_int16_t x, y, z; 243 244 /* parameters check */ 245 if (event == NULL) return -1; 246 247 /* get event data */ 248 event->version = sizeof(sensors_event_t); 249 event->sensor = (int32_t) this; 250 event->timestamp = rt_tick_get(); 251 event->type = SENSOR_TYPE_ACCELEROMETER; 252 253 read_buffer(0x02, value, 6); 254 255 /* get raw data */ 256 x = (((rt_int16_t)value[1] << 8) | value[0]); 257 y = (((rt_int16_t)value[3] << 8) | value[2]); 258 z = (((rt_int16_t)value[5] << 8) | value[4]); 259 260 if (config.mode == SENSOR_MODE_RAW) 261 { 262 event->raw_acceleration.x = x; 263 event->raw_acceleration.y = y; 264 event->raw_acceleration.z = z; 265 } 266 else 267 { 268 269 x -= x_offset; y -= y_offset; z -= z_offset; 270 event->acceleration.x = x * this->sensitivity * SENSORS_GRAVITY_STANDARD; 271 event->acceleration.y = y * this->sensitivity * SENSORS_GRAVITY_STANDARD; 272 event->acceleration.z = z * this->sensitivity * SENSORS_GRAVITY_STANDARD; 273 } 274 275 return 0; 276 } 277 278 void 279 BMI055_Accelerometer::getSensor(sensor_t *sensor) 280 { 281 /* get sensor description */ 282 if (sensor) 283 { 284 memcpy(sensor, &_BMI055_sensor[0], sizeof(sensor_t)); 285 } 286 } 287 288 BMI055_Gyroscope::BMI055_Gyroscope(const char* iic_name, int addr) 289 : BMI055(SENSOR_TYPE_GYROSCOPE, iic_name, addr) 290 { 291 int index; 292 uint8_t id; 293 rt_uint8_t value[6]; 294 rt_int32_t x, y, z; 295 296 /* initialize BMI055 */ 297 write_reg(BMI055_MODE_LPM1_ADDR, 0x00); /* normal mode */ 298 write_reg(BMI055_MODE_LPM2_ADDR, 0x80); /* fast powerup */ 299 write_reg(BMI055_BW_ADDR, 0x03); /* ODR:400Hz Filter Bandwidth:47Hz */ 300 write_reg(BMI055_RANGE_ADDR, 0x00); /* 2000dps */ 301 302 303 x_offset = y_offset = z_offset = 0; 304 x = y = z = 0; 305 306 /* read BMI055 id */ 307 read_buffer(BMI055_CHIP_ID_ADDR, &id, 1); 308 if (id != BMI055_GRRO_CHIP_ID) 309 { 310 printf("Warning: not found BMI055 id: %02x\n", id); 311 } 312 313 /* get offset */ 314 for (index = 0; index < 200; index ++) 315 { 316 read_buffer(BMI055_RATE_X_LSB_ADDR, value, 6); 317 318 x += (((rt_int16_t)value[1] << 8) | value[0]); 319 y += (((rt_int16_t)value[3] << 8) | value[2]); 320 z += (((rt_int16_t)value[5] << 8) | value[4]); 321 } 322 x_offset = x / 200; 323 y_offset = y / 200; 324 z_offset = z / 200; 325 326 this->enable = RT_FALSE; 327 this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; 328 } 329 330 int 331 BMI055_Gyroscope::configure(SensorConfig *config) 332 { 333 int range; 334 uint8_t value; 335 336 if (config == RT_NULL) return -1; 337 338 /* TODO: set datarate */ 339 340 /* get range and calc the sensitivity */ 341 range = config->range.gyro_range; 342 switch (range) 343 { 344 //to do add more range e.g 125DPS 345 //case 346 case SENSOR_GYRO_RANGE_250DPS: 347 this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS; 348 range = 0x11; 349 break; 350 case SENSOR_GYRO_RANGE_500DPS: 351 this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS; 352 range = 0x10; 353 break; 354 case SENSOR_GYRO_RANGE_1000DPS: 355 this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS; 356 range = 0x01; 357 break; 358 case SENSOR_GYRO_RANGE_2000DPS: 359 this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS; 360 range = 0x00; 361 break; 362 default: 363 return -1; 364 } 365 366 /* set range to sensor */ 367 read_reg(BMI055_RANGE_ADDR, &value); 368 value &= ~0x07; 369 value |= range; 370 write_reg(BMI055_RANGE_ADDR, value); 371 372 return 0; 373 } 374 375 int 376 BMI055_Gyroscope::activate(int enable) 377 { 378 uint8_t value; 379 380 if (enable && this->enable == RT_FALSE) 381 { 382 /* enable gyroscope */ 383 read_reg(BMI055_MODE_LPM1_ADDR, &value); 384 value &= ~(0x1010 << 4); //{0; 0} NORMAL mode 385 write_reg(BMI055_MODE_LPM1_ADDR, value); //P101 NORMAL mode 386 } 387 388 if (!enable && this->enable == RT_TRUE) 389 { 390 /* disable gyroscope */ 391 read_reg(BMI055_MODE_LPM1_ADDR, &value); 392 value &= ~(0x01 << 5); //set bit5 deep_suspend 0 393 value |= (0x01 << 7); //set bit1 suspend 1 394 write_reg(BMI055_MODE_LPM1_ADDR, value); //{1; 0} SUSPEND mode 395 } 396 397 if (enable) this->enable = RT_TRUE; 398 else this->enable = RT_FALSE; 399 400 return 0; 401 } 402 403 int 404 BMI055_Gyroscope::poll(sensors_event_t *event) 405 { 406 rt_uint8_t value[6]; 407 rt_int16_t x, y, z; 408 409 /* parameters check */ 410 if (event == NULL) return -1; 411 412 /* get event data */ 413 event->version = sizeof(sensors_event_t); 414 event->sensor = (int32_t) this; 415 event->timestamp = rt_tick_get(); 416 event->type = SENSOR_TYPE_GYROSCOPE; 417 418 read_buffer(BMI055_RATE_X_LSB_ADDR, value, 6); 419 420 /* get raw data */ 421 x = (((rt_int16_t)value[1] << 8) | value[0]); 422 y = (((rt_int16_t)value[3] << 8) | value[2]); 423 z = (((rt_int16_t)value[5] << 8) | value[4]); 424 425 426 if (config.mode == SENSOR_MODE_RAW) 427 { 428 event->raw_gyro.x = x; 429 event->raw_gyro.y = y; 430 event->raw_gyro.z = z; 431 } 432 else 433 { 434 x -= x_offset; y -= y_offset; z -= z_offset; 435 event->gyro.x = x * this->sensitivity * SENSORS_DPS_TO_RADS; 436 event->gyro.y = y * this->sensitivity * SENSORS_DPS_TO_RADS; 437 event->gyro.z = z * this->sensitivity * SENSORS_DPS_TO_RADS; 438 } 439 440 return 0; 441 } 442 443 void 444 BMI055_Gyroscope::getSensor(sensor_t *sensor) 445 { 446 /* get sensor description */ 447 if (sensor) 448 { 449 memcpy(sensor, &_BMI055_sensor[1], sizeof(sensor_t)); 450 } 451 } 452