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
BMI055(int sensor_type,const char * iic_bus,int addr)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
read_reg(rt_uint8_t reg,rt_uint8_t * value)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
read_buffer(rt_uint8_t reg,rt_uint8_t * value,rt_size_t size)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
write_reg(rt_uint8_t reg,rt_uint8_t value)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
BMI055_Accelerometer(const char * iic_name,int addr)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
configure(SensorConfig * config)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
activate(int enable)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
poll(sensors_event_t * event)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
getSensor(sensor_t * sensor)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
BMI055_Gyroscope(const char * iic_name,int addr)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
configure(SensorConfig * config)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
activate(int enable)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
poll(sensors_event_t * event)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
getSensor(sensor_t * sensor)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