1 /*
2 * Copyright (C) 2015-2017 Alibaba Group Holding Limited
3 *
4 *
5 */
6 /*********************************************************************************************
7 *
8 *Copyright (C) 2016 - 2020 Bosch Sensortec GmbH
9
10 *Redistribution and use in source and binary forms, with or without
11 *modification, are permitted provided that the following conditions are met:
12
13 *Redistributions of source code must retain the above copyright
14 *notice, this list of conditions and the following disclaimer.
15
16 *Redistributions in binary form must reproduce the above copyright
17 *notice, this list of conditions and the following disclaimer in the
18 *documentation and/or other materials provided with the distribution.
19
20 *Neither the name of the copyright holder nor the names of the
21 *contributors may be used to endorse or promote products derived from
22 *this software without specific prior written permission.
23
24 *THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
25 *CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
26 *IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
27 *WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
28 *DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER
29 *OR CONTRIBUTORS BE LIABLE FOR ANY
30 *DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
31 *OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
32 *PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
33 *LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
34 *HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
35 *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
36 *(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
37 *ANY WAY OUT OF THE USE OF THIS
38 *SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE
39
40 *The information provided is believed to be accurate and reliable.
41 *The copyright holder assumes no responsibility
42 *for the consequences of use
43 *of such information nor for any infringement of patents or
44 *other rights of third parties which may result from its use.
45 *No license is granted by implication or otherwise under any patent or
46 *patent rights of the copyright holder.
47 *
48 *
49 *******************************************************************************************/
50
51 #include "aos/kernel.h"
52 #include <stdio.h>
53 #include <stdlib.h>
54 #include <string.h>
55 #include "sensor_drv_api.h"
56 #include "sensor_hal.h"
57 #define BMI260_I2C_ADDR_TRANS(n) ((n)<<1)
58 #define BMI260_I2C_SLAVE_ADDR_LOW 0x68
59 #define BMI260_I2C_ADDR BMI260_I2C_ADDR_TRANS(BMI260_I2C_SLAVE_ADDR_LOW)
60
61 /* Utility macros */
62 #define BMI2_SET_BITS(reg_data, bitname, data) \
63 ((reg_data & ~(bitname##_MASK)) | \
64 ((data << bitname##_POS) & bitname##_MASK))
65
66 #define BMI2_GET_BITS(reg_data, bitname) \
67 ((reg_data & (bitname##_MASK)) >> \
68 (bitname##_POS))
69 #define BMI2_SET_BIT_POS0(reg_data, bitname, data) \
70 ((reg_data & ~(bitname##_MASK)) | \
71 (data & bitname##_MASK))
72
73 #define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK))
74 #define BMI2_SET_BIT_VAL0(reg_data, bitname) (reg_data & ~(bitname##_MASK))
75
76 /* For getting LSB and MSB */
77 #define BMI2_GET_LSB(var) (uint8_t)(var & BMI2_SET_LOW_BYTE)
78 #define BMI2_GET_MSB(var) (uint8_t)((var & BMI2_SET_HIGH_BYTE) >> 8)
79
80 /* BMI2 register addresses */
81 #define BMI2_CHIP_ID_ADDR UINT8_C(0x00)
82 #define BMI260_CHIP_ID UINT8_C(0x20)
83 #define BMI2_STATUS_ADDR UINT8_C(0x03)
84 #define BMI2_AUX_X_LSB_ADDR UINT8_C(0x04)
85 #define BMI2_ACC_X_LSB_ADDR UINT8_C(0x0C)
86 #define BMI2_GYR_X_LSB_ADDR UINT8_C(0x12)
87 #define BMI2_EVENT_ADDR UINT8_C(0x1B)
88 #define BMI2_INT_STATUS_0_ADDR UINT8_C(0x1C)
89 #define BMI2_INT_STATUS_1_ADDR UINT8_C(0x1D)
90 #define BMI2_SYNC_COMMAND_ADDR UINT8_C(0x1E)
91 #define BMI2_CONF_STATUS_ADDR UINT8_C(0x21)
92 #define BMI2_FIFO_LENGTH_0_ADDR UINT8_C(0X24)
93 #define BMI2_FIFO_DATA_ADDR UINT8_C(0X26)
94 #define BMI2_PAGE_REG_ADDR UINT8_C(0x2F)
95 #define BMI2_FEAT_START_ADDR UINT8_C(0x30)
96 #define BMI2_ACC_CONF_ADDR UINT8_C(0x40)
97 #define BMI2_ACC_CONF1_ADDR UINT8_C(0x41)
98 #define BMI2_GYR_CONF_ADDR UINT8_C(0x42)
99 #define BMI2_GYR_CONF1_ADDR UINT8_C(0x43)
100 #define BMI2_AUX_CONF_ADDR UINT8_C(0x44)
101 #define BMI2_FIFO_DOWN_ADDR UINT8_C(0X45)
102 #define BMI2_FIFO_WTM_0_ADDR UINT8_C(0X46)
103 #define BMI2_FIFO_WTM_1_ADDR UINT8_C(0X47)
104 #define BMI2_FIFO_CONFIG_0_ADDR UINT8_C(0X48)
105 #define BMI2_FIFO_CONFIG_1_ADDR UINT8_C(0X49)
106 #define BMI2_AUX_DEV_ID_ADDR UINT8_C(0x4B)
107 #define BMI2_AUX_IF_CONF_ADDR UINT8_C(0x4C)
108 #define BMI2_AUX_RD_ADDR UINT8_C(0x4D)
109 #define BMI2_AUX_WR_ADDR UINT8_C(0x4E)
110 #define BMI2_AUX_WR_DATA_ADDR UINT8_C(0x4F)
111 #define BMI2_INT1_IO_CTRL_ADDR UINT8_C(0x53)
112 #define BMI2_INT2_IO_CTRL_ADDR UINT8_C(0x54)
113 #define BMI2_INT1_MAP_ADDR UINT8_C(0x56)
114 #define BMI2_INT2_MAP_ADDR UINT8_C(0x57)
115 #define BMI2_INT_MAP_HW_ADDR UINT8_C(0x58)
116 #define BMI2_CONF_CTRL_ADDR UINT8_C(0x59)
117 #define BMI2_NEXT_LSB_ADDR UINT8_C(0x5B)
118 #define BMI2_NEXT_MSB_ADDR UINT8_C(0x5C)
119 #define BMI2_CONF_LOAD_ADDR UINT8_C(0x5E)
120 #define BMI2_IF_CONF_ADDR UINT8_C(0X6B)
121 #define BMI2_ACC_SELF_TEST_ADDR UINT8_C(0X6D)
122 #define BMI2_GYR_OFF_COMP_3_ADDR UINT8_C(0X74)
123 #define BMI2_GYR_OFF_COMP_EN_ADDR UINT8_C(0X77)
124 #define BMI2_GYR_USR_GAIN_0_ADDR UINT8_C(0X78)
125 #define BMI2_PWR_CONF_ADDR UINT8_C(0x7C)
126 #define BMI2_PWR_CTRL_ADDR UINT8_C(0x7D)
127 #define BMI2_CMD_REG_ADDR UINT8_C(0x7E)
128
129 /* BMI2 Commands */
130 #define BMI2_USER_DEF_CMD_0 UINT8_C(0x03)
131 #define BMI2_SOFT_RESET_CMD UINT8_C(0xB6)
132 #define BMI2_FIFO_FLUSH_CMD UINT8_C(0xB0)
133
134 /* Accelerometer Macro Definitions */
135 /* Accelerometer Bandwidth parameters */
136 #define BMI2_ACC_OSR4_AVG1 UINT8_C(0x00)
137 #define BMI2_ACC_OSR2_AVG2 UINT8_C(0x01)
138 #define BMI2_ACC_NORMAL_AVG4 UINT8_C(0x02)
139 #define BMI2_ACC_CIC_AVG8 UINT8_C(0x03)
140 #define BMI2_ACC_RES_AVG16 UINT8_C(0x04)
141 #define BMI2_ACC_RES_AVG32 UINT8_C(0x05)
142 #define BMI2_ACC_RES_AVG64 UINT8_C(0x06)
143 #define BMI2_ACC_RES_AVG128 UINT8_C(0x07)
144
145 /* Accelerometer Output Data Rate */
146 #define BMI2_ACC_ODR_0_78HZ UINT8_C(0x01)
147 #define BMI2_ACC_ODR_1_56HZ UINT8_C(0x02)
148 #define BMI2_ACC_ODR_3_12HZ UINT8_C(0x03)
149 #define BMI2_ACC_ODR_6_25HZ UINT8_C(0x04)
150 #define BMI2_ACC_ODR_12_5HZ UINT8_C(0x05)
151 #define BMI2_ACC_ODR_25HZ UINT8_C(0x06)
152 #define BMI2_ACC_ODR_50HZ UINT8_C(0x07)
153 #define BMI2_ACC_ODR_100HZ UINT8_C(0x08)
154 #define BMI2_ACC_ODR_200HZ UINT8_C(0x09)
155 #define BMI2_ACC_ODR_400HZ UINT8_C(0x0A)
156 #define BMI2_ACC_ODR_800HZ UINT8_C(0x0B)
157 #define BMI2_ACC_ODR_1600HZ UINT8_C(0x0C)
158
159 /* Accelerometer G Range */
160 #define BMI2_ACC_RANGE_2G UINT8_C(0x00)
161 #define BMI2_ACC_RANGE_4G UINT8_C(0x01)
162 #define BMI2_ACC_RANGE_8G UINT8_C(0x02)
163 #define BMI2_ACC_RANGE_16G UINT8_C(0x03)
164
165 /* Mask definitions for accelerometer configuration register */
166 #define BMI2_ACC_RANGE_MASK UINT8_C(0x03)
167 #define BMI2_ACC_ODR_MASK UINT8_C(0x0F)
168 #define BMI2_ACC_BW_PARAM_MASK UINT8_C(0x70)
169 #define BMI2_ACC_PERF_MODE_MASK UINT8_C(0x80)
170
171 /* Bit position definitions for accelerometer configuration register */
172 #define BMI2_ACC_BW_PARAM_POS UINT8_C(0x04)
173 #define BMI2_ACC_PERF_MODE_POS UINT8_C(0x07)
174
175 /* Self test macro to show resulting minimum difference signal in mg */
176 #define BMI2_ST_ACC_X_AXIS_SIGNAL_DIFF UINT16_C(1800)
177 #define BMI2_ST_ACC_Y_AXIS_SIGNAL_DIFF UINT16_C(1800)
178 #define BMI2_ST_ACC_Z_AXIS_SIGNAL_DIFF UINT16_C(1800)
179
180 /* Mask definitions for accelerometer self-test */
181 #define BMI2_ACC_SELF_TEST_EN_MASK UINT8_C(0x01)
182 #define BMI2_ACC_SELF_TEST_SIGN_MASK UINT8_C(0x04)
183 #define BMI2_ACC_SELF_TEST_AMP_MASK UINT8_C(0x08)
184
185 /* Bit Positions for accelerometer self-test */
186 #define BMI2_ACC_SELF_TEST_SIGN_POS UINT8_C(0x02)
187 #define BMI2_ACC_SELF_TEST_AMP_POS UINT8_C(0x03)
188 /* Gyroscope Macro Definitions */
189 /* Gyroscope sense drive and low power mode */
190 /* Senses low power mode */
191 #define BMI2_GYR_LOW_POWER_MODE UINT8_C(0x00)
192 /* Select drive */
193 #define BMI2_GYR_HIGH_PERF_MODE UINT8_C(0x01)
194
195 /* Gyroscope Bandwidth parameters */
196 #define BMI2_GYR_OSR4_MODE UINT8_C(0x00)
197 #define BMI2_GYR_OSR2_MODE UINT8_C(0x01)
198 #define BMI2_GYR_NORMAL_MODE UINT8_C(0x02)
199 #define BMI2_GYR_CIC_MODE UINT8_C(0x03)
200
201 /* Gyroscope Output Data Rate */
202 #define BMI2_GYR_ODR_25HZ UINT8_C(0x06)
203 #define BMI2_GYR_ODR_50HZ UINT8_C(0x07)
204 #define BMI2_GYR_ODR_100HZ UINT8_C(0x08)
205 #define BMI2_GYR_ODR_200HZ UINT8_C(0x09)
206 #define BMI2_GYR_ODR_400HZ UINT8_C(0x0A)
207 #define BMI2_GYR_ODR_800HZ UINT8_C(0x0B)
208 #define BMI2_GYR_ODR_1600HZ UINT8_C(0x0C)
209 #define BMI2_GYR_ODR_3200HZ UINT8_C(0x0D)
210
211 /* Gyroscope OIS Range */
212 #define BMI2_GYR_OIS_125 UINT8_C(0x00)
213 #define BMI2_GYR_OIS_2000 UINT8_C(0x01)
214
215 /* Gyroscope Angular Rate Measurement Range */
216 #define BMI2_GYR_RANGE_2000 UINT8_C(0x00)
217 #define BMI2_GYR_RANGE_1000 UINT8_C(0x01)
218 #define BMI2_GYR_RANGE_500 UINT8_C(0x02)
219 #define BMI2_GYR_RANGE_250 UINT8_C(0x03)
220 #define BMI2_GYR_RANGE_125 UINT8_C(0x04)
221
222 /* Mask definitions for gyroscope configuration register */
223 #define BMI2_GYR_RANGE_MASK UINT8_C(0x07)
224 #define BMI2_GYR_OIS_RANGE_MASK UINT8_C(0x08)
225 #define BMI2_GYR_ODR_MASK UINT8_C(0x0F)
226 #define BMI2_GYR_BW_PARAM_MASK UINT8_C(0x30)
227 #define BMI2_GYR_DSLP_MASK UINT8_C(0x40)
228 #define BMI2_GYR_PERF_MODE_MASK UINT8_C(0x80)
229
230 /* Bit position definitions for gyroscope configuration register */
231 #define BMI2_GYR_OIS_RANGE_POS UINT8_C(0x03)
232 #define BMI2_GYR_BW_PARAM_POS UINT8_C(0x04)
233 #define BMI2_GYR_DSLP_POS UINT8_C(0x06)
234 #define BMI2_GYR_PERF_MODE_POS UINT8_C(0x07)
235
236 /* default HZ */
237 #define BMI260_ACC_DEFAULT_ODR_100HZ (100)
238 #define BMI260_ACC_DEFAULT_ODR_25HZ (25)
239 #define BMI260_GYRO_DEFAULT_ODR_100HZ (100)
240 #define BMI260_GYRO_DEFAULT_ODR_25HZ (25)
241
242 /* Mask definitions for power control register */
243 #define BMI2_AUX_EN_MASK UINT8_C(0x01)
244 #define BMI2_GYR_EN_MASK UINT8_C(0x02)
245 #define BMI2_ACC_EN_MASK UINT8_C(0x04)
246 #define BMI2_TEMP_EN_MASK UINT8_C(0x08)
247
248 /* Bit position definitions for power control register */
249 #define BMI2_GYR_EN_POS UINT8_C(0x01)
250 #define BMI2_ACC_EN_POS UINT8_C(0x02)
251 #define BMI2_TEMP_EN_POS UINT8_C(0x03)
252
253 /* Mask definitions for power configuration register */
254 #define BMI2_ADV_POW_EN_MASK UINT8_C(0x01)
255
256 /* Mask definitions for sensor event flags */
257 #define BMI2_EVENT_FLAG_MASK UINT8_C(0x1C)
258
259 /* Bit position definitions for sensor event flags */
260 #define BMI2_EVENT_FLAG_POS UINT8_C(0x02)
261
262 /* BMI2 sensor data bytes */
263 #define BMI2_ACC_GYR_NUM_BYTES UINT8_C(6)
264 #define BMI2_AUX_NUM_BYTES UINT8_C(8)
265
266 /* For enable and disable */
267 #define BMI2_ENABLE UINT8_C(1)
268 #define BMI2_DISABLE UINT8_C(0)
269
270 /* To define success code */
271 #define BMI2_OK INT8_C(0)
272
273 /* To define error codes */
274 #define BMI2_E_NULL_PTR INT8_C(-1)
275 #define BMI2_E_COM_FAIL INT8_C(-2)
276 #define BMI2_E_DEV_NOT_FOUND INT8_C(-3)
277 #define BMI2_E_OUT_OF_RANGE INT8_C(-4)
278 #define BMI2_E_ACC_INVALID_CFG INT8_C(-5)
279 #define BMI2_E_GYRO_INVALID_CFG INT8_C(-6)
280 #define BMI2_E_ACC_GYR_INVALID_CFG INT8_C(-7)
281 #define BMI2_E_INVALID_SENSOR INT8_C(-8)
282 #define BMI2_E_CONFIG_LOAD INT8_C(-9)
283 #define BMI2_E_INVALID_PAGE INT8_C(-11)
284 #define BMI2_E_INVALID_FEAT_INT INT8_C(-12)
285 #define BMI2_E_INVALID_INT_PIN INT8_C(-13)
286 #define BMI2_E_SET_APS_FAIL INT8_C(-14)
287 #define BMI2_E_AUX_INVALID_CFG INT8_C(-15)
288 #define BMI2_E_AUX_BUSY INT8_C(-16)
289 #define BMI2_E_SELF_TEST_FAIL INT8_C(-17)
290 #define BMI2_E_REMAP_ERROR INT8_C(-18)
291 #define BMI2_E_GYR_USER_GAIN_UPD_FAIL INT8_C(-19)
292
293 enum bmi2_sensor_config_error_e {
294 BMI2_NO_ERROR,
295 BMI2_ACC_ERROR,
296 BMI2_GYR_ERROR,
297 BMI2_ACC_GYR_ERROR
298 };
299
300 i2c_dev_t bmi260_ctx = {
301 .port = 3,
302 .config.address_width = 8,
303 .config.freq = 400000,
304 .config.dev_addr = BMI260_I2C_ADDR,
305 };
306
307 static int g_gyro_active_count = 0;
308 static int g_acc_active_count = 0;
309
310 static int32_t g_bmi260_acc_factor[ACC_RANGE_MAX] = { 16384, 8192, 4096, 2048 };
311 static int32_t g_bmi260_gyro_factor[GYRO_RANGE_MAX] = {262, 131, 65, 33, 16 };
312
313 static int32_t g_cur_acc_factor = 0;
314 static int32_t g_cur_gyro_factor = 0;
315 static int32_t g_bmi260flag = 0;
316
317 /**
318 * This function shows the error status when illegal sensor
319 * configuration is set.
320 * @param[in] drv pointer to the i2c dev
321 * @return the operation status, 0 is OK, others is error
322 */
cfg_error_status(i2c_dev_t * drv)323 static int8_t cfg_error_status(i2c_dev_t* drv)
324 {
325 int8_t rslt = 0; /* Variable to define error */
326 int ret;
327 uint8_t reg_data; /* Variable to store data */
328
329 /* Get error status of the set sensor configuration */
330 ret = sensor_i2c_read(drv, BMI2_EVENT_ADDR, ®_data, I2C_DATA_LEN,
331 I2C_OP_RETRIES);
332 if(unlikely(ret) != 0) {
333 printf("read CHIPID failed \n");
334 rslt = BMI2_E_COM_FAIL;
335 return rslt;
336 }
337 reg_data = BMI2_GET_BITS(reg_data, BMI2_EVENT_FLAG);
338
339 switch(reg_data) {
340 case BMI2_NO_ERROR:
341 rslt = BMI2_OK;
342 break;
343 case BMI2_ACC_ERROR:
344 rslt = BMI2_E_ACC_INVALID_CFG;
345 break;
346 case BMI2_GYR_ERROR:
347 rslt = BMI2_E_GYRO_INVALID_CFG;
348 break;
349 case BMI2_ACC_GYR_ERROR:
350 rslt = BMI2_E_ACC_GYR_INVALID_CFG;
351 break;
352 default:
353 break;
354 }
355
356 return rslt;
357 }
358
359 /**
360 * This function does the soft reset
361 *
362 * @param[in] drv pointer to the i2c dev
363 * @return the operation status, 0 is OK, others is error
364 */
drv_acc_gyro_bosch_bmi260_soft_reset(i2c_dev_t * drv)365 static int drv_acc_gyro_bosch_bmi260_soft_reset(i2c_dev_t* drv)
366 {
367 int ret = 0;
368 uint8_t value = 0;
369 value = BMI2_SOFT_RESET_CMD;
370
371 ret = sensor_i2c_write(drv, BMI2_CMD_REG_ADDR, &value, I2C_DATA_LEN,
372 I2C_OP_RETRIES);
373 aos_msleep(2);
374 if(unlikely(ret) != 0) {
375 return -1;
376 }
377 return 0;
378 }
379
380 /**
381 * This function validates the chip ID of device
382 *
383 * @param[in] drv pointer to the i2c dev
384 * @param[in] id_value the expected CHIPID
385 * @return the operation status, 0 is OK, others is error
386 */
drv_acc_gyro_bosch_bmi260_validate_id(i2c_dev_t * drv,uint8_t id_value)387 static int drv_acc_gyro_bosch_bmi260_validate_id(i2c_dev_t* drv,
388 uint8_t id_value)
389 {
390 uint8_t value = 0x00;
391 int ret = 0;
392
393 if(drv == NULL) {
394 return -1;
395 }
396
397 ret = sensor_i2c_read(drv, BMI2_CHIP_ID_ADDR, &value, I2C_DATA_LEN,
398 I2C_OP_RETRIES);
399 if(unlikely(ret) != 0) {
400 printf("read CHIPID failed \n");
401 return ret;
402 }
403 printf("!!!!!!read CHIPID %x\n", value);
404 if(id_value != (value & 0x20)) {
405 printf("!!!!!failed read CHIPID %x\n", value);
406 return -1;
407 }
408
409 return 0;
410 }
411
412 /**
413 * This function enables the acc and gyro
414 *
415 * @param[in] drv pointer to the i2c dev
416 * @return the operation status, 0 is OK, others is error
417 */
drv_acc_gyro_bosch_bmi260_enable(i2c_dev_t * drv)418 static int drv_acc_gyro_bosch_bmi260_enable(i2c_dev_t* drv)
419 {
420 int ret = 0;
421
422 uint8_t reg_data = 0; /* Variable to store register values */
423 uint8_t aps_status = 0; /* Variable to store adv power status */
424 int8_t rslt = 0; /* Variable to define error */
425
426 if(drv == NULL) {
427 return -1;
428 }
429
430 /* first enable ACC and GYRO */
431 ret = sensor_i2c_read(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
432 I2C_OP_RETRIES);
433 if(unlikely(ret) != 0) {
434 printf("read PWR_CTRL failed \n");
435 return ret;
436 }
437
438 /* enable ACC */
439 reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE);
440 /* enable GYRO */
441 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE);
442
443 ret = sensor_i2c_write(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
444 I2C_OP_RETRIES);
445 aos_msleep(3);
446 if(unlikely(ret) != 0) {
447 printf("write PWR_CTRL failed \n");
448 return ret;
449 }
450
451 /* Get status of advance power save mode */
452 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
453 I2C_OP_RETRIES);
454 if(unlikely(ret) != 0) {
455 printf("read PWR_CONF_ADDR failed \n");
456 return ret;
457 }
458 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
459 if((aps_status == BMI2_ENABLE)) {
460 /* Disable advance power save if enabled */
461 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
462 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
463 I2C_OP_RETRIES);
464 aos_msleep(3);
465 if(unlikely(ret) != 0) {
466 printf("write PWR_CONF_ADDR failed \n");
467 return ret;
468 }
469 }
470
471 /* get ACC_CONFIG */
472 ret = sensor_i2c_read(drv, BMI2_ACC_CONF_ADDR, ®_data, I2C_DATA_LEN,
473 I2C_OP_RETRIES);
474 if(unlikely(ret) != 0) {
475 printf("read BMI2_ACC_CONF_ADDR failed \n");
476 return ret;
477 }
478 /* enable accelerometer performance mode */
479 reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_PERF_MODE, BMI2_ENABLE);
480
481 /* Write accelerometer configuration to ACC_CONF */
482 ret = sensor_i2c_write(drv, BMI2_ACC_CONF_ADDR, ®_data, I2C_DATA_LEN,
483 I2C_OP_RETRIES);
484 aos_msleep(3);
485 if(unlikely(ret) != 0) {
486 printf("write BMI2_ACC_CONF_ADDR failed \n");
487 return ret;
488 }
489
490 /* Get error status to check for invalid configurations */
491 rslt = cfg_error_status(drv);
492 if(rslt != BMI2_OK) {
493 printf("setting ACC failed, rslt = %d\n", rslt);
494 ret = -1;
495 return ret;
496 }
497
498 /* get GYRO_CONFIG */
499 ret = sensor_i2c_read(drv, BMI2_GYR_CONF_ADDR, ®_data, I2C_DATA_LEN,
500 I2C_OP_RETRIES);
501 if(unlikely(ret) != 0) {
502 printf("read BMI2_GYR_CONF_ADDR failed \n");
503 return ret;
504 }
505 /* enable gyroscope performance mode */
506 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_PERF_MODE, BMI2_ENABLE);
507
508 /* disable gyroscope high performance/low-power mode */
509 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_DSLP, BMI2_DISABLE);
510
511 /* Write accelerometer configuration to GYR_CONF */
512 ret = sensor_i2c_write(drv, BMI2_GYR_CONF_ADDR, ®_data, I2C_DATA_LEN,
513 I2C_OP_RETRIES);
514 aos_msleep(3);
515 if(unlikely(ret) != 0) {
516 printf("write BMI2_GYR_CONF_ADDR failed \n");
517 return ret;
518 }
519
520 /* Get error status to check for invalid configurations */
521 rslt = cfg_error_status(drv);
522 if(rslt != BMI2_OK) {
523 printf("setting GYRO failed, rslt = %d\n", rslt);
524 ret = -1;
525 return ret;
526 }
527
528 /* enable advance power save if needed */
529 if(aps_status == BMI2_ENABLE) {
530 /* Get status of advance power save mode */
531 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
532 I2C_OP_RETRIES);
533 if(unlikely(ret) != 0) {
534 printf("read PWR_CONF_ADDR failed \n");
535 return ret;
536 }
537 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
538 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
539 I2C_OP_RETRIES);
540 aos_msleep(3);
541 if(unlikely(ret) != 0) {
542 printf("write PWR_CONF_ADDR failed \n");
543 return ret;
544 }
545 }
546
547 return 0;
548
549 }
550
551 /**
552 * This function disables the acc and gyro
553 *
554 * @param[in] drv pointer to the i2c dev
555 * @return the operation status, 0 is OK, others is error
556 */
drv_acc_gyro_bosch_bmi260_disable(i2c_dev_t * drv)557 static int drv_acc_gyro_bosch_bmi260_disable(i2c_dev_t* drv)
558 {
559 int ret = 0;
560
561 uint8_t reg_data = 0; /* Variable to store register values */
562
563 uint8_t aps_status = 0; /* Variable to store adv power status */
564
565 if(drv == NULL) {
566 return -1;
567 }
568
569 /* first disable ACC and GYRO */
570 ret = sensor_i2c_read(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
571 I2C_OP_RETRIES);
572 if(unlikely(ret) != 0) {
573 printf("read PWR_CTRL failed \n");
574 return ret;
575 }
576
577 /* disable ACC */
578 reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN);
579 /* disable GYRO */
580 reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN);
581
582 ret = sensor_i2c_write(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
583 I2C_OP_RETRIES);
584 aos_msleep(3);
585 if(unlikely(ret) != 0) {
586 printf("write PWR_CTRL failed \n");
587 return ret;
588 }
589
590 /* Get status of advance power save mode */
591 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
592 I2C_OP_RETRIES);
593 if(unlikely(ret) != 0) {
594 printf("read PWR_CONF_ADDR failed \n");
595 return ret;
596 }
597 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
598 if((aps_status == BMI2_DISABLE)) {
599 /* enable advance power save if disabled */
600 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
601 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
602 I2C_OP_RETRIES);
603 aos_msleep(3);
604 if(unlikely(ret) != 0) {
605 printf("write PWR_CONF_ADDR failed \n");
606 return ret;
607 }
608 }
609
610 return 0;
611
612 }
613
614 /**
615 * This function enables the acc
616 *
617 * @param[in] drv pointer to the i2c dev
618 * @return the operation status, 0 is OK, others is error
619 */
drv_acc_bosch_bmi260_enable(i2c_dev_t * drv)620 static int drv_acc_bosch_bmi260_enable(i2c_dev_t* drv)
621 {
622 int ret = 0;
623 uint8_t reg_data = 0; /* Variable to store register values */
624 uint8_t aps_status = 0; /* Variable to store adv power status */
625 int8_t rslt = 0; /* Variable to define error */
626
627 if(drv == NULL) {
628 return -1;
629 }
630
631 /* first enable ACC and GYRO */
632 ret = sensor_i2c_read(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
633 I2C_OP_RETRIES);
634 if(unlikely(ret) != 0) {
635 printf("read PWR_CTRL failed \n");
636 return ret;
637 }
638
639 /* enable ACC */
640 reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE);
641 /* enable GYRO */
642 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_DISABLE);
643
644 ret = sensor_i2c_write(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
645 I2C_OP_RETRIES);
646 aos_msleep(3);
647 if(unlikely(ret) != 0) {
648 printf("write PWR_CTRL failed \n");
649 return ret;
650 }
651
652 /* Get status of advance power save mode */
653 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
654 I2C_OP_RETRIES);
655 if(unlikely(ret) != 0) {
656 printf("read PWR_CONF_ADDR failed \n");
657 return ret;
658 }
659 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
660 if((aps_status == BMI2_ENABLE)) {
661 /* Disable advance power save if enabled */
662 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
663 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
664 I2C_OP_RETRIES);
665 aos_msleep(3);
666 if(unlikely(ret) != 0) {
667 printf("write PWR_CONF_ADDR failed \n");
668 return ret;
669 }
670 }
671
672 /* get ACC_CONFIG */
673 ret = sensor_i2c_read(drv, BMI2_ACC_CONF_ADDR, ®_data, I2C_DATA_LEN,
674 I2C_OP_RETRIES);
675 if(unlikely(ret) != 0) {
676 printf("read BMI2_ACC_CONF_ADDR failed \n");
677 return ret;
678 }
679 /* enable accelerometer performance mode */
680 reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_PERF_MODE, BMI2_ENABLE);
681
682 /* Write accelerometer configuration to ACC_CONF */
683 ret = sensor_i2c_write(drv, BMI2_ACC_CONF_ADDR, ®_data, I2C_DATA_LEN,
684 I2C_OP_RETRIES);
685 aos_msleep(3);
686 if(unlikely(ret) != 0) {
687 printf("write BMI2_ACC_CONF_ADDR failed \n");
688 return ret;
689 }
690
691 /* Get error status to check for invalid configurations */
692 rslt = cfg_error_status(drv);
693 if(rslt != BMI2_OK) {
694 printf("setting ACC failed, rslt = %d\n", rslt);
695 ret = -1;
696 return ret;
697 }
698
699 /* enable advance power save if needed */
700 if(aps_status == BMI2_ENABLE) {
701 /* Get status of advance power save mode */
702 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
703 I2C_OP_RETRIES);
704 if(unlikely(ret) != 0) {
705 printf("read PWR_CONF_ADDR failed \n");
706 return ret;
707 }
708 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
709 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
710 I2C_OP_RETRIES);
711 aos_msleep(3);
712 if(unlikely(ret) != 0) {
713 printf("write PWR_CONF_ADDR failed \n");
714 return ret;
715 }
716 }
717
718 return 0;
719 }
720
721 /**
722 * This function sets the acc powermode
723 *
724 * @param[in] drv pointer to the i2c dev
725 * @param[in] mode the powermode to be setted
726 * @return the operation status, 0 is OK, others is error
727 */
drv_acc_bosch_bmi260_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)728 static int drv_acc_bosch_bmi260_set_power_mode(i2c_dev_t* drv,
729 dev_power_mode_e mode)
730 {
731 int ret = 0;
732
733 switch(mode) {
734 case DEV_POWER_ON: {
735 if(g_gyro_active_count > 0) {
736 /* use IMU setting */
737 ret = drv_acc_gyro_bosch_bmi260_enable(drv);
738 } else if(g_gyro_active_count == 0) {
739 /* use ACC only setting */
740 ret = drv_acc_bosch_bmi260_enable(drv);
741 } else {
742 printf("g_gyro_active_count error gyro_active_count %d\n",
743 g_gyro_active_count);
744 ret = -1;
745 }
746
747 aos_msleep(2);
748 if(unlikely(ret) != 0) {
749 return ret;
750 } else {
751 if(g_acc_active_count == 0)
752 g_acc_active_count++;
753 }
754 }
755 break;
756
757 case DEV_POWER_OFF:
758 case DEV_SLEEP: {
759
760 if(g_gyro_active_count >= 0) {
761 ret = drv_acc_gyro_bosch_bmi260_disable(drv);
762 } else {
763 printf("g_gyro_active_count error g_gyro_active_count %d\n",
764 g_gyro_active_count);
765 ret = -1;
766 }
767 aos_msleep(2);
768 if(unlikely(ret) != 0) {
769 return ret;
770 } else {
771 if(g_acc_active_count > 0)
772 g_acc_active_count = 0;
773 }
774 }
775 break;
776 default:
777 break;
778 }
779 return 0;
780 }
781
782 /**
783 * This function gets the XYZ data of acc
784 *
785 * @param[in] drv pointer to the i2c dev
786 * @param[in out] x pointer to the acc x data
787 * @param[in out] y pointer to the acc y data
788 * @param[in out] z pointer to the acc z data
789 * @return the operation status, 0 is OK, others is error
790 */
drv_acc_bosch_bmi260_getXYZ(i2c_dev_t * drv,int16_t * x,int16_t * y,int16_t * z)791 static uint8_t drv_acc_bosch_bmi260_getXYZ(i2c_dev_t* drv, int16_t* x,
792 int16_t* y, int16_t* z)
793 {
794 int ret = 0;
795 uint8_t msb; /* Variables to store msb value */
796 uint8_t lsb; /* Variables to store lsb value */
797 uint16_t msb_lsb; /* Variables to store both msb and lsb value */
798 uint8_t index = 0; /* Variables to define index */
799 uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = {0}; /* Array to define data stored in register */
800
801 ret = sensor_i2c_read(drv, BMI2_ACC_X_LSB_ADDR, ®_data[0],
802 BMI2_ACC_GYR_NUM_BYTES, I2C_OP_RETRIES);
803 if(unlikely(ret) != 0) {
804 printf("read BMI2_GYR_CONF_ADDR failed \n");
805 return ret;
806 }
807
808 /* Read x-axis data */
809 lsb = reg_data[index++];
810 msb = reg_data[index++];
811 msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb;
812 *x = (int16_t)msb_lsb;
813
814 /* Read y-axis data */
815 lsb = reg_data[index++];
816 msb = reg_data[index++];
817 msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb;
818 *y = (int16_t)msb_lsb;
819
820 /* Read z-axis data */
821 lsb = reg_data[index++];
822 msb = reg_data[index++];
823 msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb;
824 *z = (int16_t)msb_lsb;
825
826 return 0;
827 }
828
829 /**
830 * This function gets the acc ODR according to HZ
831 *
832 * @param[in] drv pointer to the i2c dev
833 * @param[in] hz the frequency required
834 * @return the conrresponding acc ODR
835 */
drv_acc_bosch_bmi260_hz2odr(uint32_t hz)836 static uint8_t drv_acc_bosch_bmi260_hz2odr(uint32_t hz)
837 {
838 if(hz > 800)
839 return BMI2_ACC_ODR_1600HZ;
840 else if(hz > 400)
841 return BMI2_ACC_ODR_800HZ;
842 else if(hz > 200)
843 return BMI2_ACC_ODR_400HZ;
844 else if(hz > 100)
845 return BMI2_ACC_ODR_200HZ;
846 else if(hz > 50)
847 return BMI2_ACC_ODR_100HZ;
848 else if(hz > 25)
849 return BMI2_ACC_ODR_50HZ;
850 else if(hz > 12)
851 return BMI2_ACC_ODR_25HZ;
852 else if(hz > 6)
853 return BMI2_ACC_ODR_12_5HZ;
854 else if(hz > 3)
855 return BMI2_ACC_ODR_6_25HZ;
856 else if(hz >= 1)
857 return BMI2_ACC_ODR_3_12HZ;
858 else
859 return BMI2_ACC_ODR_1_56HZ;
860
861 }
862
863 /**
864 * This function sets the acc ODR
865 *
866 * @param[in] drv pointer to the i2c dev
867 * @param[in] hz the frequency required
868 * @return the operation status, 0 is OK, others is error
869 */
drv_acc_bosch_bmi260_set_odr(i2c_dev_t * drv,uint32_t hz)870 static int drv_acc_bosch_bmi260_set_odr(i2c_dev_t* drv, uint32_t hz)
871 {
872 int ret = 0;
873 uint8_t odr = drv_acc_bosch_bmi260_hz2odr(hz);
874 uint8_t reg_data = 0; /* Variable to store register values */
875 uint8_t aps_status = 0; /* Variable to store adv power status */
876 int8_t rslt = 0; /* Variable to define error */
877
878 /* Get status of advance power save mode */
879 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
880 I2C_OP_RETRIES);
881 if(unlikely(ret) != 0) {
882 printf("read PWR_CONF_ADDR failed \n");
883 return ret;
884 }
885 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
886 if((aps_status == BMI2_ENABLE)) {
887 /* Disable advance power save if enabled */
888 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
889 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
890 I2C_OP_RETRIES);
891 aos_msleep(3);
892 if(unlikely(ret) != 0) {
893 printf("write PWR_CONF_ADDR failed \n");
894 return ret;
895 }
896 }
897
898
899 /* set odr nomatter what working mode */
900 /* get ACC_CONFIG */
901 ret = sensor_i2c_read(drv, BMI2_ACC_CONF_ADDR, ®_data, I2C_DATA_LEN,
902 I2C_OP_RETRIES);
903 if(unlikely(ret) != 0) {
904 printf("read BMI2_ACC_CONF_ADDR failed \n");
905 return ret;
906 }
907 /* Set accelerometer ODR */
908 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ACC_ODR, odr);
909
910 /* Write accelerometer configuration to ACC_CONF */
911 ret = sensor_i2c_write(drv, BMI2_ACC_CONF_ADDR, ®_data, I2C_DATA_LEN,
912 I2C_OP_RETRIES);
913 aos_msleep(3);
914 if(unlikely(ret) != 0) {
915 printf("write BMI2_ACC_CONF_ADDR failed \n");
916 return ret;
917 }
918
919 /* Get error status to check for invalid configurations */
920 rslt = cfg_error_status(drv);
921 if(rslt != BMI2_OK) {
922 printf("setting ACC failed, rslt = %d\n", rslt);
923 ret = -1;
924 return ret;
925 }
926
927 /* enable advance power save if needed */
928 if(aps_status == BMI2_ENABLE) {
929 /* Get status of advance power save mode */
930 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
931 I2C_OP_RETRIES);
932 if(unlikely(ret) != 0) {
933 printf("read PWR_CONF_ADDR failed \n");
934 return ret;
935 }
936 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
937 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
938 I2C_OP_RETRIES);
939 aos_msleep(3);
940 if(unlikely(ret) != 0) {
941 printf("write PWR_CONF_ADDR failed \n");
942 return ret;
943 }
944 }
945
946 return 0;
947 }
948
949 /**
950 * This function sets the acc range
951 *
952 * @param[in] drv pointer to the i2c dev
953 * @param[in] hz the range required
954 * @return the operation status, 0 is OK, others is error
955 */
drv_acc_bosch_bmi260_set_range(i2c_dev_t * drv,uint32_t range)956 static int drv_acc_bosch_bmi260_set_range(i2c_dev_t* drv, uint32_t range)
957 {
958 int ret = 0;
959 uint8_t tmp = 0;
960 uint8_t reg_data = 0; /* Variable to store register values */
961 uint8_t aps_status = 0; /* Variable to store adv power status */
962 int8_t rslt = 0; /* Variable to define error */
963
964 if(unlikely(ret) != 0) {
965 return ret;
966 }
967
968 switch(range) {
969 case ACC_RANGE_2G: {
970 tmp = BMI2_ACC_RANGE_2G;
971 }
972 break;
973
974 case ACC_RANGE_4G: {
975 tmp = BMI2_ACC_RANGE_4G;
976 }
977 break;
978
979 case ACC_RANGE_8G: {
980 tmp = BMI2_ACC_RANGE_8G;
981 }
982 break;
983
984 case ACC_RANGE_16G: {
985 tmp = BMI2_ACC_RANGE_16G;
986 }
987 break;
988
989 default:
990 break;
991 }
992
993 /* Get status of advance power save mode */
994 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
995 I2C_OP_RETRIES);
996 if(unlikely(ret) != 0) {
997 printf("read PWR_CONF_ADDR failed \n");
998 return ret;
999 }
1000 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
1001 if((aps_status == BMI2_ENABLE)) {
1002 /* Disable advance power save if enabled */
1003 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
1004 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1005 I2C_OP_RETRIES);
1006 aos_msleep(3);
1007 if(unlikely(ret) != 0) {
1008 printf("write PWR_CONF_ADDR failed \n");
1009 return ret;
1010 }
1011 }
1012 /* set range nomatter what working mode */
1013 /* get ACC_CONFIG1 */
1014 ret = sensor_i2c_read(drv, BMI2_ACC_CONF1_ADDR, ®_data, I2C_DATA_LEN,
1015 I2C_OP_RETRIES);
1016 if(unlikely(ret) != 0) {
1017 printf("read BMI2_ACC_CONF1_ADDR failed \n");
1018 return ret;
1019 }
1020
1021 /* Set accelerometer range */
1022 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ACC_RANGE, tmp);
1023
1024 /* Write accelerometer configuration to ACC_CONF1 */
1025 ret = sensor_i2c_write(drv, BMI2_ACC_CONF1_ADDR, ®_data, I2C_DATA_LEN,
1026 I2C_OP_RETRIES);
1027 aos_msleep(3);
1028 if(unlikely(ret) != 0) {
1029 printf("write BMI2_ACC_CONF_ADDR failed \n");
1030 return ret;
1031 }
1032
1033 /* Get error status to check for invalid configurations */
1034 rslt = cfg_error_status(drv);
1035 if(rslt != BMI2_OK) {
1036 printf("setting ACC failed, rslt = %d\n", rslt);
1037 ret = -1;
1038 return ret;
1039 }
1040
1041 if((range >= ACC_RANGE_2G)&&(range <= ACC_RANGE_16G)) {
1042 g_cur_acc_factor = g_bmi260_acc_factor[range];
1043 }
1044
1045 /* enable advance power save if needed */
1046 if(aps_status == BMI2_ENABLE) {
1047 /* Get status of advance power save mode */
1048 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1049 I2C_OP_RETRIES);
1050 if(unlikely(ret) != 0) {
1051 printf("read PWR_CONF_ADDR failed \n");
1052 return ret;
1053 }
1054 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
1055 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1056 I2C_OP_RETRIES);
1057 aos_msleep(3);
1058 if(unlikely(ret) != 0) {
1059 printf("write PWR_CONF_ADDR failed \n");
1060 return ret;
1061 }
1062 }
1063
1064 return 0;
1065 }
1066
1067 /**
1068 * This function is the ISR
1069 *
1070 * @return
1071 */
drv_acc_bosch_bmi260_irq_handle(void)1072 static void drv_acc_bosch_bmi260_irq_handle(void)
1073 {
1074 /* no handle so far */
1075 }
1076
1077 /**
1078 * This function opens the acc
1079 *
1080 * @return the operation status, 0 is OK, others is error
1081 */
drv_acc_bosch_bmi260_open(void)1082 static int drv_acc_bosch_bmi260_open(void)
1083 {
1084 int ret = 0;
1085
1086 ret = drv_acc_bosch_bmi260_set_power_mode(&bmi260_ctx, DEV_POWER_ON);
1087 if(unlikely(ret) != 0) {
1088 return -1;
1089 }
1090
1091 ret = drv_acc_bosch_bmi260_set_range(&bmi260_ctx, ACC_RANGE_8G);
1092 if(unlikely(ret) != 0) {
1093 return -1;
1094 }
1095
1096 ret = drv_acc_bosch_bmi260_set_odr(&bmi260_ctx, BMI260_ACC_DEFAULT_ODR_25HZ);
1097 if(unlikely(ret) != 0) {
1098 return -1;
1099 }
1100 LOG("%s %s successfully \n", SENSOR_STR, __func__);
1101 return 0;
1102
1103 }
1104
1105 /**
1106 * This function closes the acc
1107 *
1108 * @return the operation status, 0 is OK, others is error
1109 */
drv_acc_bosch_bmi260_close(void)1110 static int drv_acc_bosch_bmi260_close(void)
1111 {
1112 int ret = 0;
1113 ret = drv_acc_bosch_bmi260_set_power_mode(&bmi260_ctx, DEV_POWER_OFF);
1114 if(unlikely(ret) != 0) {
1115 return -1;
1116 }
1117 return 0;
1118 }
1119
1120 /**
1121 * This function reads the acc data and reports the data
1122 *
1123 * @param[in out] buf buffer for acc data
1124 * @param[in out] len length of data
1125 * @return the operation status, 0 is OK, others is error
1126 */
drv_acc_bosch_bmi260_read(void * buf,size_t len)1127 static int drv_acc_bosch_bmi260_read(void *buf, size_t len)
1128 {
1129 int ret = 0;
1130 int ret_getXYZ = 0;
1131 size_t size;
1132 uint8_t reg_data = 0; /* Variable to store register values */
1133 uint8_t aps_status = 0; /* Variable to store adv power status */
1134 int16_t x = 0;
1135 int16_t y = 0;
1136 int16_t z = 0;
1137 accel_data_t *accel = (accel_data_t *)buf;
1138 if(buf == NULL) {
1139 return -1;
1140 }
1141
1142 size = sizeof(accel_data_t);
1143 if(len < size) {
1144 return -1;
1145 }
1146
1147 /* Get status of advance power save mode */
1148 ret = sensor_i2c_read(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1149 I2C_OP_RETRIES);
1150 if(unlikely(ret) != 0) {
1151 printf("read PWR_CONF_ADDR failed \n");
1152 return ret;
1153 }
1154 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
1155 if((aps_status == BMI2_ENABLE)) {
1156 /* Disable advance power save if enabled */
1157 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
1158 ret = sensor_i2c_write(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1159 I2C_OP_RETRIES);
1160 aos_msleep(3);
1161 if(unlikely(ret) != 0) {
1162 printf("write PWR_CONF_ADDR failed \n");
1163 return ret;
1164 }
1165 }
1166
1167 ret = drv_acc_bosch_bmi260_getXYZ(&bmi260_ctx, &x, &y, &z);
1168 if(unlikely(ret)) {
1169 printf("read ACC XYZ failed \n");
1170 ret_getXYZ = ret;
1171 }
1172
1173 /* enable advance power save if needed */
1174 if(aps_status == BMI2_ENABLE) {
1175 /* Get status of advance power save mode */
1176 ret = sensor_i2c_read(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1177 I2C_OP_RETRIES);
1178 if(unlikely(ret) != 0) {
1179 printf("read PWR_CONF_ADDR failed \n");
1180 return ret;
1181 }
1182 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
1183 ret = sensor_i2c_write(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1184 I2C_OP_RETRIES);
1185 aos_msleep(3);
1186 if(unlikely(ret) != 0) {
1187 printf("write PWR_CONF_ADDR failed \n");
1188 return ret;
1189 }
1190 }
1191
1192 if(unlikely(ret_getXYZ)) {
1193 return -1;
1194 }
1195
1196 if(g_cur_acc_factor != 0) {
1197 /* the unit of acc is mg, 1000 mg = 1 g */
1198 accel->data[DATA_AXIS_X] = x * ACCELEROMETER_UNIT_FACTOR /
1199 (int32_t)g_cur_acc_factor;
1200 accel->data[DATA_AXIS_Y] = y * ACCELEROMETER_UNIT_FACTOR /
1201 (int32_t)g_cur_acc_factor;
1202 accel->data[DATA_AXIS_Z] = z * ACCELEROMETER_UNIT_FACTOR /
1203 (int32_t)g_cur_acc_factor;
1204 }
1205
1206 accel->timestamp = aos_now_ms();
1207
1208 return (int)size;
1209 }
1210
1211 /**
1212 * This function is for the acc ioctl
1213 *
1214 * @param[in] cmd the ioctl command
1215 * @param[in] arg the correspondding parameter
1216 * @return the operation status, 0 is OK, others is error
1217 */
drv_acc_bosch_bmi260_ioctl(int cmd,unsigned long arg)1218 static int drv_acc_bosch_bmi260_ioctl(int cmd, unsigned long arg)
1219 {
1220 int ret = 0;
1221
1222 switch(cmd) {
1223 case SENSOR_IOCTL_ODR_SET: {
1224 ret = drv_acc_bosch_bmi260_set_odr(&bmi260_ctx, arg);
1225 if(unlikely(ret) != 0) {
1226 return -1;
1227 }
1228 }
1229 break;
1230 case SENSOR_IOCTL_RANGE_SET: {
1231 ret = drv_acc_bosch_bmi260_set_range(&bmi260_ctx, arg);
1232 if(unlikely(ret) != 0) {
1233 return -1;
1234 }
1235 }
1236 break;
1237 case SENSOR_IOCTL_SET_POWER: {
1238 ret = drv_acc_bosch_bmi260_set_power_mode(&bmi260_ctx, arg);
1239 if(unlikely(ret) != 0) {
1240 return -1;
1241 }
1242 }
1243 break;
1244 case SENSOR_IOCTL_GET_INFO: {
1245 /* fill the dev info here */
1246 dev_sensor_info_t *info = (dev_sensor_info_t*)arg;
1247 info->model = "BMI260";
1248 info->range_max = 16;
1249 info->range_min = 2;
1250 info->unit = mg;
1251 }
1252 break;
1253
1254 default:
1255 break;
1256 }
1257 return 0;
1258 }
1259
1260 /**
1261 * This function is for the acc initialization
1262 *
1263 * @return the operation status, 0 is OK, others is error
1264 */
drv_acc_bosch_bmi260_init(void)1265 int drv_acc_bosch_bmi260_init(void) {
1266 printf("drv_acc_bosch_bmi260_init started \n");
1267 int ret = 0;
1268 sensor_obj_t sensor;
1269 memset(&sensor, 0, sizeof(sensor));
1270
1271 /* fill the sensor obj parameters here */
1272 sensor.io_port = I2C_PORT;
1273 sensor.tag = TAG_DEV_ACC;
1274 sensor.path = dev_acc_path;
1275 sensor.open = drv_acc_bosch_bmi260_open;
1276 sensor.close = drv_acc_bosch_bmi260_close;
1277 sensor.read = drv_acc_bosch_bmi260_read;
1278 sensor.write = NULL;
1279 sensor.ioctl = drv_acc_bosch_bmi260_ioctl;
1280 sensor.irq_handle = drv_acc_bosch_bmi260_irq_handle;
1281
1282 ret = sensor_create_obj(&sensor);
1283 if(unlikely(ret) != 0) {
1284 printf("sensor_create_obj failed \n");
1285 return -1;
1286 }
1287 ret = drv_acc_gyro_bosch_bmi260_validate_id(&bmi260_ctx, BMI260_CHIP_ID);
1288 if(unlikely(ret) != 0) {
1289 printf("drv_acc_gyro_bosch_bmi260_validate_id failed \n");
1290 return -1;
1291 }
1292
1293 if(0 == g_bmi260flag) {
1294 ret = drv_acc_gyro_bosch_bmi260_soft_reset(&bmi260_ctx);
1295 if(unlikely(ret) != 0) {
1296 printf("drv_acc_gyro_bosch_bmi260_soft_reset failed \n");
1297 return -1;
1298 }
1299 ret = drv_acc_bosch_bmi260_set_power_mode(&bmi260_ctx, DEV_POWER_OFF);
1300 if(unlikely(ret) != 0) {
1301 printf("drv_acc_bosch_bmi260_set_power_mode failed \n");
1302 return -1;
1303 }
1304 g_bmi260flag = 1;
1305 } else {
1306 LOG("%s %s acc do not need reset\n", SENSOR_STR, __func__);
1307 }
1308
1309 LOG("%s %s successfully \n", SENSOR_STR, __func__);
1310 return 0;
1311 }
1312
1313 /**
1314 * This function enables the gyro
1315 *
1316 * @param[in] drv pointer to the i2c dev
1317 * @return the operation status, 0 is OK, others is error
1318 */
drv_gyro_bosch_bmi260_enable(i2c_dev_t * drv)1319 static int drv_gyro_bosch_bmi260_enable(i2c_dev_t* drv)
1320 {
1321 int ret = 0;
1322 uint8_t reg_data = 0; /* Variable to store register values */
1323 uint8_t aps_status = 0; /* Variable to store adv power status */
1324 int8_t rslt = 0; /* Variable to define error */
1325
1326 if(drv == NULL) {
1327 return -1;
1328 }
1329
1330 /* first enable ACC and GYRO */
1331 ret = sensor_i2c_read(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
1332 I2C_OP_RETRIES);
1333 if(unlikely(ret) != 0) {
1334 printf("read PWR_CTRL failed \n");
1335 return ret;
1336 }
1337
1338 /* enable ACC */
1339 reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_DISABLE);
1340 /* enable GYRO */
1341 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE);
1342
1343 ret = sensor_i2c_write(drv, BMI2_PWR_CTRL_ADDR, ®_data, I2C_DATA_LEN,
1344 I2C_OP_RETRIES);
1345 aos_msleep(3);
1346 if(unlikely(ret) != 0) {
1347 printf("write PWR_CTRL failed \n");
1348 return ret;
1349 }
1350
1351 /* Get status of advance power save mode */
1352 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1353 I2C_OP_RETRIES);
1354 if(unlikely(ret) != 0) {
1355 printf("read PWR_CONF_ADDR failed \n");
1356 return ret;
1357 }
1358 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
1359 if((aps_status == BMI2_ENABLE)) {
1360 /* Disable advance power save if enabled */
1361 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
1362 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1363 I2C_OP_RETRIES);
1364 aos_msleep(3);
1365 if(unlikely(ret) != 0) {
1366 printf("write PWR_CONF_ADDR failed \n");
1367 return ret;
1368 }
1369 }
1370
1371 /* get GYRO_CONFIG */
1372 ret = sensor_i2c_read(drv, BMI2_GYR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1373 I2C_OP_RETRIES);
1374 if(unlikely(ret) != 0) {
1375 printf("read BMI2_GYR_CONF_ADDR failed \n");
1376 return ret;
1377 }
1378 /* enable gyroscope performance mode */
1379 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_PERF_MODE, BMI2_ENABLE);
1380
1381 /* disable gyroscope high performance/low-power mode */
1382 reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_DSLP, BMI2_DISABLE);
1383
1384 /* Write accelerometer configuration to GYR_CONF */
1385 ret = sensor_i2c_write(drv, BMI2_GYR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1386 I2C_OP_RETRIES);
1387 aos_msleep(3);
1388 if(unlikely(ret) != 0) {
1389 printf("write BMI2_GYR_CONF_ADDR failed \n");
1390 return ret;
1391 }
1392
1393 /* Get error status to check for invalid configurations */
1394 rslt = cfg_error_status(drv);
1395 if(rslt != BMI2_OK) {
1396 printf("setting GYRO failed, rslt = %d\n", rslt);
1397 ret = -1;
1398 return ret;
1399 }
1400
1401 /* enable advance power save if needed */
1402 if(aps_status == BMI2_ENABLE) {
1403 /* Get status of advance power save mode */
1404 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1405 I2C_OP_RETRIES);
1406 if(unlikely(ret) != 0) {
1407 printf("read PWR_CONF_ADDR failed \n");
1408 return ret;
1409 }
1410 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
1411 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1412 I2C_OP_RETRIES);
1413 aos_msleep(3);
1414 if(unlikely(ret) != 0) {
1415 printf("write PWR_CONF_ADDR failed \n");
1416 return ret;
1417 }
1418 }
1419
1420 return 0;
1421 }
1422
1423 /**
1424 * This function sets the gyro powermode
1425 *
1426 * @param[in] drv pointer to the i2c dev
1427 * @param[in] mode the powermode to be setted
1428 * @return the operation status, 0 is OK, others is error
1429 */
drv_gyro_bosch_bmi260_set_power_mode(i2c_dev_t * drv,dev_power_mode_e mode)1430 static int drv_gyro_bosch_bmi260_set_power_mode(i2c_dev_t* drv,
1431 dev_power_mode_e mode)
1432 {
1433 int ret = 0;
1434
1435 switch(mode) {
1436 case DEV_POWER_ON: {
1437
1438 if(g_acc_active_count > 0) {
1439 /*use IMU setting*/
1440 ret = drv_acc_gyro_bosch_bmi260_enable(drv);
1441 } else if(g_acc_active_count == 0) {
1442 /*use gyro only setting*/
1443 ret = drv_gyro_bosch_bmi260_enable(drv);
1444 } else {
1445 printf("g_acc_active_count error g_acc_active_count %d\n", g_acc_active_count);
1446 ret = -1;
1447 }
1448
1449 aos_msleep(2);
1450 if(unlikely(ret) != 0) {
1451 return ret;
1452 } else {
1453 if(g_gyro_active_count == 0)
1454 g_gyro_active_count++;
1455 }
1456 }
1457 break;
1458
1459 case DEV_POWER_OFF:
1460 case DEV_SLEEP: {
1461 if(g_acc_active_count >= 0) {
1462 ret = drv_acc_gyro_bosch_bmi260_disable(drv);
1463 } else {
1464 printf("g_acc_active_count error g_acc_active_count %d\n", g_acc_active_count);
1465 ret = -1;
1466 }
1467 aos_msleep(2);
1468 if(unlikely(ret) != 0) {
1469 return ret;
1470 } else {
1471 if(g_gyro_active_count > 0)
1472 g_gyro_active_count = 0;
1473 }
1474 }
1475 break;
1476 default:
1477 break;
1478 }
1479 return 0;
1480 }
1481
1482 /**
1483 * This function gets the XYZ data of gyro
1484 *
1485 * @param[in] drv pointer to the i2c dev
1486 * @param[in out] x pointer to the gyro x data
1487 * @param[in out] y pointer to the gyro y data
1488 * @param[in out] z pointer to the gyro z data
1489 * @return the operation status, 0 is OK, others is error
1490 */
drv_gyro_bosch_bmi260_getXYZ(i2c_dev_t * drv,int16_t * x,int16_t * y,int16_t * z)1491 static uint8_t drv_gyro_bosch_bmi260_getXYZ(i2c_dev_t* drv, int16_t* x,
1492 int16_t* y, int16_t* z)
1493 {
1494 int ret = 0;
1495 uint8_t msb; /* Variables to store msb value */
1496 uint8_t lsb; /* Variables to store lsb value */
1497 uint16_t msb_lsb; /* Variables to store both msb and lsb value */
1498 uint8_t index = 0; /* Variables to define index */
1499 uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = {0}; /* Array to define data stored in register */
1500
1501 ret = sensor_i2c_read(drv, BMI2_GYR_X_LSB_ADDR, ®_data[0],
1502 BMI2_ACC_GYR_NUM_BYTES, I2C_OP_RETRIES);
1503 if(unlikely(ret) != 0) {
1504 printf("read BMI2_GYR_CONF_ADDR failed \n");
1505 return ret;
1506 }
1507
1508 /* Read x-axis data */
1509 lsb = reg_data[index++];
1510 msb = reg_data[index++];
1511 msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb;
1512 *x = (int16_t)msb_lsb;
1513
1514 /* Read y-axis data */
1515 lsb = reg_data[index++];
1516 msb = reg_data[index++];
1517 msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb;
1518 *y = (int16_t)msb_lsb;
1519
1520 /* Read z-axis data */
1521 lsb = reg_data[index++];
1522 msb = reg_data[index++];
1523 msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb;
1524 *z = (int16_t)msb_lsb;
1525
1526 return 0;
1527
1528 }
1529
1530 /**
1531 * This function gets the gyro ODR according to HZ
1532 *
1533 * @param[in] drv pointer to the i2c dev
1534 * @param[in] hz the frequency required
1535 * @return the conrresponding gyro ODR
1536 */
drv_gyro_bosch_bmi260_hz2odr(uint32_t hz)1537 static uint8_t drv_gyro_bosch_bmi260_hz2odr(uint32_t hz)
1538 {
1539 if(hz > 1600)
1540 return BMI2_GYR_ODR_3200HZ ;
1541 else if(hz > 800)
1542 return BMI2_GYR_ODR_1600HZ;
1543 else if(hz > 400)
1544 return BMI2_GYR_ODR_800HZ;
1545 else if(hz > 200)
1546 return BMI2_GYR_ODR_400HZ;
1547 else if(hz > 100)
1548 return BMI2_GYR_ODR_200HZ;
1549 else if(hz > 50)
1550 return BMI2_GYR_ODR_100HZ;
1551 else if(hz > 25)
1552 return BMI2_GYR_ODR_50HZ;
1553 else
1554 return BMI2_GYR_ODR_25HZ;
1555
1556 }
1557
1558 /**
1559 * This function sets the gyro ODR
1560 *
1561 * @param[in] drv pointer to the i2c dev
1562 * @param[in] hz the frequency required
1563 * @return the operation status, 0 is OK, others is error
1564 */
drv_gyro_bosch_bmi260_set_odr(i2c_dev_t * drv,uint32_t hz)1565 static int drv_gyro_bosch_bmi260_set_odr(i2c_dev_t* drv, uint32_t hz)
1566 {
1567 int ret = 0;
1568 uint8_t odr = drv_gyro_bosch_bmi260_hz2odr(hz);
1569 uint8_t reg_data = 0; /* Variable to store register values */
1570 uint8_t aps_status = 0; /* Variable to store adv power status */
1571 int8_t rslt = 0; /* Variable to define error */
1572
1573 /* Get status of advance power save mode */
1574 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1575 I2C_OP_RETRIES);
1576 if(unlikely(ret) != 0) {
1577 printf("read PWR_CONF_ADDR failed \n");
1578 return ret;
1579 }
1580 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
1581 if((aps_status == BMI2_ENABLE)) {
1582 /* Disable advance power save if enabled */
1583 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
1584 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1585 I2C_OP_RETRIES);
1586 aos_msleep(3);
1587 if(unlikely(ret) != 0) {
1588 printf("write PWR_CONF_ADDR failed \n");
1589 return ret;
1590 }
1591 }
1592
1593 /* set gyro ODR setting */
1594
1595 /*get GYRO_CONFIG*/
1596 ret = sensor_i2c_read(drv, BMI2_GYR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1597 I2C_OP_RETRIES);
1598 if(unlikely(ret) != 0) {
1599 printf("read BMI2_GYR_CONF_ADDR failed \n");
1600 return ret;
1601 }
1602
1603 /* Set gyroscope ODR */
1604 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_ODR, odr);
1605
1606 /* Write configuration to GYR_CONF */
1607 ret = sensor_i2c_write(drv, BMI2_GYR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1608 I2C_OP_RETRIES);
1609 aos_msleep(3);
1610 if(unlikely(ret) != 0) {
1611 printf("write BMI2_GYR_CONF_ADDR failed \n");
1612 return ret;
1613 }
1614
1615 /* Get error status to check for invalid configurations */
1616 rslt = cfg_error_status(drv);
1617 if(rslt != BMI2_OK) {
1618 printf("setting GYRO failed, rslt = %d\n", rslt);
1619 ret = -1;
1620 return ret;
1621 }
1622
1623 /* enable advance power save if needed */
1624 if(aps_status == BMI2_ENABLE) {
1625 /* Get status of advance power save mode */
1626 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1627 I2C_OP_RETRIES);
1628 if(unlikely(ret) != 0) {
1629 printf("read PWR_CONF_ADDR failed \n");
1630 return ret;
1631 }
1632 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
1633 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1634 I2C_OP_RETRIES);
1635 aos_msleep(3);
1636 if(unlikely(ret) != 0) {
1637 printf("write PWR_CONF_ADDR failed \n");
1638 return ret;
1639 }
1640 }
1641
1642 return 0;
1643 }
1644
1645 /**
1646 * This function sets the gyro range
1647 *
1648 * @param[in] drv pointer to the i2c dev
1649 * @param[in] hz the range required
1650 * @return the operation status, 0 is OK, others is error
1651 */
drv_gyro_bosch_bmi260_set_range(i2c_dev_t * drv,uint32_t range)1652 static int drv_gyro_bosch_bmi260_set_range(i2c_dev_t* drv, uint32_t range)
1653 {
1654 int ret = 0;
1655 uint8_t tmp = 0;
1656 uint8_t reg_data = 0; /* Variable to store register values */
1657 uint8_t aps_status = 0; /* Variable to store adv power status */
1658 int8_t rslt = 0; /* Variable to define error */
1659
1660 switch(range) {
1661 case GYRO_RANGE_125DPS: {
1662 tmp = BMI2_GYR_RANGE_125;
1663 }
1664 break;
1665 case GYRO_RANGE_250DPS: {
1666 tmp = BMI2_GYR_RANGE_250;
1667 }
1668 break;
1669
1670 case GYRO_RANGE_500DPS: {
1671 tmp = BMI2_GYR_RANGE_500;
1672 }
1673 break;
1674
1675 case GYRO_RANGE_1000DPS: {
1676 tmp = BMI2_GYR_RANGE_1000;
1677 }
1678 break;
1679
1680 case GYRO_RANGE_2000DPS: {
1681 tmp = BMI2_GYR_RANGE_2000;
1682 }
1683 break;
1684
1685 default:
1686 break;
1687 }
1688
1689 /* Get status of advance power save mode */
1690 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1691 I2C_OP_RETRIES);
1692 if(unlikely(ret) != 0) {
1693 printf("read PWR_CONF_ADDR failed \n");
1694 return ret;
1695 }
1696 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
1697 if((aps_status == BMI2_ENABLE)) {
1698 /* Disable advance power save if enabled */
1699 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
1700 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1701 I2C_OP_RETRIES);
1702 aos_msleep(3);
1703 if(unlikely(ret) != 0) {
1704 printf("write PWR_CONF_ADDR failed \n");
1705 return ret;
1706 }
1707 }
1708
1709 /* set gyro range setting */
1710 /* get GYRO_CONFIG1 */
1711 ret = sensor_i2c_read(drv, BMI2_GYR_CONF1_ADDR, ®_data, I2C_DATA_LEN,
1712 I2C_OP_RETRIES);
1713 if(unlikely(ret) != 0) {
1714 printf("read BMI2_GYR_CONF1_ADDR failed \n");
1715 return ret;
1716 }
1717
1718 /* Set gyroscope range */
1719 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_RANGE, tmp);
1720
1721 /* Write configuration to GYR_CONF1 */
1722 ret = sensor_i2c_write(drv, BMI2_GYR_CONF1_ADDR, ®_data, I2C_DATA_LEN,
1723 I2C_OP_RETRIES);
1724 aos_msleep(3);
1725 if(unlikely(ret) != 0) {
1726 printf("write BMI2_GYR_CONF1_ADDR failed \n");
1727 return ret;
1728 }
1729
1730 /* Get error status to check for invalid configurations */
1731 rslt = cfg_error_status(drv);
1732 if(rslt != BMI2_OK) {
1733 printf("setting GYRO failed, rslt = %d\n", rslt);
1734 ret = -1;
1735 return ret;
1736 }
1737
1738 if((range >= GYRO_RANGE_250DPS)&&(range <= GYRO_RANGE_2000DPS)) {
1739 g_cur_gyro_factor = g_bmi260_gyro_factor[range];
1740 }
1741
1742 /* enable advance power save if needed */
1743 if(aps_status == BMI2_ENABLE) {
1744 /* Get status of advance power save mode */
1745 ret = sensor_i2c_read(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1746 I2C_OP_RETRIES);
1747 if(unlikely(ret) != 0) {
1748 printf("read PWR_CONF_ADDR failed \n");
1749 return ret;
1750 }
1751 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
1752 ret = sensor_i2c_write(drv, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1753 I2C_OP_RETRIES);
1754 aos_msleep(3);
1755 if(unlikely(ret) != 0) {
1756 printf("write PWR_CONF_ADDR failed \n");
1757 return ret;
1758 }
1759 }
1760
1761 return 0;
1762 }
1763
1764 /**
1765 * This function is ISR
1766 *
1767 * @return
1768 */
drv_gyro_bosch_bmi260_irq_handle(void)1769 static void drv_gyro_bosch_bmi260_irq_handle(void)
1770 {
1771 /* no handle so far */
1772 }
1773
1774 /**
1775 * This function opens the acc
1776 *
1777 * @return the operation status, 0 is OK, others is error
1778 */
drv_gyro_bosch_bmi260_open(void)1779 static int drv_gyro_bosch_bmi260_open(void)
1780 {
1781 int ret = 0;
1782 ret = drv_gyro_bosch_bmi260_set_power_mode(&bmi260_ctx, DEV_POWER_ON);
1783 if(unlikely(ret) != 0) {
1784 return -1;
1785 }
1786
1787 ret = drv_gyro_bosch_bmi260_set_range(&bmi260_ctx, GYRO_RANGE_1000DPS);
1788 if(unlikely(ret) != 0) {
1789 return -1;
1790 }
1791
1792 ret = drv_gyro_bosch_bmi260_set_odr(&bmi260_ctx, BMI260_GYRO_DEFAULT_ODR_25HZ);
1793 if(unlikely(ret) != 0) {
1794 return -1;
1795 }
1796
1797 return 0;
1798
1799 }
1800
1801 /**
1802 * This function closes the gyro
1803 *
1804 * @return the operation status, 0 is OK, others is error
1805 */
drv_gyro_bosch_bmi260_close(void)1806 static int drv_gyro_bosch_bmi260_close(void)
1807 {
1808 int ret = 0;
1809 ret = drv_gyro_bosch_bmi260_set_power_mode(&bmi260_ctx, DEV_POWER_OFF);
1810 if(unlikely(ret) != 0) {
1811 return -1;
1812 }
1813 return 0;
1814 }
1815
1816 /**
1817 * This function reads the gyro data and reports the data
1818 *
1819 * @param[in out] buf buffer for gyro data
1820 * @param[in out] len length of data
1821 * @return the operation status, 0 is OK, others is error
1822 */
drv_gyro_bosch_bmi260_read(void * buf,size_t len)1823 static int drv_gyro_bosch_bmi260_read(void *buf, size_t len)
1824 {
1825 int ret = 0;
1826 int ret_getXYZ = 0;
1827 size_t size;
1828 int16_t x = 0;
1829 int16_t y = 0;
1830 int16_t z = 0;
1831 uint8_t reg_data = 0; /* Variable to store register values */
1832 uint8_t aps_status = 0; /* Variable to store adv power status */
1833 gyro_data_t *gyro = (gyro_data_t *)buf;
1834 if(buf == NULL) {
1835 return -1;
1836 }
1837
1838 size = sizeof(gyro_data_t);
1839 if(len < size) {
1840 return -1;
1841 }
1842
1843 /* Get status of advance power save mode */
1844 ret = sensor_i2c_read(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1845 I2C_OP_RETRIES);
1846 if(unlikely(ret) != 0) {
1847 printf("read PWR_CONF_ADDR failed \n");
1848 return ret;
1849 }
1850 aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN);
1851 if((aps_status == BMI2_ENABLE)) {
1852 /* Disable advance power save if enabled */
1853 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_DISABLE);
1854 ret = sensor_i2c_write(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1855 I2C_OP_RETRIES);
1856 aos_msleep(3);
1857 if(unlikely(ret) != 0) {
1858 printf("write PWR_CONF_ADDR failed \n");
1859 return ret;
1860 }
1861 }
1862
1863 ret = drv_gyro_bosch_bmi260_getXYZ(&bmi260_ctx, &x, &y, &z);
1864 if(unlikely(ret)) {
1865 printf("!!!!read gyro XYZ failed \n");
1866 ret_getXYZ = ret;
1867 }
1868
1869 /* enable advance power save if needed */
1870 if(aps_status == BMI2_ENABLE) {
1871 /* Get status of advance power save mode */
1872 ret = sensor_i2c_read(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1873 I2C_OP_RETRIES);
1874 if(unlikely(ret) != 0) {
1875 printf("!!!!read PWR_CONF_ADDR failed \n");
1876 return ret;
1877 }
1878 reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, BMI2_ENABLE);
1879 ret = sensor_i2c_write(&bmi260_ctx, BMI2_PWR_CONF_ADDR, ®_data, I2C_DATA_LEN,
1880 I2C_OP_RETRIES);
1881 aos_msleep(3);
1882 if(unlikely(ret) != 0) {
1883
1884 printf("!!!!!write PWR_CONF_ADDR failed \n");
1885 return ret;
1886 }
1887 }
1888
1889 if(unlikely(ret_getXYZ)) {
1890 return -1;
1891 }
1892
1893 if(g_cur_gyro_factor != 0) {
1894 gyro->data[DATA_AXIS_X] = (int32_t)((int64_t)x * GYROSCOPE_UNIT_FACTOR / (int64_t)g_cur_gyro_factor);
1895 gyro->data[DATA_AXIS_Y] = (int32_t)((int64_t)y * GYROSCOPE_UNIT_FACTOR / (int64_t)g_cur_gyro_factor);
1896 gyro->data[DATA_AXIS_Z] = (int32_t)((int64_t)z * GYROSCOPE_UNIT_FACTOR / (int64_t)g_cur_gyro_factor);
1897 //printf("cur_gyro_factor = %d \n", g_cur_gyro_factor);
1898 //printf("x = %d, y = %d, z = %d \n", gyro->data[DATA_AXIS_X], gyro->data[DATA_AXIS_Y], gyro->data[DATA_AXIS_Z]);
1899 } else
1900 printf("g_cur_gyro_factor == 0 \n");
1901 gyro->timestamp = aos_now_ms();
1902
1903 return (int)size;
1904 }
1905
1906 /**
1907 * This function is for the gyro ioctl
1908 *
1909 * @param[in] cmd the ioctl command
1910 * @param[in] arg the correspondding parameter
1911 * @return the operation status, 0 is OK, others is error
1912 */
drv_gyro_bosch_bmi260_ioctl(int cmd,unsigned long arg)1913 static int drv_gyro_bosch_bmi260_ioctl(int cmd, unsigned long arg)
1914 {
1915 int ret = 0;
1916
1917 switch(cmd) {
1918 case SENSOR_IOCTL_ODR_SET: {
1919 ret = drv_gyro_bosch_bmi260_set_odr(&bmi260_ctx, arg);
1920 if(unlikely(ret) != 0) {
1921 return -1;
1922 }
1923 }
1924 break;
1925 case SENSOR_IOCTL_RANGE_SET: {
1926 ret = drv_gyro_bosch_bmi260_set_range(&bmi260_ctx, arg);
1927 if(unlikely(ret) != 0) {
1928 return -1;
1929 }
1930 }
1931 break;
1932 case SENSOR_IOCTL_SET_POWER: {
1933 ret = drv_gyro_bosch_bmi260_set_power_mode(&bmi260_ctx, arg);
1934 if(unlikely(ret) != 0) {
1935 return -1;
1936 }
1937 }
1938 break;
1939 case SENSOR_IOCTL_GET_INFO: {
1940 /* fill the dev info here */
1941 dev_sensor_info_t *info = (dev_sensor_info_t *)arg;
1942 info->model = "BMI260";
1943 info->range_max = 2000;
1944 info->range_min = 125;
1945 info->unit = udps;
1946 }
1947 break;
1948
1949 default:
1950 break;
1951 }
1952
1953 return 0;
1954 }
1955
1956 /**
1957 * This function is for the gyro initialization
1958 *
1959 * @return the operation status, 0 is OK, others is error
1960 */
drv_gyro_bosch_bmi260_init(void)1961 int drv_gyro_bosch_bmi260_init(void) {
1962 int ret = 0;
1963 sensor_obj_t sensor;
1964 memset(&sensor, 0, sizeof(sensor));
1965
1966 /* fill the sensor obj parameters here */
1967 sensor.io_port = I2C_PORT;
1968 sensor.tag = TAG_DEV_GYRO;
1969 sensor.path = dev_gyro_path;
1970 sensor.open = drv_gyro_bosch_bmi260_open;
1971 sensor.close = drv_gyro_bosch_bmi260_close;
1972 sensor.read = drv_gyro_bosch_bmi260_read;
1973 sensor.write = NULL;
1974 sensor.ioctl = drv_gyro_bosch_bmi260_ioctl;
1975 sensor.irq_handle = drv_gyro_bosch_bmi260_irq_handle;
1976
1977 ret = sensor_create_obj(&sensor);
1978 if(unlikely(ret) != 0) {
1979 return -1;
1980 }
1981
1982 ret = drv_acc_gyro_bosch_bmi260_validate_id(&bmi260_ctx, BMI260_CHIP_ID);
1983 if(unlikely(ret) != 0) {
1984 return -1;
1985 }
1986
1987 if(0 == g_bmi260flag) {
1988 ret = drv_acc_gyro_bosch_bmi260_soft_reset(&bmi260_ctx);
1989 if(unlikely(ret) != 0) {
1990 return -1;
1991 }
1992
1993 ret = drv_gyro_bosch_bmi260_set_power_mode(&bmi260_ctx, DEV_POWER_OFF);
1994 if(unlikely(ret) != 0) {
1995 return -1;
1996 }
1997 g_bmi260flag = 1;
1998 }
1999 else {
2000 LOG("%s %s gyro do not need reset\n", SENSOR_STR, __func__);
2001 }
2002
2003 /* update the phy sensor info to sensor hal */
2004 LOG("%s %s successfully \n", SENSOR_STR, __func__);
2005 return 0;
2006 }
2007
2008 SENSOR_DRV_ADD(drv_acc_bosch_bmi260_init);
2009 SENSOR_DRV_ADD(drv_gyro_bosch_bmi260_init);