Statistics
| Branch: | Tag: | Revision:

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
/** @} */