Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
invensense3_456.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
41#include <string.h>
43
44/* Local functions */
45static void invensense3_456_parse_fifo_data(struct invensense3_456_t *inv, uint8_t *data, uint16_t samples);
46static void invensense3_456_set_scalings(struct invensense3_456_t *inv);
47static bool invensense3_456_register_write(struct invensense3_456_t *inv, uint8_t reg, uint8_t value);
48static bool invensense3_456_register_read(struct invensense3_456_t *inv, uint8_t reg, uint16_t size);
49static bool invensense3_456_config(struct invensense3_456_t *inv);
50static int samples_from_odr(int odr);
51
52
59 /* General setup */
62 inv->config_idx = 0;
63
64 /* SPI setup */
67 inv->spi.trans.dss = SPIDss8bit;
69 inv->spi.trans.cdiv = SPIDiv16;
70
72 inv->spi.trans.slave_idx = inv->spi.slave_idx;
73 inv->spi.trans.output_length = 0;
74 inv->spi.trans.input_length = 0;
75 inv->spi.trans.before_cb = NULL;
76 inv->spi.trans.after_cb = NULL;
77 inv->spi.trans.input_buf = inv->spi.rx_buf;
78 inv->spi.trans.output_buf = inv->spi.tx_buf;
80
81 inv->rx_buffer = inv->spi.rx_buf;
82 inv->tx_buffer = inv->spi.tx_buf;
83 inv->rx_length = &inv->spi.trans.input_length;
84
85 inv->sample_numbers = samples_from_odr((int)inv->imu_odr);
86}
87
97 /* Idle */
98 if(inv->spi.trans.status == SPITransDone) {
99 /* Depending on the status choose what to do */
100 switch(inv->status) {
102 /* Request WHO_AM_I */
104 break;
105
107 /* Start configuring */
108 if(invensense3_456_config(inv)) {
110 }
111 break;
112
114 {
115 static const uint16_t max_bytes = sizeof(inv->spi.rx_buf) - 3;
116 int read_bytes = (inv->sample_numbers + inv->timer) * sizeof(struct FIFODataHighRes);
117 read_bytes = Min(max_bytes, read_bytes);
118 // round to the packet boundaries
119 read_bytes -= read_bytes % sizeof(struct FIFODataHighRes);
121 inv->timer = 0; // reset leftover bytes
122 break;
123 }
124
125 default: break;
126 }
127 }
128}
129
139
140 /* Successful transfer */
141 if (inv->spi.trans.status == SPITransSuccess) {
142 /* Set the transaction as done */
144
145 /* Look at the results */
146 switch(inv->status) {
148 /* Check the response of the WHO_AM_I */
150 if(inv->rx_buffer[1] == INV3_456_WHOAMI_ICM45686) {
152 } else {
154 }
155
156 /* Set the scaling */
159 break;
160
162 /* Apply the next configuration register */
163 if(invensense3_456_config(inv)) {
165 }
166 break;
167
169 {
170 /* Parse the results */
171 uint16_t n_samples = (uint16_t)inv->rx_buffer[1] | inv->rx_buffer[2] << 8;
172 uint16_t fifo_bytes = n_samples * sizeof(struct FIFODataHighRes);
173
174 inv->timer = n_samples; // samples left in FIFO
175
176 // Not enough data in FIFO
177 if(n_samples == 0) {
178 return;
179 }
180
181 // We read an incorrect length (try again)
182 if(fifo_bytes > 4096) {
184 return;
185 }
186
187 // Parse the data
188 if(*inv->rx_length > 3) {
189 uint16_t nb_packets_read = (*inv->rx_length - 3) / sizeof(struct FIFODataHighRes);
190 invensense3_456_parse_fifo_data(inv, &inv->rx_buffer[3], Min(n_samples, nb_packets_read));
191 inv->timer -= nb_packets_read;
192 }
193 }
194 break;
195 default:
197 break;
198 }
199 }
200
201 /* Failed transaction */
202 if (inv->spi.trans.status == SPITransFailed) {
203
204 /* Set the transaction as done */
206
207 /* Retry or ignore */
208 switch(inv->status) {
210 /* Try again */
211 if(invensense3_456_config(inv)) {
213 }
214 break;
217 /* Ignore while idle/running */
218 default:
219 break;
220 }
221 }
222}
223
224// Helper for reassembling the 20-bit data retrieved from the FIFO buffer
225// REF: https://github.com/ArduPilot/ardupilot/blob/85a8a55611a95d4c59052f79f56744a93a9c5a63/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp#L489
226static inline int32_t reassemble_uint20(uint8_t msb, uint8_t bits, uint8_t lsb) {
227 uint32_t value20bit = ((uint32_t)msb << 12U) | ((uint32_t)bits << 4U) | (uint32_t)lsb;
228 int32_t value32bit;
229 // Check the sign bit (MSB)
230 if (value20bit & 0x80000) { // MSB is set (negative value)
231 // Extend the sign bit to the upper 12 bits of the 32-bit integer
232 value32bit = (int32_t)(value20bit | 0xFFF00000);
233 } else { // MSB is not set (positive value)
234 // Zero-fill the upper 12 bits of the 32-bit integer
235 value32bit = value20bit;
236 }
237
238 return value32bit;
239}
240
248static void invensense3_456_parse_fifo_data(struct invensense3_456_t *inv, uint8_t *data, uint16_t samples) {
249 static struct Int32Vect3 accel[INVENSENSE3_456_FIFO_BUFFER_LEN] = {0};
250 static struct Int32Rates gyro[INVENSENSE3_456_FIFO_BUFFER_LEN] = {0};
251
252 samples = Min(samples, INVENSENSE3_456_FIFO_BUFFER_LEN);
253
254 // Go through the different samples
255 int32_t temp = 0;
256 int sample_idx = 0;
257 for(uint8_t sample = 0; sample < samples; sample++) {
258 struct FIFODataHighRes *d = (struct FIFODataHighRes *)data;
259
260 if ((d->header & 0xFC) != 0x78) { // ACCEL_EN | GYRO_EN | HIRES_EN | TMST_FIELD_EN
261 // no or bad data, skip the packet
262 } else {
263 // Valid FIFO packet - re-assemble the data
264 gyro[sample_idx].p = reassemble_uint20(d->gyro[1], d->gyro[0], d->gx);
265 gyro[sample_idx].q = reassemble_uint20(d->gyro[3], d->gyro[2], d->gy);
266 gyro[sample_idx].r = reassemble_uint20(d->gyro[5], d->gyro[4], d->gz);
267
268 accel[sample_idx].x = reassemble_uint20(d->accel[1], d->accel[0], d->ax);
269 accel[sample_idx].y = reassemble_uint20(d->accel[3], d->accel[2], d->ay);
270 accel[sample_idx].z = reassemble_uint20(d->accel[5], d->accel[4], d->az);
271
272 // Compute the average temperature over the sampling period by summing and then dividing over the number of valid samples
273 temp += d->temperature;
274
275 sample_idx++;
276 }
277
278 data += sizeof(struct FIFODataHighRes);
279 }
280
281 // ADC temp * temp sensitivity + temp zero
282 // From the datasheet for high-res 16-bit: "Temperature in Degrees Centigrade = (TEMP_DATA / 128) + 25"
283 float temp_f = (sample_idx > 0) ? ((float)temp / sample_idx) / 128.f + 25.f : NAN;
284
285 // Send the obtained samples over ABI
286 uint32_t now_ts = get_sys_time_usec();
287 AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, sample_idx, inv->imu_samplerate, temp_f);
288 AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, sample_idx, inv->imu_samplerate, temp_f);
289 AbiSendMsgTEMPERATURE_RAW(inv->abi_id, now_ts, temp_f, TEMPERATURE_TYPE_IMU);
290}
291
298 switch (inv->imu_odr) {
300 inv->imu_samplerate = 6400;
301 break;
303 inv->imu_samplerate = 3200;
304 break;
306 inv->imu_samplerate = 1600;
307 break;
309 inv->imu_samplerate = 800;
310 break;
312 inv->imu_samplerate = 400;
313 break;
315 inv->imu_samplerate = 200;
316 break;
318 inv->imu_samplerate = 100;
319 break;
321 inv->imu_samplerate = 50;
322 break;
324 inv->imu_samplerate = 25;
325 break;
326 default:
327 /* Unsupported ODR! */
328 inv->imu_samplerate = 0;
329 break;
330
331 }
332
333 /* In the 20-bit high-resolution mode, the scaling is always set to 4000dps/32G no matter what we set up, hence we don't expose any config for this */
334 /* 20 bits of data, and one for sign -> 32G*9.81(m/s^2)/g / 2^(20-1) = m/s^2 PER LSB, with the appropriate scaling for our internal representatations (e.g. 10ths of m/s for accel as of writing) */
335 static const struct FloatVect3 accel_scale_hr = {ACCEL_BFP_OF_REAL(32*9.81/(1 << 19)), ACCEL_BFP_OF_REAL(32*9.81/(1 << 19)), ACCEL_BFP_OF_REAL(32*9.81/(1 << 19))}; // 32G high-res (20 bit mode)
336 static const struct FloatRates gyro_scale_hr = {RATE_BFP_OF_REAL(RadOfDeg(4000)/(1 << 19)), RATE_BFP_OF_REAL(RadOfDeg(4000)/(1 << 19)), RATE_BFP_OF_REAL(RadOfDeg(4000)/(1 << 19))}; // 4000dps high-res (20 bit mode)
337 imu_set_defaults_accel(inv->abi_id, NULL, NULL, &accel_scale_hr);
338 imu_set_defaults_gyro(inv->abi_id, NULL, NULL, &gyro_scale_hr);
339}
340
350
351 inv->tx_buffer[0] = reg;
352 inv->tx_buffer[1] = value;
353
354 /* SPI transaction */
355 inv->spi.trans.output_length = 2;
356 inv->spi.trans.input_length = 0;
357 spi_submit(inv->spi.p, &(inv->spi.trans));
358
359 return true;
360}
361
372 // combine addr
373 uint16_t addr = bank_addr + reg;
374 inv->tx_buffer[0] = INV3REG_456_IREG_ADDRH; // indirect register address high
375 inv->tx_buffer[1] = (uint8_t)(addr >> 8);
376 inv->tx_buffer[2] = (uint8_t)(addr & 0xFF);
377 inv->tx_buffer[3] = val;
378
379 // set indirect register address
380 inv->spi.trans.output_length = 4;
381 inv->spi.trans.input_length = 0;
382 spi_submit(inv->spi.p, &(inv->spi.trans));
383
384 return true;
385}
386
397 inv->tx_buffer[0] = reg | INV3_456_READ_FLAG;
398 /* SPI transaction */
399 inv->spi.trans.output_length = 2;
400 inv->spi.trans.input_length = 1 + size;
401 inv->tx_buffer[1] = 0;
402 spi_submit(inv->spi.p, &(inv->spi.trans));
403
404 return true;
405}
406
416 switch(inv->config_idx) {
417 case 0:
418 /* Reset the device */
420 inv->config_idx++;
421 inv->timer = get_sys_time_usec();
422 break;
423
424 case 1:
425 /* Because reset takes time wait ~5ms */
426 if((get_sys_time_usec() - inv->timer) < 5e3)
427 break;
428
429 /* Start the accel en gyro in low noise mode */
431 inv->config_idx++;
432 inv->timer = get_sys_time_usec();
433 break;
434 case 2:
435 /* Because starting takes time wait ~1ms */
436 if((get_sys_time_usec() - inv->timer) < 1e3)
437 break;
438
439 // Disable FIFO first
441 inv->config_idx++;
442 break;
443 case 3:
445 inv->config_idx++;
446 break;
447 case 4:
448 /* Configure the gyro ODR/FS */
450 inv->config_idx++;
451 break;
452 case 5: {
453 /* Configure the accel ODR/FS */
455 inv->config_idx++;
456 break;
457 }
458 case 6:
459 /* SMC_CONTROL_0 -> TMST_EN needs to be set to 1, default value is 0x60 which does not set this bit to 1; ardupilot reads whole register, we just assume it's at the default */
461 inv->config_idx++;
462 break;
463 case 7:
464 /* FIFO content: enable accel, gyro, temperature + HI-RES mode */
466 inv->config_idx++;
467 break;
468 case 8:
469 // FIFO enabled - stop-on-full, disable bypass and 2K FIFO
471 inv->config_idx++;
472 break;
473 case 9:
474 /* Enable interplator & AAF on gyro */
475 if (invensense3_register_write_bank(inv, INV3BANK_456_IPREG_SYS1_ADDR, 0xA6, (0x1B & ~(0x3 << 5)) | (0x2 << 5)))
476 inv->config_idx++;
477 break;
478 case 10:
479 /* Enable interplator & AAF on accel */
480 if (invensense3_register_write_bank(inv, INV3BANK_456_IPREG_SYS2_ADDR, 0x7B, (0x14 & ~0x3) | 0x2))
481 inv->config_idx++;
482 break;
483 case 11:
484 // Enable FIFO sensor registers
486 inv->config_idx++;
487 break;
488 default:
489 inv->timer = 0;
490 return true;
491 }
492 return false;
493}
494
495static int samples_from_odr(int odr) {
496 switch (odr) {
498 return ceilf(6400 / PERIODIC_FREQUENCY);
500 return ceilf(3200 / PERIODIC_FREQUENCY);
502 return ceilf(1600 / PERIODIC_FREQUENCY);
504 return ceilf(800 / PERIODIC_FREQUENCY);
506 return ceilf(400 / PERIODIC_FREQUENCY);
508 return ceilf(200 / PERIODIC_FREQUENCY);
510 return ceilf(100 / PERIODIC_FREQUENCY);
512 return ceilf(50 / PERIODIC_FREQUENCY);
514 return ceilf(25 / PERIODIC_FREQUENCY);
515 default:
516 // Error
517 return 0;
518 }
519}
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
angular rates
#define ACCEL_BFP_OF_REAL(_af)
#define RATE_BFP_OF_REAL(_af)
angular rates
enum SPIClockPolarity cpol
clock polarity control
Definition spi.h:151
enum SPIClockPhase cpha
clock phase control
Definition spi.h:152
enum SPISlaveSelect select
slave selection behavior
Definition spi.h:150
SPICallback before_cb
NULL or function called before the transaction.
Definition spi.h:156
SPICallback after_cb
NULL or function called after the transaction.
Definition spi.h:157
enum SPIDataSizeSelect dss
data transfer word size
Definition spi.h:153
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition spi.h:146
uint16_t input_length
number of data words to read
Definition spi.h:147
enum SPIClockDiv cdiv
prescaler of main clock to use as SPI clock
Definition spi.h:155
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition spi.h:145
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition spi.h:149
enum SPIBitOrder bitorder
MSB/LSB order.
Definition spi.h:154
uint16_t output_length
number of data words to write
Definition spi.h:148
enum SPITransactionStatus status
Definition spi.h:158
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
void invensense3_456_init(struct invensense3_456_t *inv)
Initialize the invensense v3 sensor instance.
static int samples_from_odr(int odr)
static bool invensense3_456_register_read(struct invensense3_456_t *inv, uint8_t reg, uint16_t size)
Read a register based on a combined bank/regsiter value.
static bool invensense3_456_register_write(struct invensense3_456_t *inv, uint8_t reg, uint8_t value)
Write a register.
static void invensense3_456_parse_fifo_data(struct invensense3_456_t *inv, uint8_t *data, uint16_t samples)
Parse the FIFO buffer data.
static bool invensense3_register_write_bank(struct invensense3_456_t *inv, uint16_t bank_addr, uint16_t reg, uint8_t val)
Write to a register in a specific bank.
static bool invensense3_456_config(struct invensense3_456_t *inv)
Configure the Invensense 3 device register by register.
void invensense3_456_event(struct invensense3_456_t *inv)
Should be called in the event thread.
static int32_t reassemble_uint20(uint8_t msb, uint8_t bits, uint8_t lsb)
static void invensense3_456_set_scalings(struct invensense3_456_t *inv)
This sets the correct scalings.
void invensense3_456_periodic(struct invensense3_456_t *inv)
Should be called periodically to request sensor readings.
Driver for the Invensense 456XY IMUs:
struct invensense3_456_spi_t spi
SPI specific configuration (support SPI only for now).
uint8_t config_idx
The current configuration index (used for setup/initial configuration).
int sample_numbers
expected FIFO packet number, assuming reading at PERIODIC_FREQUENCY
enum invensense3_456_device_t device
The device type detected.
uint8_t rx_buf[INVENSENSE3_456_BUFFER_SIZE]
Receive buffer.
uint8_t slave_idx
Slave index used for Slave Select.
struct spi_transaction trans
Transaction used during configuration and measurements.
@ INVENSENSE3_456_CONFIG
@ INVENSENSE3_456_IDLE
@ INVENSENSE3_456_RUNNING
enum invensense3_456_odr_t imu_odr
Accelerometer / gyro Output Data Rate configuration.
enum invensense3_456_status_t status
Status of the invensense device.
@ INVENSENSE3_456_ICM45686
@ INVENSENSE3_456_UNKOWN
float imu_samplerate
Sample rate in Hz from the imu_odr.
uint8_t abi_id
The ABI id used to broadcast the device measurements.
uint8_t tx_buf[4]
Transmit buffer.
uint32_t timer
Used to time operations during configuration (samples left during measuring).
struct spi_periph * p
Peripheral device for communication.
#define INVENSENSE3_456_FIFO_BUFFER_LEN
@ INVENSENSE3_456_ODR_100HZ
@ INVENSENSE3_456_ODR_6_4KHZ
@ INVENSENSE3_456_ODR_50HZ
@ INVENSENSE3_456_ODR_800HZ
@ INVENSENSE3_456_ODR_25HZ
@ INVENSENSE3_456_ODR_3_2KHZ
@ INVENSENSE3_456_ODR_1_6KHZ
@ INVENSENSE3_456_ODR_400HZ
@ INVENSENSE3_456_ODR_200HZ
Register and address definitions for the Invensense V3 ICM456xy sensors from ardupilot.
#define INV3REG_456_REG_MISC2
#define INV3REG_456_WHOAMI
#define GYRO_ODR_SHIFT
#define INV3REG_456_IREG_ADDRH
#define ACCEL_ODR_SHIFT
#define INV3BANK_456_IPREG_TOP1_ADDR
#define INV_456_BASE_FIFO3_CONFIG_VALUE
#define INV3REG_456_GYRO_CONFIG0
#define INV3REG_456_FIFO_CONFIG0
#define INV3BANK_456_IPREG_SYS2_ADDR
#define INV3REG_456_PWR_MGMT0
#define INV_456_FIFO_IF_EN
#define INV3_456_WHOAMI_ICM45686
#define INV3REG_456_FIFO_COUNTH
#define INV3BANK_456_IPREG_SYS1_ADDR
#define INV3_456_READ_FLAG
#define INV3REG_456_FIFO_CONFIG3
#define INV3REG_456_ACCEL_CONFIG0
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
Paparazzi atmospheric pressure conversion utilities.
Architecture independent timing functions.
Periodic telemetry system header (includes downlink utility and generated code).
uint16_t val[TCOUPLE_NB]
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.