Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
invensense3.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
28
31#include "math/pprz_isa.h"
34#include "modules/imu/imu.h"
35#include "modules/core/abi.h"
36#include "mcu_periph/sys_time.h"
37#include "mcu_periph/gpio_arch.h"
38#include "std.h"
39
40/* Local functions */
41static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data, uint16_t samples);
42static void invensense3_parse_reg_data(struct invensense3_t *inv, uint8_t *data);
43static void invensense3_fix_config(struct invensense3_t *inv);
44static bool invensense3_register_write(struct invensense3_t *inv, uint16_t bank_reg, uint8_t value);
45static bool invensense3_register_read(struct invensense3_t *inv, uint16_t bank_reg, uint16_t size);
46static bool invensense3_select_bank(struct invensense3_t *inv, uint8_t bank);
47static bool invensense3_config(struct invensense3_t *inv);
48static bool invensense3_reset_fifo(struct invensense3_t *inv);
49static int samples_from_odr(int odr);
50
51/* Default gyro scalings */
52static const struct FloatRates invensense3_gyro_scale_f[8] = {
53 {4.36332, 4.36332, 4.36332}, // 2000DPS
54 {2.18166, 2.18166, 2.18166}, // 1000DPS
55 {1.09083, 1.09083, 1.09083}, // 500DPS
56 {0.545415, 0.545415, 0.545415}, // 250DPS: RATE_BFP_OF_REAL(radians(250)/(2**15))
57 {0.272707, 0.272707, 0.272707}, // 125DPS
58 {0.136353, 0.136353, 0.136353}, // 62.5DPS
59 {0.0681769, 0.0681769, 0.0681769}, // 31.25DPS
60 {0.0340884, 0.0340884, 0.0340884}, // 15.625DPS
61};
62
63/* Default accel scalings */
64static const struct FloatVect3 invensense3_accel_scale_f[5] = {
65 {9.81, 9.81, 9.81}, // 32G
66 {4.905, 4.905, 4.905}, // 16G
67 {2.4525, 2.4525, 2.4525}, // 8G
68 {1.22583, 1.22583, 1.22583}, // 4G
69 {0.61312, 0.61312, 0.61312}, // 2G: ACCEL_BFP_OF_REAL(2G*9.81/(2**15))
70};
71
72/* AAF settings (3dB Bandwidth [Hz], AAF_DELT, AAF_DELTSQR, AAF_BITSHIFT) */
73static const uint16_t invensense3_aaf[][4] = {
74 {42, 1, 1, 15},
75 {84, 2, 4, 13},
76 {126, 3, 9, 12},
77 {170, 4, 16, 11},
78 {213, 5, 25, 10},
79 {258, 6, 36, 10},
80 {303, 7, 49, 9},
81 {348, 8, 64, 9},
82 {394, 9, 81, 9},
83 {441, 10, 100, 8},
84 {488, 11, 122, 8},
85 {536, 12, 144, 8},
86 {585, 13, 170, 8},
87 {634, 14, 196, 7},
88 {684, 15, 224, 7},
89 {734, 16, 256, 7},
90 {785, 17, 288, 7},
91 {837, 18, 324, 7},
92 {890, 19, 360, 6},
93 {943, 20, 400, 6},
94 {997, 21, 440, 6},
95 {1051, 22, 488, 6},
96 {1107, 23, 528, 6},
97 {1163, 24, 576, 6},
98 {1220, 25, 624, 6},
99 {1277, 26, 680, 6},
100 {1336, 27, 736, 5},
101 {1395, 28, 784, 5},
102 {1454, 29, 848, 5},
103 {1515, 30, 896, 5},
104 {1577, 31, 960, 5},
105 {1639, 32, 1024, 5},
106 {1702, 33, 1088, 5},
107 {1766, 34, 1152, 5},
108 {1830, 35, 1232, 5},
109 {1896, 36, 1296, 5},
110 {1962, 37, 1376, 4},
111 {2029, 38, 1440, 4},
112 {2097, 39, 1536, 4},
113 {2166, 40, 1600, 4},
114 {2235, 41, 1696, 4},
115 {2306, 42, 1760, 4},
116 {2377, 43, 1856, 4},
117 {2449, 44, 1952, 4},
118 {2522, 45, 2016, 4},
119 {2596, 46, 2112, 4},
120 {2671, 47, 2208, 4},
121 {2746, 48, 2304, 4},
122 {2823, 49, 2400, 4},
123 {2900, 50, 2496, 4},
124 {2978, 51, 2592, 4},
125 {3057, 52, 2720, 4},
126 {3137, 53, 2816, 3},
127 {3217, 54, 2944, 3},
128 {3299, 55, 3008, 3},
129 {3381, 56, 3136, 3},
130 {3464, 57, 3264, 3},
131 {3548, 58, 3392, 3},
132 {3633, 59, 3456, 3},
133 {3718, 60, 3584, 3},
134 {3805, 61, 3712, 3},
135 {3892, 62, 3840, 3},
136 {3979, 63, 3968, 3}
137};
138
139static const uint16_t invensense3_aaf4x605[][4] = {
140 {10, 1, 1, 15},
141 {21, 2, 4, 13},
142 {32, 3, 9, 12},
143 {42, 4, 16, 11},
144 {53, 5, 25, 10},
145 {64, 6, 36, 10},
146 {76, 7, 49, 9},
147 {87, 8, 64, 9},
148 {99, 9, 81, 9},
149 {110, 10, 100, 8},
150 {122, 11, 122, 8},
151 {134, 12, 144, 8},
152 {146, 13, 170, 8},
153 {158, 14, 196, 7},
154 {171, 15, 224, 7},
155 {184, 16, 256, 7},
156 {196, 17, 288, 7},
157 {209, 18, 324, 7},
158 {222, 19, 360, 6},
159 {236, 20, 400, 6},
160 {249, 21, 440, 6},
161 {263, 22, 488, 6},
162 {277, 23, 528, 6},
163 {291, 24, 576, 6},
164 {305, 25, 624, 6},
165 {319, 26, 680, 6},
166 {334, 27, 736, 5},
167 {349, 28, 784, 5},
168 {364, 29, 848, 5},
169 {379, 30, 896, 5},
170 {394, 31, 960, 5},
171 {410, 32, 1024, 5},
172 {425, 33, 1088, 5},
173 {441, 34, 1152, 5},
174 {458, 35, 1232, 5},
175 {474, 36, 1296, 5},
176 {490, 37, 1376, 4},
177 {507, 38, 1440, 4},
178 {524, 39, 1536, 4},
179 {541, 40, 1600, 4},
180 {559, 41, 1696, 4},
181 {576, 42, 1760, 4},
182 {594, 43, 1856, 4},
183 {612, 44, 1952, 4},
184 {631, 45, 2016, 4},
185 {649, 46, 2112, 4},
186 {668, 47, 2208, 4},
187 {687, 48, 2304, 4},
188 {706, 49, 2400, 4},
189 {725, 50, 2496, 4},
190 {745, 51, 2592, 4},
191 {764, 52, 2720, 4},
192 {784, 53, 2816, 3},
193 {804, 54, 2944, 3},
194 {825, 55, 3008, 3},
195 {845, 56, 3136, 3},
196 {866, 57, 3264, 3},
197 {887, 58, 3392, 3},
198 {908, 59, 3456, 3},
199 {930, 60, 3584, 3},
200 {951, 61, 3712, 3},
201 {973, 62, 3840, 3},
202 {995, 63, 3968, 3}
203};
204
205static const uint8_t invensense3_fifo_sample_size[4] = {8,8,16,20};
206
213 /* General setup */
216 inv->register_bank = 0xFF;
217 inv->config_idx = 0;
218
219 /* SPI setup */
220 if(inv->bus == INVENSENSE3_SPI) {
221 inv->spi.trans.cpol = SPICpolIdleHigh;
222 inv->spi.trans.cpha = SPICphaEdge2;
223 inv->spi.trans.dss = SPIDss8bit;
224 inv->spi.trans.bitorder = SPIMSBFirst;
225 inv->spi.trans.cdiv = SPIDiv16;
226
227 inv->spi.trans.select = SPISelectUnselect;
228 inv->spi.trans.slave_idx = inv->spi.slave_idx;
229 inv->spi.trans.output_length = 0;
230 inv->spi.trans.input_length = 0;
231 inv->spi.trans.before_cb = NULL;
232 inv->spi.trans.after_cb = NULL;
233 inv->spi.trans.input_buf = inv->spi.rx_buf;
234 inv->spi.trans.output_buf = inv->spi.tx_buf;
235 inv->spi.trans.status = SPITransDone;
236
237 inv->rx_buffer = inv->spi.rx_buf;
238 inv->tx_buffer = inv->spi.tx_buf;
239 inv->rx_length = &inv->spi.trans.input_length;
240 }
241 /* I2C setup */
242 else {
243 inv->i2c.trans.slave_addr = inv->i2c.slave_addr;
244 inv->i2c.trans.status = I2CTransDone;
245
246 inv->rx_buffer = (uint8_t *)inv->i2c.trans.buf;
247 inv->tx_buffer = (uint8_t *)inv->i2c.trans.buf;
248 inv->rx_length = &inv->i2c.trans.len_r;
249 }
250
252
253 inv->sample_numbers = samples_from_odr(Min((int)inv->gyro_odr, (int)inv->accel_odr));
254 inv->fifo_cnt = 0;
255}
256
266 /* Idle */
267 if((inv->bus == INVENSENSE3_SPI && inv->spi.trans.status == SPITransDone) ||
268 (inv->bus == INVENSENSE3_I2C && inv->i2c.trans.status == I2CTransDone)) {
269
270 /* Depending on the status choose what to do */
271 switch(inv->status) {
272 case INVENSENSE3_IDLE:
273 /* Request WHO_AM_I */
275 break;
276
278 /* Start configuring */
279 if(invensense3_config(inv)) {
281 }
282 break;
283
285 /* Request a sensor reading */
286 switch (inv->parser) {
289 break;
291 {
292 // Not more than the parser can handle (INVENSENSE3_FIFO_BUFFER_LEN), the rest is read next time
294 int read_bytes = (inv->sample_numbers + inv->timer) * invensense3_fifo_sample_size[inv->sample_size];
295 read_bytes = Min(max_bytes, read_bytes);
296 // round to the packet boundaries
297 read_bytes -= read_bytes % invensense3_fifo_sample_size[inv->sample_size];
299 inv->timer = 0; // reset leftover bytes
300 break;
301 }
302 default: break;
303 }
304
305 default: break;
306 }
307 }
308}
309
319
320 /* Successful transfer */
321 if((inv->bus == INVENSENSE3_SPI && inv->spi.trans.status == SPITransSuccess) ||
322 (inv->bus == INVENSENSE3_I2C && inv->i2c.trans.status == I2CTransSuccess)) {
323
324 /* Update the register bank */
325 if(inv->tx_buffer[0] == INV3REG_BANK_SEL)
326 inv->register_bank = inv->tx_buffer[1];
327
328 /* Set the transaction as done and update register bank if needed */
329 if(inv->bus == INVENSENSE3_SPI)
330 inv->spi.trans.status = SPITransDone;
331 else
332 inv->i2c.trans.status = I2CTransDone;
333
334 /* Look at the results */
335 switch(inv->status) {
336 case INVENSENSE3_IDLE:
337 /* Check the response of the WHO_AM_I */
339 if(inv->rx_buffer[1] == INV3_WHOAMI_ICM40605) {
341 } else if(inv->rx_buffer[1] == INV3_WHOAMI_ICM40609) {
343 } else if(inv->rx_buffer[1] == INV3_WHOAMI_ICM42605) {
345 } else if(inv->rx_buffer[1] == INV3_WHOAMI_IIM42652) {
347 } else if(inv->rx_buffer[1] == INV3_WHOAMI_ICM42688) {
349 } else {
351 }
352
353 /* Fix the configuration and set the scaling */
354 if(inv->status == INVENSENSE3_CONFIG)
356 break;
357
359 /* Apply the next configuration register */
360 if(invensense3_config(inv)) {
362 }
363 break;
364
366 /* Select the desired parser */
367 switch (inv->parser) {
370 break;
371
373 /* Parse the results */
374 uint16_t n_samples = (uint16_t)inv->rx_buffer[1] | inv->rx_buffer[2] << 8;
376
377 inv->timer = n_samples; // samples left in FIFO
378
379 // Not enough data in FIFO
380 if(n_samples == 0) {
381 return;
382 }
383
384 // We read an incorrect length (try again)
385 if(fifo_bytes > 4096) {
387 return;
388 }
389
390 // Parse the data
391 if(*inv->rx_length > 3) {
392 uint16_t nb_packets_read = (*inv->rx_length - 3) / invensense3_fifo_sample_size[inv->sample_size];
393 uint16_t nb_packets = Min(n_samples, nb_packets_read);
394 invensense3_parse_fifo_data(inv, &inv->rx_buffer[3], nb_packets);
395 inv->timer -= nb_packets;
396 }
397
398 break;
399 }
400 default: break;
401 }
402 break;
403
404 default:
406 break;
407 }
408 }
409
410 /* Failed transaction */
411 if((inv->bus == INVENSENSE3_SPI && inv->spi.trans.status == SPITransFailed) ||
412 (inv->bus == INVENSENSE3_I2C && inv->i2c.trans.status == I2CTransFailed)) {
413
414 /* Set the transaction as done and update register bank if needed */
415 if(inv->bus == INVENSENSE3_SPI) {
416 inv->spi.trans.status = SPITransDone;
417 }
418 else {
419 inv->i2c.trans.status = I2CTransDone;
420 }
421
422 /* Retry or ignore */
423 switch(inv->status) {
425 /* If was not a bus switch decrease the index */
426 if(inv->config_idx > 0 && inv->tx_buffer[0] != INV3REG_BANK_SEL) {
427 inv->config_idx--;
428 }
429 /* Try again */
430 if(invensense3_config(inv)) {
432 }
433 break;
434 case INVENSENSE3_IDLE:
436 /* Ignore while idle/running */
437 default:
438 break;
439 }
440 }
441}
442
450static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data, uint16_t samples) {
451 static struct Int32Vect3 accel[INVENSENSE3_FIFO_BUFFER_LEN] = {0};
452 static struct Int32Rates gyro[INVENSENSE3_FIFO_BUFFER_LEN] = {0};
453
454 samples = Min(samples, INVENSENSE3_FIFO_BUFFER_LEN);
455
456 // The FIFO packets come at the faster ODR, the slower sensor is decimated with the ratio of the rates
457 // (keep a sample each time cnt * rate / faster_rate passes an integer, cnt is modulo faster_rate)
458 const uint32_t gyro_rate = inv->gyro_samplerate;
459 const uint32_t accel_rate = inv->accel_samplerate;
460 const uint32_t faster_rate = Max(gyro_rate, accel_rate);
461
462 // Go through the different samples
463 uint8_t i = 0;
464 uint8_t j = 0;
465 uint8_t valid = 0;
466 int32_t temp = 0;
467 for(uint8_t sample = 0; sample < samples; sample++) {
468
469 if ((data[0] & 0xFC) != 0x68) {
470 // no or bad data
471 } else {
472 const uint32_t cnt = inv->fifo_cnt;
473 inv->fifo_cnt = (cnt + 1) % faster_rate;
474
475 if(((cnt + 1) * gyro_rate) / faster_rate != (cnt * gyro_rate) / faster_rate) {
476 gyro[i].p = (int16_t)((uint16_t)data[7] | data[8] << 8 );
477 gyro[i].q = (int16_t)((uint16_t)data[9] | data[10] << 8 );
478 gyro[i].r = (int16_t)((uint16_t)data[11] | data[12] << 8 );
479 i++;
480 }
481
482 if(((cnt + 1) * accel_rate) / faster_rate != (cnt * accel_rate) / faster_rate) {
483 accel[j].x = (int16_t)((uint16_t)data[1] | data[2] << 8 );
484 accel[j].y = (int16_t)((uint16_t)data[3] | data[4] << 8 );
485 accel[j].z = (int16_t)((uint16_t)data[5] | data[6] << 8 );
486 j++;
487 }
488
489 // Temperature is updated at the faster ODR, once in every packet
490 temp += (int8_t)data[13];
491 valid++;
492 }
493
495 }
496
497 // ADC temp * temp sensitivity + temp zero
498 float temp_f = (valid > 0) ? ((float)temp / valid) / 2.07f + 25.f : NAN;
499
500 // Send the scaled values over ABI
501 uint32_t now_ts = get_sys_time_usec();
502 AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, i, inv->gyro_samplerate, temp_f);
503 AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, inv->accel_samplerate, temp_f);
504 AbiSendMsgTEMPERATURE_RAW(inv->abi_id, now_ts, temp_f, TEMPERATURE_TYPE_IMU);
505}
506
513static void invensense3_parse_reg_data(struct invensense3_t *inv, uint8_t *data) {
514 static struct Int32Vect3 accel[1] = {0};
515 static struct Int32Rates gyro[1] = {0};
516
517 int32_t temp = (int16_t)((uint16_t)data[0] | data[1] << 8);
518 // ADC temp * temp sensitivity + temp zero
519 float temp_f = (float)temp / 132.48 + 25.f;
520
521 accel[0].x = (int16_t)((uint16_t)data[2] | data[3] << 8 );
522 accel[0].y = (int16_t)((uint16_t)data[4] | data[5] << 8 );
523 accel[0].z = (int16_t)((uint16_t)data[6] | data[7] << 8 );
524
525 gyro[0].p = (int16_t)((uint16_t)data[8] | data[9] << 8 );
526 gyro[0].q = (int16_t)((uint16_t)data[10]| data[11] << 8 );
527 gyro[0].r = (int16_t)((uint16_t)data[12]| data[13] << 8 );
528
529 // Send the scaled values over ABI
530 uint32_t now_ts = get_sys_time_usec();
531 AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, 1, inv->gyro_samplerate, temp_f);
532 AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, 1, inv->accel_samplerate, temp_f);
533 AbiSendMsgTEMPERATURE_RAW(inv->abi_id, now_ts, temp_f, TEMPERATURE_TYPE_IMU);
534}
535
541static void invensense3_fix_config(struct invensense3_t *inv) {
542 /* Fix wrong configuration settings (prevent user error) */
545
549 else if(inv->gyro_odr < INVENSENSE3_GYRO_ODR_8KHZ)
551
556 }
557
562
563 /* Fix the AAF bandwidth setting */
564 const uint16_t (*aaf_table)[4];
565 uint16_t aaf_len;
567 aaf_len = sizeof(invensense3_aaf4x605) / sizeof(invensense3_aaf4x605[0]);
568 aaf_table = invensense3_aaf4x605;
569 }
570 else {
571 aaf_len = sizeof(invensense3_aaf) / sizeof(invensense3_aaf[0]);
572 aaf_table = invensense3_aaf;
573 }
574
575 uint16_t i = 0;
576 for(i = 0; i < aaf_len; i++) {
577 if(inv->gyro_aaf <= aaf_table[i][0]) {
578 inv->gyro_aaf = aaf_table[i][0];
579 RMAT_COPY(inv->gyro_aaf_regs, aaf_table[i]);
580 break;
581 }
582 }
583 if(i >= (aaf_len-1)) {
584 inv->gyro_aaf = aaf_table[aaf_len-1][0];
585 RMAT_COPY(inv->gyro_aaf_regs, aaf_table[aaf_len-1]);
586 }
587
588 for(i = 0; i < aaf_len; i++) {
589 if(inv->accel_aaf <= aaf_table[i][0]) {
590 inv->accel_aaf = aaf_table[i][0];
591 RMAT_COPY(inv->accel_aaf_regs, aaf_table[i]);
592 break;
593 }
594 }
595 if(i >= (aaf_len-1)) {
596 inv->accel_aaf = aaf_table[aaf_len-1][0];
597 RMAT_COPY(inv->accel_aaf_regs, aaf_table[aaf_len-1]);
598 }
599
600 /* Calculate the samplerates in Hz */
601 switch(inv->gyro_odr) {
603 inv->gyro_samplerate = 32000;
604 break;
606 inv->gyro_samplerate = 16000;
607 break;
609 inv->gyro_samplerate = 8000;
610 break;
612 inv->gyro_samplerate = 4000;
613 break;
615 inv->gyro_samplerate = 2000;
616 break;
618 inv->gyro_samplerate = 1000;
619 break;
621 inv->gyro_samplerate = 200;
622 break;
624 inv->gyro_samplerate = 100;
625 break;
627 inv->gyro_samplerate = 50;
628 break;
630 inv->gyro_samplerate = 25;
631 break;
633 inv->gyro_samplerate = 12.5;
634 break;
636 inv->gyro_samplerate = 6.25;
637 break;
639 inv->gyro_samplerate = 3.125;
640 break;
642 inv->gyro_samplerate = 1.5625;
643 break;
645 inv->gyro_samplerate = 500;
646 break;
647 }
648 switch(inv->accel_odr) {
650 inv->accel_samplerate = 32000;
651 break;
653 inv->accel_samplerate = 16000;
654 break;
656 inv->accel_samplerate = 8000;
657 break;
659 inv->accel_samplerate = 4000;
660 break;
662 inv->accel_samplerate = 2000;
663 break;
665 inv->accel_samplerate = 1000;
666 break;
668 inv->accel_samplerate = 200;
669 break;
671 inv->accel_samplerate = 100;
672 break;
674 inv->accel_samplerate = 50;
675 break;
677 inv->accel_samplerate = 25;
678 break;
680 inv->accel_samplerate = 12.5;
681 break;
683 inv->accel_samplerate = 6.25;
684 break;
686 inv->accel_samplerate = 3.125;
687 break;
689 inv->accel_samplerate = 1.5625;
690 break;
692 inv->accel_samplerate = 500;
693 break;
694 }
695
696 /* Set the default values */
699}
700
710static bool invensense3_register_write(struct invensense3_t *inv, uint16_t bank_reg, uint8_t value) {
711 /* Split the register and bank */
712 uint8_t bank = bank_reg >> 8;
713 uint8_t reg = bank_reg & 0xFF;
714
715 /* Switch the register bank if needed */
716 if(invensense3_select_bank(inv, bank)) {
717 return false;
718 }
719
720 inv->tx_buffer[0] = reg;
721 inv->tx_buffer[1] = value;
722
723 /* SPI transaction */
724 if(inv->bus == INVENSENSE3_SPI) {
725 inv->spi.trans.output_length = 2;
726 inv->spi.trans.input_length = 0;
727 spi_submit(inv->spi.p, &(inv->spi.trans));
728 }
729 /* I2C transaction */
730 else {
731 i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2);
732 }
733
734 return true;
735}
736
746static bool invensense3_register_read(struct invensense3_t *inv, uint16_t bank_reg, uint16_t size) {
747 /* Split the register and bank */
748 uint8_t bank = bank_reg >> 8;
749 uint8_t reg = bank_reg & 0xFF;
750
751 /* Switch the register bank if needed */
752 if(invensense3_select_bank(inv, bank)) {
753 return false;
754 }
755
756 inv->tx_buffer[0] = reg | INV3_READ_FLAG;
757 /* SPI transaction */
758 if(inv->bus == INVENSENSE3_SPI) {
759 inv->spi.trans.output_length = 2;
760 inv->spi.trans.input_length = 1 + size;
761 inv->tx_buffer[1] = 0;
762 spi_submit(inv->spi.p, &(inv->spi.trans));
763 }
764 /* I2C transaction */
765 else {
766 i2c_transceive(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 1, (1 + size));
767 }
768
769 return true;
770}
771
780static bool invensense3_select_bank(struct invensense3_t *inv, uint8_t bank) {
781 /* If we already selected the correct bank continue */
782 if(inv->register_bank == bank)
783 return false;
784
785 inv->tx_buffer[0] = INV3REG_BANK_SEL;
786 inv->tx_buffer[1] = bank;
787 /* SPI transaction */
788 if(inv->bus == INVENSENSE3_SPI) {
789 inv->spi.trans.output_length = 2;
790 inv->spi.trans.input_length = 0;
791 spi_submit(inv->spi.p, &(inv->spi.trans));
792 }
793 /* I2C transaction */
794 else {
795 i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2);
796 }
797
798 return true;
799}
800
807static bool invensense3_reset_fifo(struct invensense3_t *inv) {
809 return true;
810 return false;
811}
812
820static bool invensense3_config(struct invensense3_t *inv) {
821 switch(inv->config_idx) {
822 case 0:
823 /* Reset the device */
825 inv->config_idx++;
826 inv->timer = get_sys_time_usec();
827 break;
828
829 case 1:
830 /* Because reset takes time wait ~5ms */
831 if((get_sys_time_usec() - inv->timer) < 5e3)
832 break;
833
834 /* Start the accel en gyro in low noise mode */
836 inv->config_idx++;
837 inv->timer = get_sys_time_usec();
838 break;
839
840 case 2:
841 /* Because starting takes time wait ~1ms */
842 if((get_sys_time_usec() - inv->timer) < 1e3)
843 break;
844
845 /* Configure the gyro ODR/FS */
847 inv->config_idx++;
848 break;
849
850 case 3: {
851 /* Configure the accel ODR/FS */
852 uint8_t accel_config = (inv->accel_odr << ACCEL_ODR_SHIFT);
853 if(inv->device == INVENSENSE3_ICM40609)
854 accel_config |= inv->accel_range << ACCEL_FS_SEL_SHIFT;
855 else
856 accel_config |= (inv->accel_range - 1) << ACCEL_FS_SEL_SHIFT;
857
859 inv->config_idx++;
860 break;
861 }
862
863 case 4:
864 /* Configure gyro AAF enable */
866 inv->config_idx++;
867 break;
868 case 5:
869 /* Configure gyro AAF DELT */
871 inv->config_idx++;
872 break;
873 case 6:
874 /* Configure gyro AAF DELTSQR lower */
876 inv->config_idx++;
877 break;
878 case 7:
879 /* Configure gyro AAF DELTSQR upper and bitshift */
881 inv->config_idx++;
882 break;
883 case 8:
884 /* Configure accel AAF enable and DELT */
886 inv->config_idx++;
887 break;
888 case 9:
889 /* Configure accel AAF DELTSQR lower */
891 inv->config_idx++;
892 break;
893 case 10:
894 /* Configure accel AAF DELTSQR upper and bitshift */
896 inv->config_idx++;
897 break;
898
899 case 11:
900 /* FIFO count in records (little-endian data) */
902 inv->config_idx++;
903 break;
904 case 12:
905 /* Disable AFSR, which changes noise sensitivity with angular rate, causing stuck gyro's for 2ms resulting in DC gyro bias */
907 inv->config_idx++;
908 break;
909 case 13:
910 /* FIFO Stop-on-Full mode (enable the FIFO) */
912 inv->config_idx++;
913 break;
914 case 14:
915 /* FIFO content: enable accel, gyro, temperature */
918 inv->config_idx++;
919 break;
920 case 15:
921 /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */
923 inv->config_idx++;
924 break;
925 case 16:
926 /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */
928 inv->config_idx++;
929 break;
930 case 17:
931 /* Enable interrupt pin/status */
932 switch(inv->parser) {
935 inv->config_idx++;
936 break;
939 inv->config_idx++;
940 break;
941 default:
942 break;
943 }
944 break;
945 case 18:
946 /* Interrupt pins ASYNC_RESET configuration */
947 // Datasheet: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
949 inv->config_idx++;
950 break;
951 case 19:
952 /* FIFO flush */
954 inv->config_idx++;
955 break;
956
957 default:
958 inv->timer = 0;
959 return true;
960 }
961 return false;
962}
963
964static int samples_from_odr(int odr) {
965 float freq = 0;
967 freq = 32000 / pow(2, odr-INVENSENSE3_GYRO_ODR_32KHZ);
968 }
969 else if(odr < INVENSENSE3_GYRO_ODR_500HZ) {
970 freq = 200 / pow(2, odr-INVENSENSE3_GYRO_ODR_200HZ);
971 }
972 else if(odr == INVENSENSE3_GYRO_ODR_500HZ) {
973 freq = 500;
974 } else {
975 // error
976 }
977 return ceilf(freq/PERIODIC_FREQUENCY);
978}
Main include for ABI (AirBorneInterface).
#define TEMPERATURE_TYPE_IMU
IMU die.
Definition abi_common.h:92
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
int n_samples
Definition detect_gate.c:85
#define Min(x, y)
Definition esc_dshot.c:110
bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition i2c.c:202
bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction.
Definition i2c.c:222
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition i2c.h:58
@ I2CTransFailed
transaction failed
Definition i2c.h:59
@ I2CTransDone
transaction set to done by user level
Definition i2c.h:60
angular rates
#define RMAT_COPY(_o, _i)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
angular rates
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition spi_arch.c:548
@ SPICphaEdge2
CPHA = 1.
Definition spi.h:71
@ SPITransFailed
Definition spi.h:96
@ SPITransSuccess
Definition spi.h:95
@ SPITransDone
Definition spi.h:97
@ SPICpolIdleHigh
CPOL = 1.
Definition spi.h:80
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition spi.h:59
@ SPIMSBFirst
Definition spi.h:108
@ SPIDiv16
Definition spi.h:119
@ SPIDss8bit
Definition spi.h:86
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition imu.c:842
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition imu.c:871
Inertial Measurement Unit interface.
#define GYRO_FS_SEL_SHIFT
#define ACCEL_FS_SEL_SHIFT
static const uint16_t invensense3_aaf4x605[][4]
void invensense3_periodic(struct invensense3_t *inv)
Should be called periodically to request sensor readings.
static const struct FloatRates invensense3_gyro_scale_f[8]
Definition invensense3.c:52
static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data, uint16_t samples)
Parse the FIFO buffer data.
static int samples_from_odr(int odr)
static const uint8_t invensense3_fifo_sample_size[4]
static const uint16_t invensense3_aaf[][4]
Definition invensense3.c:73
static void invensense3_parse_reg_data(struct invensense3_t *inv, uint8_t *data)
Parse data from registers.
static bool invensense3_reset_fifo(struct invensense3_t *inv)
Reset FIFO (can be useful in some situations).
static bool invensense3_select_bank(struct invensense3_t *inv, uint8_t bank)
Select the correct register bank.
void invensense3_event(struct invensense3_t *inv)
Should be called in the event thread.
static bool invensense3_config(struct invensense3_t *inv)
Configure the Invensense 3 device register by register.
static void invensense3_fix_config(struct invensense3_t *inv)
This fixes the configuration errors and sets the correct scalings.
static const struct FloatVect3 invensense3_accel_scale_f[5]
Definition invensense3.c:64
static bool invensense3_register_read(struct invensense3_t *inv, uint16_t bank_reg, uint16_t size)
Read a register based on a combined bank/regsiter value.
static bool invensense3_register_write(struct invensense3_t *inv, uint16_t bank_reg, uint8_t value)
Write a register based on a combined bank/regsiter value.
void invensense3_init(struct invensense3_t *inv)
Initialize the invensense v3 sensor instance.
Driver for the Invensense V3 IMUs ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688.
enum invensense3_gyro_range_t gyro_range
Gyro range configuration.
@ INVENSENSE3_ACCEL_RANGE_16G
@ INVENSENSE3_ACCEL_RANGE_4G
uint8_t register_bank
The current register bank communicating with.
enum invensense3_status_t status
Status of the invensense v3 device.
uint16_t * rx_length
@ INVENSENSE3_GYRO_ODR_4KHZ
Definition invensense3.h:99
@ INVENSENSE3_GYRO_ODR_25HZ
@ INVENSENSE3_GYRO_ODR_500HZ
@ INVENSENSE3_GYRO_ODR_8KHZ
Definition invensense3.h:98
@ INVENSENSE3_GYRO_ODR_1_5625HZ
@ INVENSENSE3_GYRO_ODR_100HZ
@ INVENSENSE3_GYRO_ODR_2KHZ
@ INVENSENSE3_GYRO_ODR_50HZ
@ INVENSENSE3_GYRO_ODR_1KHZ
@ INVENSENSE3_GYRO_ODR_16KHZ
Not possible for ICM40605 and ICM42605.
Definition invensense3.h:97
@ INVENSENSE3_GYRO_ODR_32KHZ
Not possible for ICM40605 and ICM42605.
Definition invensense3.h:96
@ INVENSENSE3_GYRO_ODR_3_125HZ
@ INVENSENSE3_GYRO_ODR_6_25HZ
@ INVENSENSE3_GYRO_ODR_200HZ
@ INVENSENSE3_GYRO_ODR_12_5HZ
@ INVENSENSE3_UNKOWN
Definition invensense3.h:86
@ INVENSENSE3_ICM42688
Definition invensense3.h:91
@ INVENSENSE3_ICM40605
Definition invensense3.h:87
@ INVENSENSE3_IIM42652
Definition invensense3.h:90
@ INVENSENSE3_ICM40609
Definition invensense3.h:88
@ INVENSENSE3_ICM42605
Definition invensense3.h:89
enum invensense3_parser_t parser
Parser of the device.
uint16_t accel_aaf
Accelerometer Anti-alias filter 3dB Bandwidth configuration [Hz].
@ INVENSENSE3_PARSER_FIFO
Definition invensense3.h:74
@ INVENSENSE3_PARSER_REGISTERS
Definition invensense3.h:73
uint32_t fifo_cnt
FIFO packet counter to decimate the slower sensor (modulo the faster rate).
uint32_t timer
Used to time operations during configuration (samples left during measuring).
enum invensense3_accel_odr_t accel_odr
Accelerometer Output Data Rate configuration.
@ INVENSENSE3_ACCEL_ODR_4KHZ
@ INVENSENSE3_ACCEL_ODR_16KHZ
Not possible for ICM40605 and ICM42605.
@ INVENSENSE3_ACCEL_ODR_50HZ
@ INVENSENSE3_ACCEL_ODR_32KHZ
Not possible for ICM40605 and ICM42605.
@ INVENSENSE3_ACCEL_ODR_2KHZ
@ INVENSENSE3_ACCEL_ODR_25HZ
@ INVENSENSE3_ACCEL_ODR_1_5625HZ
@ INVENSENSE3_ACCEL_ODR_8KHZ
@ INVENSENSE3_ACCEL_ODR_6_25HZ
@ INVENSENSE3_ACCEL_ODR_200HZ
@ INVENSENSE3_ACCEL_ODR_100HZ
@ INVENSENSE3_ACCEL_ODR_1KHZ
@ INVENSENSE3_ACCEL_ODR_500HZ
@ INVENSENSE3_ACCEL_ODR_12_5HZ
@ INVENSENSE3_ACCEL_ODR_3_125HZ
enum invensense3_bus_t bus
The communication bus used to connect the device SPI/I2C.
uint8_t config_idx
The current configuration index.
uint8_t * rx_buffer
int sample_numbers
expected FIFO packet number, assuming reading at PERIODIC_FREQUENCY
enum invensense3_device_t device
The device type detected.
float gyro_samplerate
Sample rate in Hz from the gyro_odr.
@ INVENSENSE3_I2C
Definition invensense3.h:61
@ INVENSENSE3_SPI
Definition invensense3.h:60
uint8_t * tx_buffer
float accel_samplerate
Sample rate in Hz from the accel_odr.
enum invensense3_accel_range_t accel_range
Accelerometer range configuration.
enum invensense3_fifo_packet_t sample_size
FIFO packet size.
@ INVENSENSE3_SAMPLE_SIZE_PK3
Definition invensense3.h:80
uint16_t gyro_aaf_regs[4]
Gyro Anti-alias filter register values.
uint8_t abi_id
The ABI id used to broadcast the device measurements.
uint16_t accel_aaf_regs[4]
Accelerometer Anti-alias filter register values.
uint16_t gyro_aaf
Gyro Anti-alias filter 3dB Bandwidth configuration [Hz].
enum invensense3_gyro_odr_t gyro_odr
Gyro Output Data Rate configuration.
#define INVENSENSE3_FIFO_BUFFER_LEN
Definition invensense3.h:38
@ INVENSENSE3_CONFIG
Definition invensense3.h:67
@ INVENSENSE3_IDLE
Definition invensense3.h:66
@ INVENSENSE3_RUNNING
Definition invensense3.h:68
#define GYRO_ODR_SHIFT
#define ACCEL_ODR_SHIFT
Register and address definitions for the Invensense V3 from ardupilot.
#define INV3REG_INTF_CONFIG1
#define FIFO_CONFIG_MODE_SHIFT
#define INV3REG_FIFO_CONFIG
#define FIFO_CONFIG_MODE_STOP_ON_FULL
#define ACCEL_MODE_SHIFT
#define INV3REG_GYRO_CONFIG_STATIC2
#define INV3REG_ACCEL_CONFIG_STATIC2
#define BIT_FIFO_CONFIG1_ACCEL_EN
#define INV3_WHOAMI_ICM42605
#define INV3REG_WHO_AM_I
#define INV3REG_BANK_SEL
#define BIT_FIFO_CONFIG1_GYRO_EN
#define INV3REG_FIFO_CONFIG3
#define ACCEL_AAF_BITSHIFT_SHIFT
#define INV3_READ_FLAG
#define INV3REG_PWR_MGMT0
#define ACCEL_MODE_LN
#define INV3REG_INT_CONFIG1
#define BIT_SIGNAL_PATH_RESET_FIFO_FLUSH
#define INV3REG_FIFO_CONFIG1
#define BIT_FIFO_FULL_INT_EN
#define FIFO_COUNT_REC
#define ACCEL_AAF_DELT_SHIFT
#define BIT_FIFO_CONFIG1_TEMP_EN
#define INV3REG_INTF_CONFIG0
#define INV3REG_INT_SOURCE0
#define INV3_WHOAMI_ICM42688
#define GYRO_AAF_BITSHIFT_SHIFT
#define INV3REG_TEMP_DATA1
#define INV3_WHOAMI_ICM40605
#define BIT_INT_ASYNC_RESET
#define GYRO_MODE_SHIFT
#define INV3REG_DEVICE_CONFIG
#define INV3REG_GYRO_CONFIG_STATIC3
#define BIT_FIFO_THS_INT_EN
#define INV3REG_ACCEL_CONFIG_STATIC4
#define INV3REG_GYRO_CONFIG_STATIC5
#define INV3REG_GYRO_CONFIG_STATIC4
#define BIT_UI_DRDY_INT_EN
#define INV3_WHOAMI_IIM42652
#define INV3REG_ACCEL_CONFIG_STATIC3
#define GYRO_MODE_LN
#define INV3REG_ACCEL_CONFIG0
#define BIT_DEVICE_CONFIG_SOFT_RESET_CONFIG
#define INV3REG_FIFO_CONFIG2
#define INV3REG_FIFO_COUNTH
#define INV3_WHOAMI_ICM40609
#define INV3REG_SIGNAL_PATH_RESET
#define INV3REG_GYRO_CONFIG0
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Paparazzi atmospheric pressure conversion utilities.
Architecture independent timing functions.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.