amiro-lld / source / BNO055 / v1 / alld_BNO055_v1.c @ 5d67f4db
History | View | Annotate | Download (16.291 KB)
1 |
/*
|
---|---|
2 |
AMiRo-LLD is a compilation of low-level hardware drivers for the Autonomous Mini Robot (AMiRo) platform.
|
3 |
Copyright (C) 2016..2019 Thomas Schöpping et al.
|
4 |
|
5 |
This program is free software: you can redistribute it and/or modify
|
6 |
it under the terms of the GNU Lesser General Public License as published by
|
7 |
the Free Software Foundation, either version 3 of the License, or
|
8 |
(at your option) any later version.
|
9 |
|
10 |
This program is distributed in the hope that it will be useful,
|
11 |
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
12 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
13 |
GNU Lesser General Public License for more details.
|
14 |
|
15 |
You should have received a copy of the GNU Lesser General Public License
|
16 |
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
17 |
*/
|
18 |
|
19 |
/**
|
20 |
* @file alld_BNO055_v1.c
|
21 |
* @brief Accelerometer function implementations.
|
22 |
*
|
23 |
* @addtogroup lld_accel
|
24 |
* @{
|
25 |
*/
|
26 |
|
27 |
#include <alld_BNO055.h> |
28 |
#include <bno055_reg.h> |
29 |
|
30 |
#if (defined(AMIROLLD_CFG_BNO055) && (AMIROLLD_CFG_BNO055 == 1)) || defined(__DOXYGEN__) |
31 |
|
32 |
#include <string.h> |
33 |
|
34 |
/******************************************************************************/
|
35 |
/* LOCAL DEFINITIONS */
|
36 |
/******************************************************************************/
|
37 |
|
38 |
/******************************************************************************/
|
39 |
/* EXPORTED VARIABLES */
|
40 |
/******************************************************************************/
|
41 |
|
42 |
/******************************************************************************/
|
43 |
/* LOCAL TYPES */
|
44 |
/******************************************************************************/
|
45 |
|
46 |
/******************************************************************************/
|
47 |
/* LOCAL VARIABLES */
|
48 |
/******************************************************************************/
|
49 |
|
50 |
static BNO055_Driver local_drv_cpy;
|
51 |
|
52 |
/******************************************************************************/
|
53 |
/* LOCAL FUNCTIONS */
|
54 |
/******************************************************************************/
|
55 |
|
56 |
/* BNO055 callbacks */
|
57 |
|
58 |
void BNO055_delay_msek(u32 msek) {
|
59 |
//TODO aos or chibios fct?
|
60 |
chThdSleepMicroseconds(msek); |
61 |
} |
62 |
|
63 |
/* \Brief: The API is used as I2C bus write
|
64 |
* \Return : Status of the I2C write
|
65 |
* \param dev_addr : The device address of the sensor
|
66 |
* \param reg_addr : Address of the first register,
|
67 |
* will data is going to be written
|
68 |
* \param reg_data : It is a value hold in the array,
|
69 |
* will be used for write the value into the register
|
70 |
* \param cnt : The no of byte of data to be write
|
71 |
*/
|
72 |
s8 BNO055_I2C_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt) { |
73 |
s8 BNO055_iERROR = BNO055_INIT_VALUE; |
74 |
|
75 |
uint8_t buffer[cnt+1];
|
76 |
uint8_t i = BNO055_INIT_VALUE; |
77 |
|
78 |
buffer[0] = reg_addr;
|
79 |
memcpy(buffer+1, reg_data, cnt);
|
80 |
|
81 |
apalExitStatus_t status = apalSPITransmit(local_drv_cpy.spid, |
82 |
buffer, cnt+1);
|
83 |
|
84 |
|
85 |
switch (status) {
|
86 |
case APAL_STATUS_OK:
|
87 |
BNO055_iERROR = BNO055_SUCCESS; |
88 |
break;
|
89 |
case APAL_STATUS_ERROR:
|
90 |
case APAL_STATUS_TIMEOUT:
|
91 |
case APAL_STATUS_INVALIDARGUMENTS:
|
92 |
case APAL_STATUS_UNAVAILABLE:
|
93 |
BNO055_iERROR = BNO055_ERROR; |
94 |
break;
|
95 |
default:
|
96 |
BNO055_iERROR = BNO055_ERROR; |
97 |
break;
|
98 |
} |
99 |
|
100 |
return (s8) BNO055_iERROR;
|
101 |
} |
102 |
|
103 |
/* \Brief: The API is used as I2C bus read
|
104 |
* \Return : Status of the I2C read
|
105 |
* \param dev_addr : The device address of the sensor
|
106 |
* \param reg_addr : Address of the first register,
|
107 |
* will data is going to be read
|
108 |
* \param reg_data : This data read from the sensor,
|
109 |
* which is hold in an array
|
110 |
* \param cnt : The no of byte of data to be read
|
111 |
*/
|
112 |
s8 BNO055_I2C_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt) { |
113 |
s8 BNO055_iERROR = BNO055_INIT_VALUE; |
114 |
uint8_t buffer[cnt+1];
|
115 |
uint8_t i = 0;
|
116 |
|
117 |
buffer[0] = reg_addr;
|
118 |
apalExitStatus_t status = apalSPIExchange(local_drv_cpy.spid, |
119 |
buffer, buffer, cnt+1);
|
120 |
|
121 |
switch (status) {
|
122 |
case APAL_STATUS_OK:
|
123 |
BNO055_iERROR = BNO055_SUCCESS; |
124 |
memcpy(reg_data, buffer+1, cnt);
|
125 |
break;
|
126 |
case APAL_STATUS_ERROR:
|
127 |
case APAL_STATUS_TIMEOUT:
|
128 |
case APAL_STATUS_INVALIDARGUMENTS:
|
129 |
case APAL_STATUS_UNAVAILABLE:
|
130 |
BNO055_iERROR = BNO055_ERROR; |
131 |
break;
|
132 |
default:
|
133 |
BNO055_iERROR = BNO055_ERROR; |
134 |
break;
|
135 |
} |
136 |
|
137 |
return (s8) BNO055_iERROR;
|
138 |
} |
139 |
|
140 |
/******************************************************************************/
|
141 |
/* EXPORTED FUNCTIONS */
|
142 |
/******************************************************************************/
|
143 |
|
144 |
|
145 |
apalExitStatus_t BNO055_lld_init(const BNO055_Driver* const bno){ |
146 |
//TODO refactor local drv copy -> make stateless
|
147 |
local_drv_cpy = *bno; |
148 |
|
149 |
static struct bno055_t bno055; |
150 |
|
151 |
bno055.bus_write = BNO055_I2C_bus_write; |
152 |
bno055.bus_read = BNO055_I2C_bus_read; |
153 |
bno055.delay_msec = BNO055_delay_msek; |
154 |
//TODO check I2C address
|
155 |
bno055.dev_addr = BNO055_I2C_ADDR2; |
156 |
|
157 |
BNO055_RETURN_FUNCTION_TYPE ret = bno055_init(&bno055); |
158 |
|
159 |
if (ret == BNO055_SUCCESS){
|
160 |
return BNO055_lld_set_mode(local_drv_cpy.mode, local_drv_cpy.sr);
|
161 |
} |
162 |
|
163 |
return APAL_STATUS_ERROR;
|
164 |
|
165 |
} |
166 |
|
167 |
apalExitStatus_t BNO055_lld_set_mode(const BNO055_Mode mode, const BNO055_SampleRate sr){ |
168 |
|
169 |
local_drv_cpy.mode = mode; |
170 |
local_drv_cpy.sr = sr; |
171 |
|
172 |
// switch BNO on/off
|
173 |
switch (local_drv_cpy.mode) {
|
174 |
case MOTN_MODE_NONE:
|
175 |
bno055_set_power_mode(BNO055_POWER_MODE_SUSPEND); |
176 |
return APAL_STATUS_OK;
|
177 |
case MOTN_MODE_GYR:
|
178 |
case MOTN_MODE_MAG:
|
179 |
case MOTN_MODE_ACC_GYR:
|
180 |
case MOTN_MODE_ACC_MAG:
|
181 |
case MOTN_MODE_GYRO_MAG:
|
182 |
case MOTN_MODE_ACC_GYR_MAG:
|
183 |
case MOTN_MODE_IMU:
|
184 |
case MOTN_MODE_COMPASS:
|
185 |
case MOTN_MODE_M4G:
|
186 |
case MOTN_MODE_NDOF:
|
187 |
bno055_set_power_mode(BNO055_POWER_MODE_NORMAL); |
188 |
break;
|
189 |
default:
|
190 |
bno055_set_power_mode(BNO055_POWER_MODE_SUSPEND); |
191 |
break;
|
192 |
} |
193 |
|
194 |
// and configure BNO as requested
|
195 |
|
196 |
switch (local_drv_cpy.mode) {
|
197 |
case MOTN_MODE_ACC:
|
198 |
bno055_set_power_mode(BNO055_POWER_MODE_NORMAL); |
199 |
switch (local_drv_cpy.sr) {
|
200 |
case MOTN_SR_LOW:
|
201 |
bno055_set_accel_bw(BNO055_ACCEL_BW_15_63HZ); |
202 |
break;
|
203 |
case MOTN_SR_NORMAL:
|
204 |
bno055_set_accel_bw(BNO055_ACCEL_BW_125HZ); |
205 |
break;
|
206 |
case MOTN_SR_HIGHSPEED:
|
207 |
bno055_set_accel_bw(BNO055_ACCEL_BW_1000HZ); |
208 |
break;
|
209 |
default:
|
210 |
break;
|
211 |
} |
212 |
bno055_set_gyro_range(BNO055_ACCEL_RANGE_8G); |
213 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_ACCONLY); |
214 |
bno055_set_accel_power_mode(BNO055_ACCEL_NORMAL); |
215 |
break;
|
216 |
|
217 |
case MOTN_MODE_GYR:
|
218 |
switch (local_drv_cpy.sr) {
|
219 |
case MOTN_SR_LOW:
|
220 |
bno055_set_gyro_bw(BNO055_GYRO_BW_47HZ); |
221 |
break;
|
222 |
case MOTN_SR_NORMAL:
|
223 |
bno055_set_gyro_bw(BNO055_GYRO_BW_116HZ); |
224 |
break;
|
225 |
case MOTN_SR_HIGHSPEED:
|
226 |
bno055_set_gyro_bw(BNO055_GYRO_BW_523HZ); |
227 |
break;
|
228 |
default:
|
229 |
break;
|
230 |
} |
231 |
bno055_set_gyro_range(BNO055_GYRO_RANGE_2000DPS); |
232 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_GYRONLY); |
233 |
bno055_set_gyro_power_mode(BNO055_GYRO_POWER_MODE_NORMAL); |
234 |
|
235 |
break;
|
236 |
case MOTN_MODE_MAG:
|
237 |
switch (local_drv_cpy.sr) {
|
238 |
case MOTN_SR_LOW:
|
239 |
bno055_set_mag_data_output_rate( |
240 |
BNO055_MAG_DATA_OUTRATE_8HZ); |
241 |
break;
|
242 |
case MOTN_SR_NORMAL:
|
243 |
bno055_set_mag_data_output_rate( |
244 |
BNO055_MAG_DATA_OUTRATE_15HZ); |
245 |
break;
|
246 |
case MOTN_SR_HIGHSPEED:
|
247 |
bno055_set_mag_data_output_rate( |
248 |
BNO055_MAG_DATA_OUTRATE_30HZ); |
249 |
break;
|
250 |
default:
|
251 |
break;
|
252 |
} |
253 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_MAGONLY); |
254 |
bno055_set_mag_power_mode(BNO055_MAG_POWER_MODE_NORMAL); |
255 |
|
256 |
break;
|
257 |
case MOTN_MODE_ACC_GYR:
|
258 |
switch (local_drv_cpy.sr) {
|
259 |
case MOTN_SR_LOW:
|
260 |
bno055_set_gyro_bw(BNO055_GYRO_BW_47HZ); |
261 |
bno055_set_accel_bw(BNO055_ACCEL_BW_62_5HZ); |
262 |
break;
|
263 |
case MOTN_SR_NORMAL:
|
264 |
bno055_set_gyro_bw(BNO055_GYRO_BW_116HZ); |
265 |
bno055_set_accel_bw(BNO055_ACCEL_BW_125HZ); |
266 |
break;
|
267 |
case MOTN_SR_HIGHSPEED:
|
268 |
bno055_set_gyro_bw(BNO055_GYRO_BW_523HZ); |
269 |
bno055_set_accel_bw(BNO055_ACCEL_BW_500HZ); |
270 |
break;
|
271 |
default:
|
272 |
break;
|
273 |
} |
274 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_ACCGYRO); |
275 |
bno055_set_gyro_range(BNO055_GYRO_RANGE_2000DPS); |
276 |
bno055_set_gyro_power_mode(BNO055_GYRO_POWER_MODE_NORMAL); |
277 |
bno055_set_accel_range(BNO055_ACCEL_RANGE_8G); |
278 |
bno055_set_accel_power_mode(BNO055_ACCEL_NORMAL); |
279 |
break;
|
280 |
case MOTN_MODE_ACC_MAG:
|
281 |
switch (local_drv_cpy.sr) {
|
282 |
case MOTN_SR_LOW:
|
283 |
bno055_set_mag_data_output_rate( |
284 |
BNO055_MAG_DATA_OUTRATE_8HZ); |
285 |
bno055_set_accel_bw(BNO055_ACCEL_BW_62_5HZ); |
286 |
break;
|
287 |
case MOTN_SR_NORMAL:
|
288 |
bno055_set_mag_data_output_rate( |
289 |
BNO055_MAG_DATA_OUTRATE_15HZ); |
290 |
bno055_set_accel_bw(BNO055_ACCEL_BW_125HZ); |
291 |
break;
|
292 |
case MOTN_SR_HIGHSPEED:
|
293 |
bno055_set_mag_data_output_rate( |
294 |
BNO055_MAG_DATA_OUTRATE_30HZ); |
295 |
bno055_set_accel_bw(BNO055_ACCEL_BW_500HZ); |
296 |
break;
|
297 |
default:
|
298 |
break;
|
299 |
} |
300 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_ACCMAG); |
301 |
bno055_set_mag_power_mode(BNO055_MAG_POWER_MODE_NORMAL); |
302 |
bno055_set_accel_range(BNO055_ACCEL_RANGE_8G); |
303 |
bno055_set_accel_power_mode(BNO055_ACCEL_NORMAL); |
304 |
break;
|
305 |
case MOTN_MODE_GYRO_MAG:
|
306 |
switch (local_drv_cpy.sr) {
|
307 |
case MOTN_SR_LOW:
|
308 |
bno055_set_gyro_bw(BNO055_GYRO_BW_47HZ); |
309 |
bno055_set_mag_data_output_rate( |
310 |
BNO055_MAG_DATA_OUTRATE_8HZ); |
311 |
break;
|
312 |
case MOTN_SR_NORMAL:
|
313 |
bno055_set_gyro_bw(BNO055_GYRO_BW_116HZ); |
314 |
bno055_set_mag_data_output_rate( |
315 |
BNO055_MAG_DATA_OUTRATE_15HZ); |
316 |
break;
|
317 |
case MOTN_SR_HIGHSPEED:
|
318 |
bno055_set_gyro_bw(BNO055_GYRO_BW_523HZ); |
319 |
bno055_set_mag_data_output_rate( |
320 |
BNO055_MAG_DATA_OUTRATE_30HZ); |
321 |
break;
|
322 |
default:
|
323 |
break;
|
324 |
} |
325 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_MAGGYRO); |
326 |
bno055_set_gyro_range(BNO055_GYRO_RANGE_2000DPS); |
327 |
bno055_set_gyro_power_mode(BNO055_GYRO_POWER_MODE_NORMAL); |
328 |
bno055_set_mag_power_mode(BNO055_MAG_POWER_MODE_NORMAL); |
329 |
break;
|
330 |
case MOTN_MODE_ACC_GYR_MAG:
|
331 |
switch (local_drv_cpy.sr) {
|
332 |
case MOTN_SR_LOW:
|
333 |
bno055_set_gyro_bw(BNO055_GYRO_BW_47HZ); |
334 |
bno055_set_mag_data_output_rate( |
335 |
BNO055_MAG_DATA_OUTRATE_8HZ); |
336 |
bno055_set_accel_bw(BNO055_ACCEL_BW_62_5HZ); |
337 |
break;
|
338 |
case MOTN_SR_NORMAL:
|
339 |
bno055_set_gyro_bw(BNO055_GYRO_BW_116HZ); |
340 |
bno055_set_mag_data_output_rate( |
341 |
BNO055_MAG_DATA_OUTRATE_15HZ); |
342 |
bno055_set_accel_bw(BNO055_ACCEL_BW_125HZ); |
343 |
break;
|
344 |
case MOTN_SR_HIGHSPEED:
|
345 |
bno055_set_gyro_bw(BNO055_GYRO_BW_523HZ); |
346 |
bno055_set_mag_data_output_rate( |
347 |
BNO055_MAG_DATA_OUTRATE_30HZ); |
348 |
bno055_set_accel_bw(BNO055_ACCEL_BW_500HZ); |
349 |
break;
|
350 |
default:
|
351 |
break;
|
352 |
} |
353 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_AMG); |
354 |
bno055_set_gyro_range(BNO055_GYRO_RANGE_2000DPS); |
355 |
bno055_set_gyro_power_mode(BNO055_GYRO_POWER_MODE_NORMAL); |
356 |
bno055_set_mag_power_mode(BNO055_MAG_POWER_MODE_NORMAL); |
357 |
bno055_set_accel_range(BNO055_ACCEL_RANGE_8G); |
358 |
bno055_set_accel_power_mode(BNO055_ACCEL_NORMAL); |
359 |
break;
|
360 |
case MOTN_MODE_IMU:
|
361 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_IMUPLUS); |
362 |
//sensor parameters are auto controlled in fusion mode
|
363 |
break;
|
364 |
case MOTN_MODE_COMPASS:
|
365 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_COMPASS); |
366 |
//sensor parameters are auto controlled in fusion mode
|
367 |
break;
|
368 |
case MOTN_MODE_M4G:
|
369 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_M4G); |
370 |
//sensor parameters are auto controlled in fusion mode
|
371 |
break;
|
372 |
case MOTN_MODE_NDOF:
|
373 |
bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF); |
374 |
//sensor parameters are auto controlled in fusion mode
|
375 |
break;
|
376 |
default:
|
377 |
return APAL_STATUS_INVALIDARGUMENTS;
|
378 |
} |
379 |
|
380 |
return APAL_STATUS_OK;
|
381 |
} |
382 |
|
383 |
apalExitStatus_t BNO055_lld_get_data(const BNO055_DataUnion* const dto){ |
384 |
|
385 |
switch (local_drv_cpy.mode) {
|
386 |
|
387 |
case MOTN_MODE_ACC:
|
388 |
bno055_read_accel_xyz((struct bno055_accel_t*) &(dto->raw.acc));
|
389 |
break;
|
390 |
case MOTN_MODE_GYR:
|
391 |
bno055_read_gyro_xyz((struct bno055_gyro_t*) &(dto->raw.gyr));
|
392 |
break;
|
393 |
case MOTN_MODE_MAG:
|
394 |
bno055_read_mag_xyz((struct bno055_mag_t*) &(dto->raw.mag));
|
395 |
break;
|
396 |
case MOTN_MODE_ACC_GYR:
|
397 |
bno055_read_accel_xyz((struct bno055_accel_t*) &(dto->raw.acc));
|
398 |
bno055_read_gyro_xyz((struct bno055_gyro_t*) &(dto->raw.gyr));
|
399 |
break;
|
400 |
case MOTN_MODE_ACC_MAG:
|
401 |
bno055_read_accel_xyz((struct bno055_accel_t*) &(dto->raw.acc));
|
402 |
bno055_read_mag_xyz((struct bno055_mag_t*) &(dto->raw.mag));
|
403 |
break;
|
404 |
case MOTN_MODE_GYRO_MAG:
|
405 |
bno055_read_gyro_xyz((struct bno055_gyro_t*) &(dto->raw.gyr));
|
406 |
bno055_read_mag_xyz((struct bno055_mag_t*) &(dto->raw.mag));
|
407 |
break;
|
408 |
case MOTN_MODE_ACC_GYR_MAG:
|
409 |
bno055_read_accel_xyz((struct bno055_accel_t*) &(dto->raw.acc));
|
410 |
bno055_read_gyro_xyz((struct bno055_gyro_t*) &(dto->raw.gyr));
|
411 |
bno055_read_mag_xyz((struct bno055_mag_t*) &(dto->raw.mag));
|
412 |
break;
|
413 |
case MOTN_MODE_COMPASS:
|
414 |
bno055_read_mag_xyz((struct bno055_mag_t*) &(dto->raw.mag));
|
415 |
break;
|
416 |
case MOTN_MODE_IMU:
|
417 |
case MOTN_MODE_M4G:
|
418 |
case MOTN_MODE_NDOF:
|
419 |
bno055_read_accel_xyz((struct bno055_accel_t*) &(dto->raw.acc));
|
420 |
bno055_read_gyro_xyz((struct bno055_gyro_t*) &(dto->raw.gyr));
|
421 |
bno055_read_mag_xyz((struct bno055_mag_t*) &(dto->raw.mag));
|
422 |
bno055_read_quaternion_wxyz( |
423 |
(struct bno055_quaternion_t*) &(dto->fsn.quaternion));
|
424 |
//bno055_read_euler_wxyz((struct bno055_euler_t*) &(dto->fusion.euler));
|
425 |
break;
|
426 |
default:
|
427 |
return APAL_STATUS_INVALIDARGUMENTS;
|
428 |
} |
429 |
|
430 |
} |
431 |
|
432 |
|
433 |
|
434 |
|
435 |
#endif /* defined(AMIROLLD_CFG_BNO055) && (AMIROLLD_CFG_BNO055 == 1) */ |
436 |
|
437 |
/** @} */
|