diff --git a/conf/abi.xml b/conf/abi.xml index 869a205c22..add63110a6 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -212,17 +212,19 @@ - - - - + Registered timestamp of the measurement + Raw gyro samples measured + Amount of samples + Sampling rate in Hz of the samples + Temperature of the sensor - - - - + Registered timestamp of the measurement + Raw accelration samples measured + Amount of samples + Sampling rate in Hz of the samples + Temperature of the sensor diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml index a7099b7526..5b7722388f 100644 --- a/conf/airframes/ENAC/conf_enac.xml +++ b/conf/airframes/ENAC/conf_enac.xml @@ -142,4 +142,15 @@ settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/jevois.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_poly_rotorcraft.xml modules/stabilization_indi.xml modules/switch_servo.xml modules/tag_tracking.xml" gui_color="#c6d33e3ef958" /> + diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml index 296fb8a0df..ecba76cc44 100644 --- a/conf/airframes/examples/cube_orange.xml +++ b/conf/airframes/examples/cube_orange.xml @@ -14,7 +14,7 @@ Neddrone5 - + @@ -73,6 +73,7 @@ + diff --git a/conf/airframes/examples/matek_h743_slim.xml b/conf/airframes/examples/matek_h743_slim.xml index ae19539305..7f4825cc2b 100644 --- a/conf/airframes/examples/matek_h743_slim.xml +++ b/conf/airframes/examples/matek_h743_slim.xml @@ -28,9 +28,11 @@ - - - + + + + diff --git a/conf/boards/cube_orange.makefile b/conf/boards/cube_orange.makefile index c078ce00e7..a5c3fe05e7 100644 --- a/conf/boards/cube_orange.makefile +++ b/conf/boards/cube_orange.makefile @@ -46,7 +46,7 @@ CHIBIOS_BOARD_STARTUP = startup_stm32h7xx.mk FLASH_MODE ?= PX4_BOOTLOADER PX4_TARGET = "ap" PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/cube_orange.prototype" -PX4_BL_PORT ?= "/dev/serial/by-id/usb-Hex_ProfiCNC_CubeOrange*-BL*" +PX4_BL_PORT ?= "/dev/serial/by-id/*-BL_*,/dev/serial/by-id/*_BL_*" # # default LED configuration diff --git a/conf/boards/cube_orangeplus.makefile b/conf/boards/cube_orangeplus.makefile new file mode 100644 index 0000000000..262f13e17c --- /dev/null +++ b/conf/boards/cube_orangeplus.makefile @@ -0,0 +1,91 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# cube_orange_plus.makefile +# +# This is for the main MCU (STM32F767) on the PX4 board +# See https://pixhawk.org/modules/pixhawk for details +# + +BOARD=cube +BOARD_VERSION=orange_plus +BOARD_DIR=$(BOARD)/orange +BOARD_CFG=\"arch/chibios/common_board.h\" + +ARCH=chibios +$(TARGET).ARCHDIR = $(ARCH) + +RTOS=chibios +MCU=cortex-m7 + +# FPU on F7 +USE_FPU=hard +USE_FPU_OPT= -mfpu=fpv5-d16 + +#USE_LTO=yes + +$(TARGET).CFLAGS += -DPPRZLINK_ENABLE_FD + +############################################################################## +# Architecture or project specific options +# +# Define project name here (target) +PROJECT = $(TARGET) + +# Project specific files and paths (see Makefile.chibios for details) +CHIBIOS_BOARD_PLATFORM = STM32H7xx/platform.mk +CHIBIOS_LINKER_DIR = $(PAPARAZZI_SRC)/sw/airborne/arch/chibios/ +CHIBIOS_BOARD_LINKER = STM32H743xI.ld +CHIBIOS_BOARD_STARTUP = startup_stm32h7xx.mk + +############################################################################## +# Compiler settings +# + +# default flash mode is the PX4 bootloader +# possibilities: DFU, SWD, PX4 bootloader +FLASH_MODE ?= PX4_BOOTLOADER +PX4_TARGET = "ap" +PX4_PROTOTYPE ?= "${PAPARAZZI_HOME}/sw/tools/px4/cube_orangeplus.prototype" +PX4_BL_PORT ?= "/dev/serial/by-id/*-BL_*,/dev/serial/by-id/*_BL_*" + +# +# default LED configuration +# +SDLOG_LED ?= none +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= none +GPS_LED ?= none +SYS_TIME_LED ?= 1 + +# +# default UART configuration (modem, gps, spektrum) +# The TELEM2 port +SBUS_PORT ?= UART3 +RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3 + +# The TELEM1 port (UART3 is TELEM2) +MODEM_PORT ?= UART2 +MODEM_BAUD ?= B57600 + +# The GPS1 port (UART8 is GPS2) +GPS_PORT ?= UART4 +GPS_BAUD ?= B57600 + +# InterMCU port connected to the IO processor +INTERMCU_PORT ?= UART6 +INTERMCU_BAUD ?= B1500000 + +# Overwrite default Cube IMU slave ID +CUBE_IMU2_SPI_SLAVE_IDX ?= SPI_SLAVE5 + +# +# default actuator configuration +# +# you can use different actuators by adding a configure option to your firmware section +# e.g. +# +ACTUATORS ?= actuators_pwm + diff --git a/conf/modules/boards/cube_orangeplus.xml b/conf/modules/boards/cube_orangeplus.xml new file mode 100644 index 0000000000..d9c9171c3d --- /dev/null +++ b/conf/modules/boards/cube_orangeplus.xml @@ -0,0 +1,18 @@ + + + + + + Specific configuration for Cube Orange with ChibiOS + + + + spi_master,baro_ms5611_spi + + + + + + + + diff --git a/conf/modules/imu_apogee.xml b/conf/modules/imu_apogee.xml index b24ea741e1..dfb5cd5c38 100644 --- a/conf/modules/imu_apogee.xml +++ b/conf/modules/imu_apogee.xml @@ -33,6 +33,7 @@ + diff --git a/conf/modules/imu_bebop.xml b/conf/modules/imu_bebop.xml index 5dc38f78b0..4932b34230 100644 --- a/conf/modules/imu_bebop.xml +++ b/conf/modules/imu_bebop.xml @@ -43,6 +43,7 @@ + diff --git a/conf/modules/imu_cube.xml b/conf/modules/imu_cube.xml index 31d653e42b..fafe54341e 100644 --- a/conf/modules/imu_cube.xml +++ b/conf/modules/imu_cube.xml @@ -28,9 +28,9 @@ - + - + @@ -50,9 +50,30 @@ + + + - - + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/imu_disco.xml b/conf/modules/imu_disco.xml index c16c84d79f..d01a56e6fb 100644 --- a/conf/modules/imu_disco.xml +++ b/conf/modules/imu_disco.xml @@ -38,6 +38,7 @@ + diff --git a/conf/modules/imu_icm42688.xml b/conf/modules/imu_icm42688.xml new file mode 100644 index 0000000000..089a1b6bc7 --- /dev/null +++ b/conf/modules/imu_icm42688.xml @@ -0,0 +1,52 @@ + + + + + + Driver module for the Invensense v3 IMU ICM42688. + + + + + + + + + + + + i2c,spi_master,imu_common + imu + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/imu_matek_h743_slim.xml b/conf/modules/imu_matek_h743_slim.xml index 2bf80f6101..1cb4dc3747 100644 --- a/conf/modules/imu_matek_h743_slim.xml +++ b/conf/modules/imu_matek_h743_slim.xml @@ -7,7 +7,9 @@ MPU6000 IMU via SPI. - + + imu_mpu6000 + diff --git a/conf/modules/imu_mpu6000.xml b/conf/modules/imu_mpu6000.xml index 264aa06c80..fff72770e9 100644 --- a/conf/modules/imu_mpu6000.xml +++ b/conf/modules/imu_mpu6000.xml @@ -42,6 +42,7 @@ + diff --git a/conf/modules/imu_mpu6000_hmc5883.xml b/conf/modules/imu_mpu6000_hmc5883.xml index a71bb47afe..845887f92a 100644 --- a/conf/modules/imu_mpu6000_hmc5883.xml +++ b/conf/modules/imu_mpu6000_hmc5883.xml @@ -67,6 +67,7 @@ + diff --git a/conf/modules/imu_mpu9250_i2c.xml b/conf/modules/imu_mpu9250_i2c.xml index 7aacf7eb76..9609edbd6d 100644 --- a/conf/modules/imu_mpu9250_i2c.xml +++ b/conf/modules/imu_mpu9250_i2c.xml @@ -42,6 +42,7 @@ + diff --git a/conf/modules/imu_mpu9250_spi.xml b/conf/modules/imu_mpu9250_spi.xml index 3f47fcbf9e..16e09882f1 100644 --- a/conf/modules/imu_mpu9250_spi.xml +++ b/conf/modules/imu_mpu9250_spi.xml @@ -51,6 +51,7 @@ + diff --git a/conf/modules/imu_px4fmu_v1.7.xml b/conf/modules/imu_px4fmu_v1.7.xml index 0b2028e01b..0d7d427469 100644 --- a/conf/modules/imu_px4fmu_v1.7.xml +++ b/conf/modules/imu_px4fmu_v1.7.xml @@ -39,6 +39,7 @@ + diff --git a/conf/modules/imu_px4fmu_v2.4.xml b/conf/modules/imu_px4fmu_v2.4.xml index b1f3ab3f4c..a97311785f 100644 --- a/conf/modules/imu_px4fmu_v2.4.xml +++ b/conf/modules/imu_px4fmu_v2.4.xml @@ -49,6 +49,7 @@ + diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 071d395d37..c1f2a00851 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -32,6 +32,7 @@ #include "mcu_periph/i2c.h" #include "led.h" #include "modules/core/abi.h" +#include "generated/modules.h" // Downlink #include "mcu_periph/uart.h" @@ -140,13 +141,13 @@ void imu_apogee_event(void) (int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Y]), (int32_t)(-imu_apogee.mpu.data_rates.value[IMU_APOGEE_CHAN_Z]) }; - AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &rates, 1, imu_apogee.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &rates, 1, IMU_APOGEE_PERIODIC_FREQ, imu_apogee.mpu.temp); struct Int32Vect3 accel = { (int32_t)( imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_X]), (int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Y]), (int32_t)(-imu_apogee.mpu.data_accel.value[IMU_APOGEE_CHAN_Z]) }; - AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_apogee.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, IMU_APOGEE_PERIODIC_FREQ, imu_apogee.mpu.temp); imu_apogee.mpu.data_available = false; } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 893f6eb5f4..cabbb57c11 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -367,14 +367,14 @@ static void navdata_publish_imu(void) -navdata.measure.vy, -navdata.measure.vz }; - AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, navdata.measure.temperature_gyro); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, NAN, navdata.measure.temperature_gyro); //FIXME: samplerate? struct Int32Vect3 accel = { navdata.measure.ax, 4096 - navdata.measure.ay, 4096 - navdata.measure.az }; - AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, navdata.measure.temperature_acc); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, NAN, navdata.measure.temperature_acc); //FIXME: samplerate? struct Int32Vect3 mag = { -navdata.measure.mx, diff --git a/sw/airborne/modules/ahrs/ahrs_vectornav.c b/sw/airborne/modules/ahrs/ahrs_vectornav.c index 70c3dc8313..0db153f9ab 100644 --- a/sw/airborne/modules/ahrs/ahrs_vectornav.c +++ b/sw/airborne/modules/ahrs/ahrs_vectornav.c @@ -83,9 +83,9 @@ void ahrs_vectornav_event(void) uint32_t now_ts = get_sys_time_usec(); // in fixed point for sending as ABI and telemetry msgs RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro); - AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i, 1, ahrs_vn.vn_freq, NAN); ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel); - AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i, 1, ahrs_vn.vn_freq, NAN); } } diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index 3421e3ac98..0277744e32 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -387,6 +387,10 @@ #define IMU_NPS_ID 23 #endif +#ifndef IMU_ICM42688_ID +#define IMU_ICM42688_ID 24 +#endif + // prefiltering with OneEuro filter #ifndef IMU_F1E_ID #define IMU_F1E_ID 30 diff --git a/sw/airborne/modules/imu/imu.c b/sw/airborne/modules/imu/imu.c index 3dc51ac893..533efc4df1 100644 --- a/sw/airborne/modules/imu/imu.c +++ b/sw/airborne/modules/imu/imu.c @@ -273,8 +273,8 @@ static void send_mag_current(struct transport_tx *trans, struct link_device *dev struct Imu imu = {0}; static abi_event imu_gyro_raw_ev, imu_accel_raw_ev, imu_mag_raw_ev; -static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp); -static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp); +static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp); +static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp); static void imu_mag_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data); static void imu_set_body_to_imu_eulers(struct FloatEulers *body_to_imu_eulers); @@ -489,7 +489,7 @@ void imu_set_defaults_mag(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, } } -static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float temp) +static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *data, uint8_t samples, float rate, float temp) { // Find the correct gyro struct imu_gyro_t *gyro = imu_get_gyro(sender_id, true); @@ -510,9 +510,8 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates #if IMU_INTEGRATION // Only integrate if we have gotten a previous measurement and didn't overflow the timer - if(gyro->last_stamp > 0 && stamp > gyro->last_stamp) { + if(!isnan(rate) && gyro->last_stamp > 0 && stamp > gyro->last_stamp) { struct FloatRates integrated; - uint16_t dt = stamp - gyro->last_stamp; // Trapezoidal integration (TODO: coning correction) integrated.p = RATE_FLOAT_OF_BFP(gyro->scaled.p + scaled_rot.p) * 0.5f; @@ -538,14 +537,17 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates float_rmat_transp_ratemult(&integrated, &body_to_sensor, &integrated_sensor); } - // Divide by the amount of samples and multiply by the delta time - integrated.p = integrated.p / samples * ((float)dt * 1e-6f); - integrated.q = integrated.q / samples * ((float)dt * 1e-6f); - integrated.r = integrated.r / samples * ((float)dt * 1e-6f); + // Divide by the time of the collected samples + integrated.p = integrated.p * (1.f / rate); + integrated.q = integrated.q * (1.f / rate); + integrated.r = integrated.r * (1.f / rate); // Send the integrated values + uint16_t dt = (1e6 / rate) * samples; AbiSendMsgIMU_GYRO_INT(sender_id, stamp, &integrated, dt); } +#else + (void)rate; // Surpress compile warning not used #endif // Copy and send @@ -555,7 +557,7 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates gyro->last_stamp = stamp; } -static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float temp) +static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *data, uint8_t samples, float rate, float temp) { // Find the correct accel struct imu_accel_t *accel = imu_get_accel(sender_id, true); @@ -576,9 +578,8 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect #if IMU_INTEGRATION // Only integrate if we have gotten a previous measurement and didn't overflow the timer - if(accel->last_stamp > 0 && stamp > accel->last_stamp) { + if(!isnan(rate) && accel->last_stamp > 0 && stamp > accel->last_stamp) { struct FloatVect3 integrated; - uint16_t dt = stamp - accel->last_stamp; // Trapezoidal integration integrated.x = ACCEL_FLOAT_OF_BFP(accel->scaled.x + scaled_rot.x) * 0.5f; @@ -604,14 +605,17 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect float_rmat_transp_vmult(&integrated, &body_to_sensor, &integrated_sensor); } - // Divide by the amount of samples and multiply by the delta time - integrated.x = integrated.x / samples * ((float)dt * 1e-6f); - integrated.y = integrated.y / samples * ((float)dt * 1e-6f); - integrated.z = integrated.z / samples * ((float)dt * 1e-6f); + // Divide by the time of the collected samples + integrated.x = integrated.x * (1.f / rate); + integrated.y = integrated.y * (1.f / rate); + integrated.z = integrated.z * (1.f / rate); // Send the integrated values + uint16_t dt = (1e6 / rate) * samples; AbiSendMsgIMU_ACCEL_INT(sender_id, stamp, &integrated, dt); } +#else + (void)rate; // Surpress compile warning not used #endif // Copy and send diff --git a/sw/airborne/modules/imu/imu_aspirin.c b/sw/airborne/modules/imu/imu_aspirin.c index 8dfe9e4f68..5980d59e6e 100644 --- a/sw/airborne/modules/imu/imu_aspirin.c +++ b/sw/airborne/modules/imu/imu_aspirin.c @@ -30,6 +30,7 @@ #include "modules/core/abi.h" #include "mcu_periph/i2c.h" #include "mcu_periph/spi.h" +#include "generated/modules.h" /* defaults suitable for Lisa */ @@ -141,14 +142,14 @@ void imu_aspirin_event(void) if (imu_aspirin.acc_adxl.data_available) { struct Int32Vect3 accel; VECT3_COPY(accel, imu_aspirin.acc_adxl.data.vect); - AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &accel, 1, IMU_ASPIRIN_PERIODIC_FREQ, NAN); imu_aspirin.acc_adxl.data_available = false; } /* If the itg3200 I2C transaction has succeeded: convert the data */ itg3200_event(&imu_aspirin.gyro_itg); if (imu_aspirin.gyro_itg.data_available) { - AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1, IMU_ASPIRIN_PERIODIC_FREQ, NAN); imu_aspirin.gyro_itg.data_available = false; } diff --git a/sw/airborne/modules/imu/imu_aspirin_2_spi.c b/sw/airborne/modules/imu/imu_aspirin_2_spi.c index 2d4dc2831f..053f44910f 100644 --- a/sw/airborne/modules/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/modules/imu/imu_aspirin_2_spi.c @@ -30,6 +30,7 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/spi.h" #include "peripherals/hmc58xx_regs.h" +#include "generated/modules.h" /* defaults suitable for Lisa */ #ifndef ASPIRIN_2_SPI_SLAVE_IDX @@ -237,8 +238,8 @@ void imu_aspirin2_event(void) imu_aspirin2.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN2_ID, now_ts, &gyro, 1, imu_aspirin2.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN2_ID, now_ts, &accel, 1, imu_aspirin2.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN2_ID, now_ts, &gyro, 1, IMU_ASPIRIN2_PERIODIC_FREQ, imu_aspirin2.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN2_ID, now_ts, &accel, 1, IMU_ASPIRIN2_PERIODIC_FREQ, imu_aspirin2.mpu.temp); #if !ASPIRIN_2_DISABLE_MAG AbiSendMsgIMU_MAG_RAW(IMU_ASPIRIN2_ID, now_ts, &mag_rot); #endif diff --git a/sw/airborne/modules/imu/imu_aspirin_i2c.c b/sw/airborne/modules/imu/imu_aspirin_i2c.c index 0ca385f0fe..42ceea35af 100644 --- a/sw/airborne/modules/imu/imu_aspirin_i2c.c +++ b/sw/airborne/modules/imu/imu_aspirin_i2c.c @@ -29,6 +29,7 @@ #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" +#include "generated/modules.h" // Set SPI_CS High to enable I2C mode of ADXL345 #include "mcu_periph/gpio.h" @@ -154,14 +155,14 @@ void imu_aspirin_i2c_event(void) adxl345_i2c_event(&imu_aspirin.acc_adxl); if (imu_aspirin.acc_adxl.data_available) { - AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.acc_adxl.data.vect, 1, IMU_ASPIRIN_I2C_PERIODIC_FREQ, NAN); imu_aspirin.acc_adxl.data_available = false; } /* If the itg3200 I2C transaction has succeeded: convert the data */ itg3200_event(&imu_aspirin.gyro_itg); if (imu_aspirin.gyro_itg.data_available) { - AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_ASPIRIN_ID, now_ts, &imu_aspirin.gyro_itg.data.rates, 1, IMU_ASPIRIN_I2C_PERIODIC_FREQ, NAN); imu_aspirin.gyro_itg.data_available = false; } diff --git a/sw/airborne/modules/imu/imu_bebop.c b/sw/airborne/modules/imu/imu_bebop.c index 9bbd6f6a79..f8e748bb77 100644 --- a/sw/airborne/modules/imu/imu_bebop.c +++ b/sw/airborne/modules/imu/imu_bebop.c @@ -28,6 +28,7 @@ #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" +#include "generated/modules.h" /* defaults suitable for Bebop */ @@ -131,8 +132,8 @@ void imu_bebop_event(void) -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, imu_bebop.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_bebop.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, IMU_BEBOP_PERIODIC_FREQ, imu_bebop.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, IMU_BEBOP_PERIODIC_FREQ, imu_bebop.mpu.temp); } /* AKM8963 event task */ diff --git a/sw/airborne/modules/imu/imu_bmi088_i2c.c b/sw/airborne/modules/imu/imu_bmi088_i2c.c index 9cb0014049..d8d03d9f66 100644 --- a/sw/airborne/modules/imu/imu_bmi088_i2c.c +++ b/sw/airborne/modules/imu/imu_bmi088_i2c.c @@ -125,7 +125,7 @@ void imu_bmi088_event(void) }; imu_bmi088.bmi.gyro_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_BMI088_ID, now_ts, &rates, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_BMI088_ID, now_ts, &rates, 1, imu_bmi088.bmi.config.gyro_samplerate, NAN); } if (imu_bmi088.bmi.accel_available) { // set channel order @@ -136,7 +136,7 @@ void imu_bmi088_event(void) }; imu_bmi088.bmi.accel_available = false; - AbiSendMsgIMU_ACCEL_RAW(IMU_BMI088_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_BMI088_ID, now_ts, &accel, 1, imu_bmi088.bmi.config.accel_samplerate, NAN); } } diff --git a/sw/airborne/modules/imu/imu_cube.c b/sw/airborne/modules/imu/imu_cube.c index 0292d951e7..75df3cf534 100644 --- a/sw/airborne/modules/imu/imu_cube.c +++ b/sw/airborne/modules/imu/imu_cube.c @@ -24,15 +24,21 @@ * Driver for the IMU's in the Cube autopilots. */ +#include "generated/modules.h" #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" #include "peripherals/invensense2.h" +#include "peripherals/invensense3.h" #include "peripherals/mpu60x0_spi.h" static struct invensense2_t imu1; +#if IMU_CUBE_ORANGEPLUS +static struct invensense3_t imu2; +#else static struct Mpu60x0_Spi imu2; +#endif static struct invensense2_t imu3; void imu_cube_init(void) @@ -59,6 +65,31 @@ void imu_cube_init(void) imu_set_defaults_gyro(IMU_CUBE1_ID, &rmat, NULL, NULL); imu_set_defaults_accel(IMU_CUBE1_ID, &rmat, NULL, NULL); +#if IMU_CUBE_ORANGEPLUS + /* IMU 2 (ICM20602 isolated) */ + imu2.abi_id = IMU_CUBE2_ID; + imu2.parser = INVENSENSE3_PARSER_FIFO; + imu2.bus = INVENSENSE3_SPI; + imu2.spi.p = &CUBE_IMU2_SPI_DEV; + imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX; + + imu2.gyro_odr = INVENSENSE3_GYRO_ODR_4KHZ; + imu2.gyro_range = INVENSENSE3_GYRO_RANGE_500DPS; + imu2.accel_odr = INVENSENSE3_ACCEL_ODR_4KHZ; + imu2.accel_range = INVENSENSE3_ACCEL_RANGE_32G; + imu2.gyro_aaf = 977; // ~ODR/4 + imu2.accel_aaf = 213; // Fixed + + invensense3_init(&imu2); + + // Rotation + eulers.phi = ANGLE_BFP_OF_REAL(0), + eulers.theta = ANGLE_BFP_OF_REAL(0); + eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90)); + int32_rmat_of_eulers(&rmat, &eulers); + imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL); + imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL); +#else /* IMU 2 (ICM20602 isolated) */ mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX); // change the default configuration @@ -75,6 +106,7 @@ void imu_cube_init(void) int32_rmat_of_eulers(&rmat, &eulers); imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_GYRO_SENS_FRAC[MPU60X0_GYRO_RANGE_2000]); imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, MPU60X0_ACCEL_SENS_FRAC[MPU60X0_ACCEL_RANGE_16G]); +#endif /* IMU 3 (ICM2094 isolated) */ imu3.abi_id = IMU_CUBE3_ID; @@ -99,7 +131,11 @@ void imu_cube_init(void) void imu_cube_periodic(void) { invensense2_periodic(&imu1); +#if IMU_CUBE_ORANGEPLUS + invensense3_periodic(&imu2); +#else mpu60x0_spi_periodic(&imu2); +#endif invensense2_periodic(&imu3); } @@ -107,6 +143,9 @@ void imu_cube_event(void) { invensense2_event(&imu1); +#if IMU_CUBE_ORANGEPLUS + invensense3_event(&imu2); +#else mpu60x0_spi_event(&imu2); if (imu2.data_available) { uint32_t now_ts = get_sys_time_usec(); @@ -126,9 +165,10 @@ void imu_cube_event(void) imu2.data_available = false; // Send the scaled values over ABI - AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1, imu2.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1, imu2.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1, IMU_CUBE_PERIODIC_FREQ, imu2.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1, IMU_CUBE_PERIODIC_FREQ, imu2.temp); } +#endif invensense2_event(&imu3); } diff --git a/sw/airborne/modules/imu/imu_disco.c b/sw/airborne/modules/imu/imu_disco.c index b3541f749a..9e356ede80 100644 --- a/sw/airborne/modules/imu/imu_disco.c +++ b/sw/airborne/modules/imu/imu_disco.c @@ -28,6 +28,7 @@ #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/i2c.h" +#include "generated/modules.h" /* I2C is hardwired on Disco autopilot */ @@ -131,8 +132,8 @@ void imu_disco_event(void) imu_disco.mpu.data_accel.vect.z); imu_disco.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, imu_disco.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_disco.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, IMU_DISCO_PERIODIC_FREQ, imu_disco.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, IMU_DISCO_PERIODIC_FREQ, imu_disco.mpu.temp); } /* AKM8963 event task */ diff --git a/sw/airborne/modules/imu/imu_heater.c b/sw/airborne/modules/imu/imu_heater.c index 8e1636264c..40bbe04052 100644 --- a/sw/airborne/modules/imu/imu_heater.c +++ b/sw/airborne/modules/imu/imu_heater.c @@ -75,7 +75,7 @@ static void send_imu_heater(struct transport_tx *trans, struct link_device *dev) #endif #if defined(IMU_HEATER_GYRO_ID) -static void imu_heater_gyro_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Rates *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float temp) { +static void imu_heater_gyro_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Rates *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float rate __attribute__((unused)), float temp) { if(isnan(temp)) return; @@ -85,7 +85,7 @@ static void imu_heater_gyro_raw_cb(uint8_t sender_id __attribute__((unused)), ui #endif #if defined(IMU_HEATER_ACCEL_ID) -static void imu_heater_accel_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float temp) { +static void imu_heater_accel_raw_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct Int32Vect3 *data __attribute__((unused)), uint8_t samples __attribute__((unused)), float rate __attribute__((unused)), float temp) { if(isnan(temp)) return; diff --git a/sw/airborne/modules/imu/imu_icm42688.c b/sw/airborne/modules/imu/imu_icm42688.c new file mode 100644 index 0000000000..e2ee9025f6 --- /dev/null +++ b/sw/airborne/modules/imu/imu_icm42688.c @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2022 Freek van tieen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file modules/imu/imu_icm42688.c + * Driver module for the Invensense v3 IMU ICM42688. + */ + +#include "modules/imu/imu_icm42688.h" +#include "modules/imu/imu.h" +#include "modules/core/abi.h" +#include "mcu_periph/spi.h" +#include "peripherals/invensense3.h" + + +#ifndef INVENSENSE3_GYRO_AAF +#define INVENSENSE3_GYRO_AAF 977 +#endif + +#ifndef INVENSENSE3_ACCEL_AAF +#define INVENSENSE3_ACCEL_AAF 213 +#endif + +static struct invensense3_t imu_icm42688; + +void imu_icm42688_init(void) +{ + imu_icm42688.abi_id = IMU_ICM42688_ID; + //imu_icm42688.parser = INVENSENSE3_PARSER_REGISTERS; + imu_icm42688.parser = INVENSENSE3_PARSER_FIFO; + + imu_icm42688.bus = INVENSENSE3_SPI; + imu_icm42688.spi.p = &IMU_SPI_DEV; + imu_icm42688.spi.slave_idx = IMU_SPI_SLAVE_IDX; + + imu_icm42688.gyro_odr = INVENSENSE3_GYRO_ODR; + imu_icm42688.gyro_range = INVENSENSE3_GYRO_RANGE; + imu_icm42688.accel_odr = INVENSENSE3_ACCEL_ODR; + imu_icm42688.accel_range = INVENSENSE3_ACCEL_RANGE; + + // The AAF freq needs to be high enough to avoid group delay and low enough to minimise noise and clipping + imu_icm42688.gyro_aaf = INVENSENSE3_GYRO_AAF; // ~ODR/4 + imu_icm42688.accel_aaf = INVENSENSE3_ACCEL_AAF; // Fixed + + invensense3_init(&imu_icm42688); +} + +void imu_icm42688_periodic(void) +{ + invensense3_periodic(&imu_icm42688); +} + +void imu_icm42688_event(void) +{ + invensense3_event(&imu_icm42688); +} + + + diff --git a/sw/airborne/modules/imu/imu_icm42688.h b/sw/airborne/modules/imu/imu_icm42688.h new file mode 100644 index 0000000000..6d3cae5f72 --- /dev/null +++ b/sw/airborne/modules/imu/imu_icm42688.h @@ -0,0 +1,36 @@ +/* + * Copyright (C) 2022 Jesús Bautista Villar + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file modules/imu/imu_icm42688.h + * Driver for the IMU ICM42688. + */ + +#ifndef IMU_ICM42688_H +#define IMU_ICM42688_H + +#include "std.h" + +extern void imu_icm42688_init(void); +extern void imu_icm42688_periodic(void); +extern void imu_icm42688_event(void); + +#endif /* IMU_ICM42688_H */ diff --git a/sw/airborne/modules/imu/imu_mpu6000.c b/sw/airborne/modules/imu/imu_mpu6000.c index 66c58a9908..9d38905c88 100644 --- a/sw/airborne/modules/imu/imu_mpu6000.c +++ b/sw/airborne/modules/imu/imu_mpu6000.c @@ -29,6 +29,7 @@ #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" +#include "generated/modules.h" #if IMU_MPU_USE_MEDIAN_FILTER #include "filters/median_filter.h" #endif @@ -165,7 +166,7 @@ void imu_mpu_spi_event(void) imu_mpu_spi.mpu.data_available = false; // Send the scaled values over ABI - AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_ID, now_ts, &rates, 1, imu_mpu_spi.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_ID, now_ts, &accel, 1, imu_mpu_spi.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_ID, now_ts, &rates, 1, IMU_MPU_SPI_PERIODIC_FREQ, imu_mpu_spi.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_ID, now_ts, &accel, 1, IMU_MPU_SPI_PERIODIC_FREQ, imu_mpu_spi.mpu.temp); } } diff --git a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c index 05edd91cd7..17be9e8c80 100644 --- a/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c +++ b/sw/airborne/modules/imu/imu_mpu6000_hmc5883.c @@ -29,6 +29,7 @@ #include "modules/core/abi.h" #include "mcu_periph/spi.h" #include "peripherals/hmc58xx_regs.h" +#include "generated/modules.h" /* SPI/I2C defaults set in subsystem makefile, can be configured from airframe file */ @@ -167,8 +168,8 @@ void imu_mpu_hmc_event(void) }; imu_mpu_hmc.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_HMC_ID, now_ts, &rates, 1, imu_mpu_hmc.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_HMC_ID, now_ts, &accel, 1, imu_mpu_hmc.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU6000_HMC_ID, now_ts, &rates, 1, IMU_MPU_HMC_PERIODIC_FREQ, imu_mpu_hmc.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU6000_HMC_ID, now_ts, &accel, 1, IMU_MPU_HMC_PERIODIC_FREQ, imu_mpu_hmc.mpu.temp); } /* HMC58XX event task */ diff --git a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c index 3d2d76eec7..cba9683190 100644 --- a/sw/airborne/modules/imu/imu_mpu60x0_i2c.c +++ b/sw/airborne/modules/imu/imu_mpu60x0_i2c.c @@ -30,6 +30,7 @@ #include "modules/core/abi.h" #include "mcu_periph/i2c.h" #include "led.h" +#include "generated/modules.h" /* MPU60x0 gyro/accel internal lowpass frequency */ #if !defined IMU_MPU60X0_LOWPASS_FILTER && !defined IMU_MPU60X0_SMPLRT_DIV @@ -89,8 +90,8 @@ void imu_mpu_i2c_event(void) // If the MPU60X0 I2C transaction has succeeded: convert the data mpu60x0_i2c_event(&imu_mpu_i2c.mpu); if (imu_mpu_i2c.mpu.data_available) { - AbiSendMsgIMU_GYRO_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_rates.rates, 1, imu_mpu_i2c.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_accel.vect, 1, imu_mpu_i2c.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_rates.rates, 1, IMU_MPU_I2C_PERIODIC_FREQ, imu_mpu_i2c.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU60X0_ID, now_ts, &imu_mpu_i2c.mpu.data_accel.vect, 1, IMU_MPU_I2C_PERIODIC_FREQ, imu_mpu_i2c.mpu.temp); imu_mpu_i2c.mpu.data_available = false; } } diff --git a/sw/airborne/modules/imu/imu_mpu9250_i2c.c b/sw/airborne/modules/imu/imu_mpu9250_i2c.c index 7199592e91..84d0a2dd08 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_i2c.c +++ b/sw/airborne/modules/imu/imu_mpu9250_i2c.c @@ -30,6 +30,7 @@ #include "mcu_periph/i2c.h" #include "mcu_periph/sys_time.h" #include "modules/core/abi.h" +#include "generated/modules.h" #if !defined IMU_MPU9250_GYRO_LOWPASS_FILTER && !defined IMU_MPU9250_ACCEL_LOWPASS_FILTER && !defined IMU_MPU9250_SMPLRT_DIV #if (PERIODIC_FREQUENCY >= 60) && (PERIODIC_FREQUENCY <= 120) @@ -142,8 +143,8 @@ void imu_mpu9250_event(void) }; imu_mpu9250.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1, NAN); - AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1, IMU_MPU9250_PERIODIC_FREQ, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1, IMU_MPU9250_PERIODIC_FREQ, NAN); } #if IMU_MPU9250_READ_MAG // Test if mag data are updated diff --git a/sw/airborne/modules/imu/imu_mpu9250_spi.c b/sw/airborne/modules/imu/imu_mpu9250_spi.c index e73e97639d..a500857116 100644 --- a/sw/airborne/modules/imu/imu_mpu9250_spi.c +++ b/sw/airborne/modules/imu/imu_mpu9250_spi.c @@ -31,6 +31,7 @@ #include "mcu_periph/sys_time.h" #include "mcu_periph/spi.h" #include "peripherals/ak8963_regs.h" +#include "generated/modules.h" /* SPI defaults set in subsystem makefile, can be configured from airframe file */ PRINT_CONFIG_VAR(IMU_MPU9250_SPI_SLAVE_IDX) @@ -200,8 +201,8 @@ void imu_mpu9250_event(void) #endif imu_mpu9250.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1, NAN); - AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_MPU9250_ID, now_ts, &rates, 1, IMU_MPU9250_PERIODIC_FREQ, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_MPU9250_ID, now_ts, &accel, 1, IMU_MPU9250_PERIODIC_FREQ, NAN); } } diff --git a/sw/airborne/modules/imu/imu_nps.c b/sw/airborne/modules/imu/imu_nps.c index 4abb2ca935..b323b0c3d5 100644 --- a/sw/airborne/modules/imu/imu_nps.c +++ b/sw/airborne/modules/imu/imu_nps.c @@ -88,11 +88,11 @@ void imu_nps_event(void) { uint32_t now_ts = get_sys_time_usec(); if (imu_nps.gyro_available) { - AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_NPS_ID, now_ts, &imu_nps.gyro, 1, NPS_PROPAGATE, NAN); imu_nps.gyro_available = false; } if (imu_nps.accel_available) { - AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_NPS_ID, now_ts, &imu_nps.accel, 1, NPS_PROPAGATE, NAN); imu_nps.accel_available = false; } if (imu_nps.mag_available) { diff --git a/sw/airborne/modules/imu/imu_px4fmu.c b/sw/airborne/modules/imu/imu_px4fmu.c index b3c1000647..5488274d7e 100644 --- a/sw/airborne/modules/imu/imu_px4fmu.c +++ b/sw/airborne/modules/imu/imu_px4fmu.c @@ -29,6 +29,7 @@ #include "modules/core/abi.h" #include "mcu_periph/spi.h" #include "peripherals/hmc58xx_regs.h" +#include "generated/modules.h" /* MPU60x0 gyro/accel internal lowpass frequency */ @@ -103,8 +104,8 @@ void imu_px4fmu_event(void) -imu_px4fmu.mpu.data_accel.vect.z }; imu_px4fmu.mpu.data_available = false; - AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, imu_px4fmu.mpu.temp); - AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, imu_px4fmu.mpu.temp); + AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, IMU_PX4FMU_PERIODIC_FREQ, imu_px4fmu.mpu.temp); + AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, IMU_PX4FMU_PERIODIC_FREQ, imu_px4fmu.mpu.temp); } /* HMC58XX event task */ diff --git a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c index 478de45123..3e168250d1 100644 --- a/sw/airborne/modules/imu/imu_px4fmu_v2.4.c +++ b/sw/airborne/modules/imu/imu_px4fmu_v2.4.c @@ -29,6 +29,7 @@ #include "modules/imu/imu.h" #include "modules/core/abi.h" #include "mcu_periph/spi.h" +#include "generated/modules.h" /* SPI defaults set in subsystem makefile, can be configured from airframe file */ PRINT_CONFIG_VAR(IMU_LSM_SPI_SLAVE_IDX) @@ -90,7 +91,7 @@ void imu_px4_event(void) { imu_px4.l3g.data_rates.rates.r }; imu_px4.l3g.data_available = FALSE; - AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_PX4_ID, now_ts, &gyro, 1, IMU_PX4_PERIODIC_FREQ, NAN); } /* LSM303d event task */ @@ -98,7 +99,7 @@ void imu_px4_event(void) { if (imu_px4.lsm_acc.data_available_acc) { struct Int32Vect3 accel; VECT3_COPY(accel, imu_px4.lsm_acc.data_accel.vect); - AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_PX4_ID, now_ts, &accel, 1, IMU_PX4_PERIODIC_FREQ, NAN); imu_px4.lsm_acc.data_available_acc = FALSE; } #if !IMU_PX4_DISABLE_MAG diff --git a/sw/airborne/modules/imu/imu_vectornav.c b/sw/airborne/modules/imu/imu_vectornav.c index 4987de82a4..e42a52a132 100644 --- a/sw/airborne/modules/imu/imu_vectornav.c +++ b/sw/airborne/modules/imu/imu_vectornav.c @@ -148,6 +148,6 @@ void imu_vectornav_propagate(void) uint32_t now_ts = (uint32_t) tmp; // Send accel and gyro data separate for other AHRS algorithms - AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.gyro, 1, NAN); - AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.accel, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.gyro, 1, imu_vn.vn_freq, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_VECTORNAV_ID, now_ts, &imu_vn.vn_data.accel, 1, imu_vn.vn_freq, NAN); } diff --git a/sw/airborne/modules/ins/imu_xsens.c b/sw/airborne/modules/ins/imu_xsens.c index da4f49872a..b868702ac9 100644 --- a/sw/airborne/modules/ins/imu_xsens.c +++ b/sw/airborne/modules/ins/imu_xsens.c @@ -61,7 +61,7 @@ static void handle_ins_msg(void) RATE_BFP_OF_REAL(xsens.gyro.r) }; xsens.gyro_available = FALSE - AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN, NAN); //FIXME: samplerate? } if (xsens.accel_available) { struct Int32Vect3 accel = { @@ -70,7 +70,7 @@ static void handle_ins_msg(void) ACCEL_BFP_OF_REAL(xsens.accel.z) }; xsens.accel_available = FALSE; - AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN, NAN); //FIXME: samplerate? } if (xsens.mag_available) { struct Int32Vect3 mag = { @@ -88,7 +88,7 @@ static void handle_ins_msg(void) RATE_BFP_OF_REAL(xsens.gyro.q), RATE_BFP_OF_REAL(xsens.gyro.r) }; - AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN); + AbiSendMsgIMU_GYRO_RAW(IMU_XSENS_ID, now_ts, &gyro, 1, NAN, NAN); //FIXME: samplerate? xsens.gyro_available = FALSE; } if (xsens.accel_available) { @@ -97,7 +97,7 @@ static void handle_ins_msg(void) ACCEL_BFP_OF_REAL(xsens.accel.y), ACCEL_BFP_OF_REAL(xsens.accel.z) }; - AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN); + AbiSendMsgIMU_ACCEL_RAW(IMU_XSENS_ID, now_ts, &accel, 1, NAN, NAN); //FIXME: samplerate? xsens.accel_available = FALSE; } if (xsens.mag_available) { diff --git a/sw/airborne/peripherals/bmi088.c b/sw/airborne/peripherals/bmi088.c index 3bd9915683..f428ed52c9 100644 --- a/sw/airborne/peripherals/bmi088.c +++ b/sw/airborne/peripherals/bmi088.c @@ -115,6 +115,54 @@ void bmi088_send_config(Bmi088ConfigSet bmi_set, void *bmi, struct Bmi088Config config->init_status++; break; case BMI088_CONF_DONE: + /* Set the samplerates from the ODR */ + switch(config->gyro_odr) { + case BMI088_GYRO_ODR_2000_BW_532: + case BMI088_GYRO_ODR_2000_BW_230: + config->gyro_samplerate = 2000; + break; + case BMI088_GYRO_ODR_1000_BW_116: + config->gyro_samplerate = 1000; + break; + case BMI088_GYRO_ODR_400_BW_47: + config->gyro_samplerate = 400; + break; + case BMI088_GYRO_ODR_200_BW_23: + case BMI088_GYRO_ODR_200_BW_64: + config->gyro_samplerate = 200; + break; + case BMI088_GYRO_ODR_100_BW_12: + case BMI088_GYRO_ODR_100_BW_32: + config->gyro_samplerate = 100; + break; + } + switch(config->accel_odr) { + case BMI088_ACCEL_ODR_1600: + config->accel_samplerate = 1600; + break; + case BMI088_ACCEL_ODR_800: + config->accel_samplerate = 800; + break; + case BMI088_ACCEL_ODR_400: + config->accel_samplerate = 400; + break; + case BMI088_ACCEL_ODR_200: + config->accel_samplerate = 200; + break; + case BMI088_ACCEL_ODR_100: + config->accel_samplerate = 100; + break; + case BMI088_ACCEL_ODR_50: + config->accel_samplerate = 50; + break; + case BMI088_ACCEL_ODR_25: + config->accel_samplerate = 25; + break; + case BMI088_ACCEL_ODR_12: + config->accel_samplerate = 12; + break; + } + config->initialized = true; break; default: diff --git a/sw/airborne/peripherals/bmi088.h b/sw/airborne/peripherals/bmi088.h index a8de841205..fbb20eeac1 100644 --- a/sw/airborne/peripherals/bmi088.h +++ b/sw/airborne/peripherals/bmi088.h @@ -119,6 +119,9 @@ struct Bmi088Config { enum Bmi088AccelBW accel_bw; ///< bandwidth enum Bmi088ConfStatus init_status; ///< init status bool initialized; ///< config done flag + + float gyro_samplerate; ///< samplerate in Hz from gyro_odr + float accel_samplerate; ///< samplerate in Hz from accel_odr }; extern void bmi088_set_default_config(struct Bmi088Config *c); diff --git a/sw/airborne/peripherals/invensense2.c b/sw/airborne/peripherals/invensense2.c index b89fa44fd8..3a3c6f4e77 100644 --- a/sw/airborne/peripherals/invensense2.c +++ b/sw/airborne/peripherals/invensense2.c @@ -74,7 +74,7 @@ static const struct Int32Vect3 invensense2_accel_scale[5][2] = { /** * @brief Initialize the invensense v2 sensor instance * - * @param inv The structure containing the configuratio of the invensense v2 instance + * @param inv The structure containing the configuration of the invensense v2 instance */ void invensense2_init(struct invensense2_t *inv) { /* General setup */ @@ -314,8 +314,8 @@ static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t * // Send the scaled values over ABI uint32_t now_ts = get_sys_time_usec(); - AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples, temp_f); - AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, temp_f); + AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples, gyro_samplerate, temp_f); + AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, accel_samplerate, temp_f); } /** diff --git a/sw/airborne/peripherals/invensense2.h b/sw/airborne/peripherals/invensense2.h index 80ab174dff..8d9d0b1f61 100644 --- a/sw/airborne/peripherals/invensense2.h +++ b/sw/airborne/peripherals/invensense2.h @@ -139,8 +139,6 @@ struct invensense2_t { enum invensense2_gyro_range_t gyro_range; ///< Gyro range configuration enum invensense2_accel_dlpf_t accel_dlpf; ///< Accelerometer DLPF configuration enum invensense2_accel_range_t accel_range; ///< Accelerometer range configuration - - // float temp; ///< temperature in degrees Celcius }; /* External functions */ diff --git a/sw/airborne/peripherals/invensense3.c b/sw/airborne/peripherals/invensense3.c new file mode 100644 index 0000000000..c4be3c7a76 --- /dev/null +++ b/sw/airborne/peripherals/invensense3.c @@ -0,0 +1,990 @@ +/* + * Copyright (C) 2022 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/invensense3.c + * + * Driver for the Invensense v3 IMUs + * ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688 + */ + +#include "peripherals/invensense3.h" +#include "peripherals/invensense3_regs.h" +#include "math/pprz_isa.h" +#include "math/pprz_algebra_int.h" +#include "modules/imu/imu.h" +#include "modules/core/abi.h" +#include "mcu_periph/gpio_arch.h" +#include "std.h" + +/* Local functions */ +static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data, uint16_t samples); +static void invensense3_parse_reg_data(struct invensense3_t *inv, uint8_t *data); +static void invensense3_fix_config(struct invensense3_t *inv); +static bool invensense3_register_write(struct invensense3_t *inv, uint16_t bank_reg, uint8_t value); +static bool invensense3_register_read(struct invensense3_t *inv, uint16_t bank_reg, uint16_t size); +static bool invensense3_select_bank(struct invensense3_t *inv, uint8_t bank); +static bool invensense3_config(struct invensense3_t *inv); +static bool invensense3_reset_fifo(struct invensense3_t *inv); +static int samples_from_odr(int odr); + +/* Default gyro scalings */ +static const struct Int32Rates invensense3_gyro_scale[8][2] = { + { {40147, 40147, 40147}, + {9210, 9210, 9210} }, // 2000DPS + { {40147, 40147, 40147}, + {18420, 18420, 18420} }, // 1000DPS + { {60534, 60534, 60534}, + {55463, 55463, 55463} }, // 500DPS + { {30267, 30267, 30267}, + {55463, 55463, 55463} }, // 250DPS + { {30267, 30267, 30267}, + {110926, 110926, 110926} }, // 125DPS vvv (TODO: the new scales are not tested yet) vvv + { {3292054, 3292054, 3292054}, + {24144015, 24144015, 24144015} }, // 62.5DPS + { {1646027, 1646027, 1646027}, + {24144015, 24144015, 24144015} }, // 31.25DPS + { {1646027, 1646027, 1646027}, + {48288030, 48288030, 48288030} }, // 15.625DPS +}; + +/* Default accel scalings */ +static const struct Int32Vect3 invensense3_accel_scale[5][2] = { + { {51024, 51024, 51024}, + {5203, 5203, 5203} }, // 32G + { {25512, 25512, 25512}, + {5203, 5203, 5203} }, // 16G + { {12756, 12756, 12756}, + {5203, 5203, 5203} }, // 8G + { {6378, 6378, 6378}, + {5203, 5203, 5203} }, // 4G + { {3189, 3189, 3189}, + {5203, 5203, 5203} } // 2G +}; + +/* AAF settings (3dB Bandwidth [Hz], AAF_DELT, AAF_DELTSQR, AAF_BITSHIFT) */ +static const uint16_t invensense3_aaf[][4] = { + {42, 1, 1, 15}, + {84, 2, 4, 13}, + {126, 3, 9, 12}, + {170, 4, 16, 11}, + {213, 5, 25, 10}, + {258, 6, 36, 10}, + {303, 7, 49, 9}, + {348, 8, 64, 9}, + {394, 9, 81, 9}, + {441, 10, 100, 8}, + {488, 11, 122, 8}, + {536, 12, 144, 8}, + {585, 13, 170, 8}, + {634, 14, 196, 7}, + {684, 15, 224, 7}, + {734, 16, 256, 7}, + {785, 17, 288, 7}, + {837, 18, 324, 7}, + {890, 19, 360, 6}, + {943, 20, 400, 6}, + {997, 21, 440, 6}, + {1051, 22, 488, 6}, + {1107, 23, 528, 6}, + {1163, 24, 576, 6}, + {1220, 25, 624, 6}, + {1277, 26, 680, 6}, + {1336, 27, 736, 5}, + {1395, 28, 784, 5}, + {1454, 29, 848, 5}, + {1515, 30, 896, 5}, + {1577, 31, 960, 5}, + {1639, 32, 1024, 5}, + {1702, 33, 1088, 5}, + {1766, 34, 1152, 5}, + {1830, 35, 1232, 5}, + {1896, 36, 1296, 5}, + {1962, 37, 1376, 4}, + {2029, 38, 1440, 4}, + {2097, 39, 1536, 4}, + {2166, 40, 1600, 4}, + {2235, 41, 1696, 4}, + {2306, 42, 1760, 4}, + {2377, 43, 1856, 4}, + {2449, 44, 1952, 4}, + {2522, 45, 2016, 4}, + {2596, 46, 2112, 4}, + {2671, 47, 2208, 4}, + {2746, 48, 2304, 4}, + {2823, 49, 2400, 4}, + {2900, 50, 2496, 4}, + {2978, 51, 2592, 4}, + {3057, 52, 2720, 4}, + {3137, 53, 2816, 3}, + {3217, 54, 2944, 3}, + {3299, 55, 3008, 3}, + {3381, 56, 3136, 3}, + {3464, 57, 3264, 3}, + {3548, 58, 3392, 3}, + {3633, 59, 3456, 3}, + {3718, 60, 3584, 3}, + {3805, 61, 3712, 3}, + {3892, 62, 3840, 3}, + {3979, 63, 3968, 3} +}; + +static const uint16_t invensense3_aaf4x605[][4] = { + {10, 1, 1, 15}, + {21, 2, 4, 13}, + {32, 3, 9, 12}, + {42, 4, 16, 11}, + {53, 5, 25, 10}, + {64, 6, 36, 10}, + {76, 7, 49, 9}, + {87, 8, 64, 9}, + {99, 9, 81, 9}, + {110, 10, 100, 8}, + {122, 11, 122, 8}, + {134, 12, 144, 8}, + {146, 13, 170, 8}, + {158, 14, 196, 7}, + {171, 15, 224, 7}, + {184, 16, 256, 7}, + {196, 17, 288, 7}, + {209, 18, 324, 7}, + {222, 19, 360, 6}, + {236, 20, 400, 6}, + {249, 21, 440, 6}, + {263, 22, 488, 6}, + {277, 23, 528, 6}, + {291, 24, 576, 6}, + {305, 25, 624, 6}, + {319, 26, 680, 6}, + {334, 27, 736, 5}, + {349, 28, 784, 5}, + {364, 29, 848, 5}, + {379, 30, 896, 5}, + {394, 31, 960, 5}, + {410, 32, 1024, 5}, + {425, 33, 1088, 5}, + {441, 34, 1152, 5}, + {458, 35, 1232, 5}, + {474, 36, 1296, 5}, + {490, 37, 1376, 4}, + {507, 38, 1440, 4}, + {524, 39, 1536, 4}, + {541, 40, 1600, 4}, + {559, 41, 1696, 4}, + {576, 42, 1760, 4}, + {594, 43, 1856, 4}, + {612, 44, 1952, 4}, + {631, 45, 2016, 4}, + {649, 46, 2112, 4}, + {668, 47, 2208, 4}, + {687, 48, 2304, 4}, + {706, 49, 2400, 4}, + {725, 50, 2496, 4}, + {745, 51, 2592, 4}, + {764, 52, 2720, 4}, + {784, 53, 2816, 3}, + {804, 54, 2944, 3}, + {825, 55, 3008, 3}, + {845, 56, 3136, 3}, + {866, 57, 3264, 3}, + {887, 58, 3392, 3}, + {908, 59, 3456, 3}, + {930, 60, 3584, 3}, + {951, 61, 3712, 3}, + {973, 62, 3840, 3}, + {995, 63, 3968, 3} +}; + +static const uint8_t invensense3_fifo_sample_size[4] = {8,8,16,20}; + +/** + * @brief Initialize the invensense v3 sensor instance + * + * @param inv The structure containing the configuration of the invensense v3 instance + */ +void invensense3_init(struct invensense3_t *inv) { + /* General setup */ + inv->status = INVENSENSE3_IDLE; + inv->device = INVENSENSE3_UNKOWN; + inv->register_bank = 0xFF; + inv->config_idx = 0; + + /* SPI setup */ + if(inv->bus == INVENSENSE3_SPI) { + inv->spi.trans.cpol = SPICpolIdleHigh; + inv->spi.trans.cpha = SPICphaEdge2; + inv->spi.trans.dss = SPIDss8bit; + inv->spi.trans.bitorder = SPIMSBFirst; + inv->spi.trans.cdiv = SPIDiv16; + + inv->spi.trans.select = SPISelectUnselect; + inv->spi.trans.slave_idx = inv->spi.slave_idx; + inv->spi.trans.output_length = 0; + inv->spi.trans.input_length = 0; + inv->spi.trans.before_cb = NULL; + inv->spi.trans.after_cb = NULL; + inv->spi.trans.input_buf = inv->spi.rx_buf; + inv->spi.trans.output_buf = inv->spi.tx_buf; + inv->spi.trans.status = SPITransDone; + + inv->rx_buffer = inv->spi.rx_buf; + inv->tx_buffer = inv->spi.tx_buf; + inv->rx_length = &inv->spi.trans.input_length; + } + /* I2C setup */ + else { + inv->i2c.trans.slave_addr = inv->i2c.slave_addr; + inv->i2c.trans.status = I2CTransDone; + + inv->rx_buffer = (uint8_t *)inv->i2c.trans.buf; + inv->tx_buffer = (uint8_t *)inv->i2c.trans.buf; + inv->rx_length = &inv->i2c.trans.len_r; + } + + inv->sample_size = INVENSENSE3_SAMPLE_SIZE_PK3; + + inv->sample_numbers = samples_from_odr(Min((int)inv->gyro_odr, (int)inv->accel_odr)); +} + +/** + * @brief Should be called periodically to request sensor readings + * - First detects the sensor using WHO_AM_I reading + * - Configures the sensor according the users requested configuration + * - Requests a sensor reading by reading the FIFO_COUNT register + * + * @param inv The invensense v3 instance + */ +void invensense3_periodic(struct invensense3_t *inv) { + /* Idle */ + if((inv->bus == INVENSENSE3_SPI && inv->spi.trans.status == SPITransDone) || + (inv->bus == INVENSENSE3_I2C && inv->i2c.trans.status == I2CTransDone)) { + + /* Depending on the status choose what to do */ + switch(inv->status) { + case INVENSENSE3_IDLE: + /* Request WHO_AM_I */ + invensense3_register_read(inv, INV3REG_WHO_AM_I, 1); + break; + + case INVENSENSE3_CONFIG: + /* Start configuring */ + if(invensense3_config(inv)) { + inv->status = INVENSENSE3_RUNNING; + } + break; + + case INVENSENSE3_RUNNING: + /* Request a sensor reading */ + switch (inv->parser) { + case INVENSENSE3_PARSER_REGISTERS: + invensense3_register_read(inv, INV3REG_TEMP_DATA1, 14); + break; + case INVENSENSE3_PARSER_FIFO: + { + static const uint16_t max_bytes = sizeof(inv->spi.rx_buf) - 3; + int read_bytes = (inv->sample_numbers + inv->timer) * invensense3_fifo_sample_size[inv->sample_size]; + read_bytes = Min(max_bytes, read_bytes); + // round to the packet boundaries + read_bytes -= read_bytes % invensense3_fifo_sample_size[inv->sample_size]; + invensense3_register_read(inv, INV3REG_FIFO_COUNTH, read_bytes + 2); + inv->timer = 0; // reset leftover bytes + break; + } + default: break; + } + + default: break; + } + } +} + +/** + * @brief Should be called in the event thread + * - Checks the response of the WHO_AM_I reading + * - Configures the sensor and reads the responses + * - Parse and request the sensor data from the FIFO buffers + * + * @param inv The invensense v3 instance + */ +void invensense3_event(struct invensense3_t *inv) { + + /* Successful transfer */ + if((inv->bus == INVENSENSE3_SPI && inv->spi.trans.status == SPITransSuccess) || + (inv->bus == INVENSENSE3_I2C && inv->i2c.trans.status == I2CTransSuccess)) { + + /* Update the register bank */ + if(inv->tx_buffer[0] == INV3REG_BANK_SEL) + inv->register_bank = inv->tx_buffer[1]; + + /* Set the transaction as done and update register bank if needed */ + if(inv->bus == INVENSENSE3_SPI) + inv->spi.trans.status = SPITransDone; + else + inv->i2c.trans.status = I2CTransDone; + + /* Look at the results */ + switch(inv->status) { + case INVENSENSE3_IDLE: + /* Check the response of the WHO_AM_I */ + inv->status = INVENSENSE3_CONFIG; + if(inv->rx_buffer[1] == INV3_WHOAMI_ICM40605) { + inv->device = INVENSENSE3_ICM40605; + } else if(inv->rx_buffer[1] == INV3_WHOAMI_ICM40609) { + inv->device = INVENSENSE3_ICM40609; + } else if(inv->rx_buffer[1] == INV3_WHOAMI_ICM42605) { + inv->device = INVENSENSE3_ICM42605; + } else if(inv->rx_buffer[1] == INV3_WHOAMI_IIM42652) { + inv->device = INVENSENSE3_IIM42652; + } else if(inv->rx_buffer[1] == INV3_WHOAMI_ICM42688) { + inv->device = INVENSENSE3_ICM42688; + } else { + inv->status = INVENSENSE3_IDLE; + } + + /* Fix the configuration and set the scaling */ + if(inv->status == INVENSENSE3_CONFIG) + invensense3_fix_config(inv); + break; + + case INVENSENSE3_CONFIG: + /* Apply the next configuration register */ + if(invensense3_config(inv)) { + inv->status = INVENSENSE3_RUNNING; + } + break; + + case INVENSENSE3_RUNNING: + /* Select the desired parser */ + switch (inv->parser) { + case INVENSENSE3_PARSER_REGISTERS: + invensense3_parse_reg_data(inv, &inv->rx_buffer[1]); + break; + + case INVENSENSE3_PARSER_FIFO: { + /* Parse the results */ + uint16_t n_samples = (uint16_t)inv->rx_buffer[1] | inv->rx_buffer[2] << 8; + uint16_t fifo_bytes = n_samples * invensense3_fifo_sample_size[inv->sample_size]; + + inv->timer = n_samples; // samples left in FIFO + + // Not enough data in FIFO + if(n_samples == 0) { + return; + } + + // We read an incorrect length (try again) + if(fifo_bytes > 4096) { + invensense3_register_read(inv, INV3REG_FIFO_COUNTH, 2); + return; + } + + // Parse the data + if(*inv->rx_length > 3) { + uint16_t nb_packets_read = (*inv->rx_length - 3) / invensense3_fifo_sample_size[inv->sample_size]; + invensense3_parse_fifo_data(inv, &inv->rx_buffer[3], Min(n_samples, nb_packets_read)); + inv->timer -= nb_packets_read; + } + + break; + } + default: break; + } + break; + + default: + inv->status = INVENSENSE3_IDLE; + break; + } + } + + /* Failed transaction */ + if((inv->bus == INVENSENSE3_SPI && inv->spi.trans.status == SPITransFailed) || + (inv->bus == INVENSENSE3_I2C && inv->i2c.trans.status == I2CTransFailed)) { + + /* Set the transaction as done and update register bank if needed */ + if(inv->bus == INVENSENSE3_SPI) { + inv->spi.trans.status = SPITransDone; + } + else { + inv->i2c.trans.status = I2CTransDone; + } + + /* Retry or ignore */ + switch(inv->status) { + case INVENSENSE3_CONFIG: + /* If was not a bus switch decrease the index */ + if(inv->config_idx > 0 && inv->tx_buffer[0] != INV3REG_BANK_SEL) { + inv->config_idx--; + } + /* Try again */ + if(invensense3_config(inv)) { + inv->status = INVENSENSE3_RUNNING; + } + break; + case INVENSENSE3_IDLE: + case INVENSENSE3_RUNNING: + /* Ignore while idle/running */ + default: + break; + } + } +} + +/** + * @brief Parse the FIFO buffer data + * + * @param inv The invensense v3 instance + * @param data The FIFO buffer data to parse + * @param len The length of the FIFO buffer + */ +static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data, uint16_t samples) { + static struct Int32Vect3 accel[INVENSENSE3_FIFO_BUFFER_LEN] = {0}; + static struct Int32Rates gyro[INVENSENSE3_FIFO_BUFFER_LEN] = {0}; + + samples = Min(samples, INVENSENSE3_FIFO_BUFFER_LEN); + + uint8_t gyro_samplerate = 17 - inv->gyro_odr; + uint8_t accel_samplerate = 17 - inv->accel_odr; + + uint8_t faster_odr = gyro_samplerate; + if (accel_samplerate > gyro_samplerate) + faster_odr = accel_samplerate; + + // Go through the different samples + uint8_t i = 0; + uint8_t j = 0; + int32_t temp = 0; + uint16_t gyro_samplerate_count; + uint16_t accel_samplerate_count; + for(uint8_t sample = 0; sample < samples; sample++) { + + if ((data[0] & 0xFC) != 0x68) { + // no or bad data + } else { + + gyro_samplerate_count = gyro_samplerate * (sample + 1); + if(gyro_samplerate_count % faster_odr == 0) { + gyro[i].p = (int16_t)((uint16_t)data[7] | data[8] << 8 ); + gyro[i].q = (int16_t)((uint16_t)data[9] | data[10] << 8 ); + gyro[i].r = (int16_t)((uint16_t)data[11] | data[12] << 8 ); + + // Temperature sensor register data TEMP_DATA is updated with new data at max(Accelerometer ODR, Gyroscope ODR) + if(gyro_samplerate == faster_odr) + temp += data[13]; + + i++; + } + + accel_samplerate_count = accel_samplerate * (sample + 1); + if(accel_samplerate_count % faster_odr == 0) { + accel[j].x = (int16_t)((uint16_t)data[1] | data[2] << 8 ); + accel[j].y = (int16_t)((uint16_t)data[3] | data[4] << 8 ); + accel[j].z = (int16_t)((uint16_t)data[5] | data[6] << 8 ); + + if(accel_samplerate == faster_odr) + temp += data[13]; + + j++; + } + } + + data += invensense3_fifo_sample_size[inv->sample_size]; + } + + // ADC temp * temp sensitivity + temp zero + float temp_f = ((float)temp / i) / 2.07 + 25.f; + if (accel_samplerate == faster_odr) + temp_f = ((float)temp / j) / 2.07 + 25.f; + + // Send the scaled values over ABI + uint32_t now_ts = get_sys_time_usec(); + AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, i, inv->gyro_samplerate, temp_f); + AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j, inv->accel_samplerate, temp_f); + AbiSendMsgTEMPERATURE(inv->abi_id, temp_f); +} + +/** + * @brief Parse data from registers + * + * @param inv The invensense v3 instance + * @param data The data from all registers (DATA_TEMP, DATA_ACCEL and DATA_GYRO) + */ +static void invensense3_parse_reg_data(struct invensense3_t *inv, uint8_t *data) { + static struct Int32Vect3 accel[1] = {0}; + static struct Int32Rates gyro[1] = {0}; + + int32_t temp = (int16_t)((uint16_t)data[0] | data[1] << 8); + // ADC temp * temp sensitivity + temp zero + float temp_f = (float)temp / 132.48 + 25.f; + + accel[0].x = (int16_t)((uint16_t)data[2] | data[3] << 8 ); + accel[0].y = (int16_t)((uint16_t)data[4] | data[5] << 8 ); + accel[0].z = (int16_t)((uint16_t)data[6] | data[7] << 8 ); + + gyro[0].p = (int16_t)((uint16_t)data[8] | data[9] << 8 ); + gyro[0].q = (int16_t)((uint16_t)data[10]| data[11] << 8 ); + gyro[0].r = (int16_t)((uint16_t)data[12]| data[13] << 8 ); + + // Send the scaled values over ABI + uint32_t now_ts = get_sys_time_usec(); + AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, 1, inv->gyro_samplerate, temp_f); + AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, 1, inv->accel_samplerate, temp_f); + AbiSendMsgTEMPERATURE(inv->abi_id, temp_f); +} + +/** + * @brief This fixes the configuration errors and sets the correct scalings + * + * @param inv The invensense v3 instance + */ +static void invensense3_fix_config(struct invensense3_t *inv) { + /* Fix wrong configuration settings (prevent user error) */ + if(inv->gyro_odr > INVENSENSE3_GYRO_ODR_12_5HZ && inv->gyro_odr != INVENSENSE3_GYRO_ODR_500HZ) + inv->gyro_odr = INVENSENSE3_GYRO_ODR_12_5HZ; + + if(inv->device == INVENSENSE3_ICM40605 || inv->device == INVENSENSE3_ICM42605) { + if(inv->gyro_odr > INVENSENSE3_GYRO_ODR_25HZ && inv->gyro_odr != INVENSENSE3_GYRO_ODR_500HZ) + inv->gyro_odr = INVENSENSE3_GYRO_ODR_25HZ; + else if(inv->gyro_odr < INVENSENSE3_GYRO_ODR_8KHZ) + inv->gyro_odr = INVENSENSE3_GYRO_ODR_8KHZ; + + if(inv->device == INVENSENSE3_ICM40605 && inv->accel_odr > INVENSENSE3_ACCEL_ODR_25HZ && inv->accel_odr != INVENSENSE3_ACCEL_ODR_500HZ) + inv->accel_odr = INVENSENSE3_ACCEL_ODR_25HZ; + else if(inv->accel_odr < INVENSENSE3_ACCEL_ODR_8KHZ) + inv->accel_odr = INVENSENSE3_ACCEL_ODR_8KHZ; + } + + if(inv->device != INVENSENSE3_ICM40609 && inv->accel_range < INVENSENSE3_ACCEL_RANGE_16G) + inv->accel_range = INVENSENSE3_ACCEL_RANGE_16G; + else if(inv->device == INVENSENSE3_ICM40609 && inv->accel_range > INVENSENSE3_ACCEL_RANGE_4G) + inv->accel_range = INVENSENSE3_ACCEL_RANGE_4G; + + /* Fix the AAF bandwidth setting */ + const uint16_t (*aaf_table)[4]; + uint16_t aaf_len; + if(inv->device == INVENSENSE3_ICM40605 || inv->device == INVENSENSE3_ICM42605) { + aaf_len = sizeof(invensense3_aaf4x605) / sizeof(invensense3_aaf4x605[0]); + aaf_table = invensense3_aaf4x605; + } + else { + aaf_len = sizeof(invensense3_aaf) / sizeof(invensense3_aaf[0]); + aaf_table = invensense3_aaf; + } + + uint16_t i = 0; + for(i = 0; i < aaf_len; i++) { + if(inv->gyro_aaf <= aaf_table[i][0]) { + inv->gyro_aaf = aaf_table[i][0]; + RMAT_COPY(inv->gyro_aaf_regs, aaf_table[i]); + break; + } + } + if(i >= (aaf_len-1)) { + inv->gyro_aaf = aaf_table[aaf_len-1][0]; + RMAT_COPY(inv->gyro_aaf_regs, aaf_table[aaf_len-1]); + } + + for(i = 0; i < aaf_len; i++) { + if(inv->accel_aaf <= aaf_table[i][0]) { + inv->accel_aaf = aaf_table[i][0]; + RMAT_COPY(inv->accel_aaf_regs, aaf_table[i]); + break; + } + } + if(i >= (aaf_len-1)) { + inv->accel_aaf = aaf_table[aaf_len-1][0]; + RMAT_COPY(inv->accel_aaf_regs, aaf_table[aaf_len-1]); + } + + /* Calculate the samplerates in Hz */ + switch(inv->gyro_odr) { + case INVENSENSE3_GYRO_ODR_32KHZ: + inv->gyro_samplerate = 32000; + break; + case INVENSENSE3_GYRO_ODR_16KHZ: + inv->gyro_samplerate = 16000; + break; + case INVENSENSE3_GYRO_ODR_8KHZ: + inv->gyro_samplerate = 8000; + break; + case INVENSENSE3_GYRO_ODR_4KHZ: + inv->gyro_samplerate = 4000; + break; + case INVENSENSE3_GYRO_ODR_2KHZ: + inv->gyro_samplerate = 2000; + break; + case INVENSENSE3_GYRO_ODR_1KHZ: + inv->gyro_samplerate = 1000; + break; + case INVENSENSE3_GYRO_ODR_200HZ: + inv->gyro_samplerate = 200; + break; + case INVENSENSE3_GYRO_ODR_100HZ: + inv->gyro_samplerate = 100; + break; + case INVENSENSE3_GYRO_ODR_50HZ: + inv->gyro_samplerate = 50; + break; + case INVENSENSE3_GYRO_ODR_25HZ: + inv->gyro_samplerate = 25; + break; + case INVENSENSE3_GYRO_ODR_12_5HZ: + inv->gyro_samplerate = 12.5; + break; + case INVENSENSE3_GYRO_ODR_6_25HZ: + inv->gyro_samplerate = 6.25; + break; + case INVENSENSE3_GYRO_ODR_3_125HZ: + inv->gyro_samplerate = 3.125; + break; + case INVENSENSE3_GYRO_ODR_1_5625HZ: + inv->gyro_samplerate = 1.5625; + break; + case INVENSENSE3_GYRO_ODR_500HZ: + inv->gyro_samplerate = 500; + break; + } + switch(inv->accel_odr) { + case INVENSENSE3_ACCEL_ODR_32KHZ: + inv->accel_samplerate = 32000; + break; + case INVENSENSE3_ACCEL_ODR_16KHZ: + inv->accel_samplerate = 16000; + break; + case INVENSENSE3_ACCEL_ODR_8KHZ: + inv->accel_samplerate = 8000; + break; + case INVENSENSE3_ACCEL_ODR_4KHZ: + inv->accel_samplerate = 4000; + break; + case INVENSENSE3_ACCEL_ODR_2KHZ: + inv->accel_samplerate = 2000; + break; + case INVENSENSE3_ACCEL_ODR_1KHZ: + inv->accel_samplerate = 1000; + break; + case INVENSENSE3_ACCEL_ODR_200HZ: + inv->accel_samplerate = 200; + break; + case INVENSENSE3_ACCEL_ODR_100HZ: + inv->accel_samplerate = 100; + break; + case INVENSENSE3_ACCEL_ODR_50HZ: + inv->accel_samplerate = 50; + break; + case INVENSENSE3_ACCEL_ODR_25HZ: + inv->accel_samplerate = 25; + break; + case INVENSENSE3_ACCEL_ODR_12_5HZ: + inv->accel_samplerate = 12.5; + break; + case INVENSENSE3_ACCEL_ODR_6_25HZ: + inv->accel_samplerate = 6.25; + break; + case INVENSENSE3_ACCEL_ODR_3_125HZ: + inv->accel_samplerate = 3.125; + break; + case INVENSENSE3_ACCEL_ODR_1_5625HZ: + inv->accel_samplerate = 1.5625; + break; + case INVENSENSE3_ACCEL_ODR_500HZ: + inv->accel_samplerate = 500; + break; + } + + /* Set the default values */ + imu_set_defaults_gyro(inv->abi_id, NULL, NULL, invensense3_gyro_scale[inv->gyro_range]); + imu_set_defaults_accel(inv->abi_id, NULL, NULL, invensense3_accel_scale[inv->accel_range]); +} + +/** + * @brief Write a register based on a combined bank/regsiter value + * + * @param inv The invensense v3 instance + * @param bank_reg The bank is shifted 8 bits left, adn register is &0xFF + * @param value The value to write to the register + * @return true Whenever the register write was started + * @return false First we are busy switching the register bank + */ +static bool invensense3_register_write(struct invensense3_t *inv, uint16_t bank_reg, uint8_t value) { + /* Split the register and bank */ + uint8_t bank = bank_reg >> 8; + uint8_t reg = bank_reg & 0xFF; + + /* Switch the register bank if needed */ + if(invensense3_select_bank(inv, bank)) { + return false; + } + + inv->tx_buffer[0] = reg; + inv->tx_buffer[1] = value; + + /* SPI transaction */ + if(inv->bus == INVENSENSE3_SPI) { + inv->spi.trans.output_length = 2; + inv->spi.trans.input_length = 0; + spi_submit(inv->spi.p, &(inv->spi.trans)); + } + /* I2C transaction */ + else { + i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2); + } + + return true; +} + +/** + * @brief Read a register based on a combined bank/regsiter value + * + * @param inv The invensense v3 instance + * @param bank_reg The bank is shifted 8 bits left, adn register is &0xFF + * @param size The size to read (already 1 is added for the transmission of the register to read) + * @return true If we initiated the register read succesfully + * @return false First we are busy switching the register bank + */ +static bool invensense3_register_read(struct invensense3_t *inv, uint16_t bank_reg, uint16_t size) { + /* Split the register and bank */ + uint8_t bank = bank_reg >> 8; + uint8_t reg = bank_reg & 0xFF; + + /* Switch the register bank if needed */ + if(invensense3_select_bank(inv, bank)) { + return false; + } + + inv->tx_buffer[0] = reg | INV3_READ_FLAG; + /* SPI transaction */ + if(inv->bus == INVENSENSE3_SPI) { + inv->spi.trans.output_length = 2; + inv->spi.trans.input_length = 1 + size; + inv->tx_buffer[1] = 0; + spi_submit(inv->spi.p, &(inv->spi.trans)); + } + /* I2C transaction */ + else { + i2c_transceive(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 1, (1 + size)); + } + + return true; +} + +/** + * @brief Select the correct register bank + * + * @param inv The invensense v3 instance + * @param bank The bank ID to select + * @return true The bank change has been requested + * @return false The register bank is already correct + */ +static bool invensense3_select_bank(struct invensense3_t *inv, uint8_t bank) { + /* If we already selected the correct bank continue */ + if(inv->register_bank == bank) + return false; + + inv->tx_buffer[0] = INV3REG_BANK_SEL; + inv->tx_buffer[1] = bank; + /* SPI transaction */ + if(inv->bus == INVENSENSE3_SPI) { + inv->spi.trans.output_length = 2; + inv->spi.trans.input_length = 0; + spi_submit(inv->spi.p, &(inv->spi.trans)); + } + /* I2C transaction */ + else { + i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2); + } + + return true; +} + +/** + * @brief Reset FIFO (can be useful in some situations) + * + * @param inv The invensense v3 instance + * @return true When reset is done + */ +static bool invensense3_reset_fifo(struct invensense3_t *inv) { + if(invensense3_register_write(inv, INV3REG_SIGNAL_PATH_RESET, BIT_SIGNAL_PATH_RESET_FIFO_FLUSH)) + return true; + return false; +} + +/** + * @brief Configure the Invensense 3 device register by register + * + * @param inv The invensense v3 instance + * @return true When the configuration is completed + * @return false Still busy configuring + */ +static bool invensense3_config(struct invensense3_t *inv) { + switch(inv->config_idx) { + case 0: + /* Reset the device */ + if(invensense3_register_write(inv, INV3REG_DEVICE_CONFIG, BIT_DEVICE_CONFIG_SOFT_RESET_CONFIG)) + inv->config_idx++; + inv->timer = get_sys_time_usec(); + break; + + case 1: + /* Because reset takes time wait ~5ms */ + if((get_sys_time_usec() - inv->timer) < 5e3) + break; + + /* Start the accel en gyro in low noise mode */ + if(invensense3_register_write(inv, INV3REG_PWR_MGMT0, (ACCEL_MODE_LN << ACCEL_MODE_SHIFT) | (GYRO_MODE_LN << GYRO_MODE_SHIFT))) + inv->config_idx++; + inv->timer = get_sys_time_usec(); + break; + + case 2: + /* Because starting takes time wait ~1ms */ + if((get_sys_time_usec() - inv->timer) < 1e3) + break; + + /* Configure the gyro ODR/FS */ + if(invensense3_register_write(inv, INV3REG_GYRO_CONFIG0, (inv->gyro_range << GYRO_FS_SEL_SHIFT) | (inv->gyro_odr << GYRO_ODR_SHIFT))) + inv->config_idx++; + break; + + case 3: { + /* Configure the accel ODR/FS */ + uint8_t accel_config = (inv->accel_odr << ACCEL_ODR_SHIFT); + if(inv->device == INVENSENSE3_ICM40609) + accel_config |= inv->accel_range << ACCEL_FS_SEL_SHIFT; + else + accel_config |= (inv->accel_range - 1) << ACCEL_FS_SEL_SHIFT; + + if(invensense3_register_write(inv, INV3REG_ACCEL_CONFIG0, accel_config)) + inv->config_idx++; + break; + } + + case 4: + /* Configure gyro AAF enable */ + if(invensense3_register_write(inv, INV3REG_GYRO_CONFIG_STATIC2, 0x00)) + inv->config_idx++; + break; + case 5: + /* Configure gyro AAF DELT */ + if(invensense3_register_write(inv, INV3REG_GYRO_CONFIG_STATIC3, inv->gyro_aaf_regs[1])) + inv->config_idx++; + break; + case 6: + /* Configure gyro AAF DELTSQR lower */ + if(invensense3_register_write(inv, INV3REG_GYRO_CONFIG_STATIC4, (inv->gyro_aaf_regs[2]&0xFF))) + inv->config_idx++; + break; + case 7: + /* Configure gyro AAF DELTSQR upper and bitshift */ + if(invensense3_register_write(inv, INV3REG_GYRO_CONFIG_STATIC5, (inv->gyro_aaf_regs[3] << GYRO_AAF_BITSHIFT_SHIFT) | ((inv->gyro_aaf_regs[2]>>8) & 0x0F))) + inv->config_idx++; + break; + case 8: + /* Configure accel AAF enable and DELT */ + if(invensense3_register_write(inv, INV3REG_ACCEL_CONFIG_STATIC2, (inv->accel_aaf_regs[1] << ACCEL_AAF_DELT_SHIFT))) + inv->config_idx++; + break; + case 9: + /* Configure accel AAF DELTSQR lower */ + if(invensense3_register_write(inv, INV3REG_ACCEL_CONFIG_STATIC3, (inv->accel_aaf_regs[2]&0xFF))) + inv->config_idx++; + break; + case 10: + /* Configure accel AAF DELTSQR upper and bitshift */ + if(invensense3_register_write(inv, INV3REG_ACCEL_CONFIG_STATIC4, (inv->accel_aaf_regs[3] << ACCEL_AAF_BITSHIFT_SHIFT) | ((inv->accel_aaf_regs[2]>>8) & 0x0F))) + inv->config_idx++; + break; + + case 11: + /* FIFO count in records (little-endian data) */ + if(invensense3_register_write(inv, INV3REG_INTF_CONFIG0, FIFO_COUNT_REC)) + inv->config_idx++; + break; + case 12: + /* FIFO Stop-on-Full mode (enable the FIFO) */ + if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG , FIFO_CONFIG_MODE_STOP_ON_FULL << FIFO_CONFIG_MODE_SHIFT)) + inv->config_idx++; + break; + case 13: + /* FIFO content: enable accel, gyro, temperature */ + if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG1 , BIT_FIFO_CONFIG1_ACCEL_EN | + BIT_FIFO_CONFIG1_GYRO_EN | BIT_FIFO_CONFIG1_TEMP_EN)) + inv->config_idx++; + break; + case 14: + /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */ + if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG2, 20)) + inv->config_idx++; + break; + case 15: + /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */ + if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG3, 0x00)) + inv->config_idx++; + break; + case 16: + /* Enable interrupt pin/status */ + switch(inv->parser) { + case INVENSENSE3_PARSER_REGISTERS: + if(invensense3_register_write(inv, INV3REG_INT_SOURCE0, BIT_UI_DRDY_INT_EN)) + inv->config_idx++; + break; + case INVENSENSE3_PARSER_FIFO: + if(invensense3_register_write(inv, INV3REG_INT_SOURCE0, BIT_FIFO_FULL_INT_EN | BIT_FIFO_THS_INT_EN)) + inv->config_idx++; + break; + default: + break; + } + break; + case 17: + /* Interrupt pins ASYNC_RESET configuration */ + // Datasheet: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation" + if(invensense3_register_write(inv, INV3REG_INT_CONFIG1, BIT_INT_ASYNC_RESET)) + inv->config_idx++; + break; + case 18: + /* FIFO flush */ + if(invensense3_reset_fifo(inv)) + inv->config_idx++; + break; + + default: + inv->timer = 0; + return true; + } + return false; +} + +static int samples_from_odr(int odr) { + float freq = 0; + if(odr < INVENSENSE3_GYRO_ODR_200HZ) { + freq = 32000 / pow(2, odr-INVENSENSE3_GYRO_ODR_32KHZ); + } + else if(odr < INVENSENSE3_GYRO_ODR_500HZ) { + freq = 200 / pow(2, odr-INVENSENSE3_GYRO_ODR_200HZ); + } + else if(odr == INVENSENSE3_GYRO_ODR_500HZ) { + freq = 500; + } else { + // error + } + return ceilf(freq/PERIODIC_FREQUENCY); +} diff --git a/sw/airborne/peripherals/invensense3.h b/sw/airborne/peripherals/invensense3.h new file mode 100644 index 0000000000..05a1ba2a3f --- /dev/null +++ b/sw/airborne/peripherals/invensense3.h @@ -0,0 +1,192 @@ +/* + * Copyright (C) 2022 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/invensense3.h + * + * Driver for the Invensense V3 IMUs + * ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688 + */ + +#ifndef INVENSENSE3_H +#define INVENSENSE3_H + +#include "std.h" +#include "mcu_periph/spi.h" +#include "mcu_periph/i2c.h" + +/* This sensor has an option to request little-endian data */ +// Hold 22 measurements + 3 for the register address and length +#define INVENSENSE3_FIFO_BUFFER_LEN 22 +#define INVENSENSE3_BUFFER_SIZE INVENSENSE3_FIFO_BUFFER_LEN * 20 + 3 // 20 bytes is the maximum sample size + +/* Invensense v3 SPI peripheral */ +struct invensense3_spi_t { + struct spi_periph *p; ///< Peripheral device for communication + uint8_t slave_idx; ///< Slave index used for Slave Select + struct spi_transaction trans; ///< Transaction used during configuration and measurements + + uint8_t tx_buf[2]; ///< Transmit buffer + uint8_t rx_buf[INVENSENSE3_BUFFER_SIZE]; ///< Receive buffer +}; + +/* Invensense v3 I2C peripheral */ +struct invensense3_i2c_t { + struct i2c_periph *p; ///< Peripheral device for communication + uint8_t slave_addr; ///< The I2C slave address on the bus + struct i2c_transaction trans; ///< TRansaction used during configuration and measurements +}; + +/* Possible communication busses for the invense V3 driver */ +enum invensense3_bus_t { + INVENSENSE3_SPI, + INVENSENSE3_I2C +}; + +/* Different states the invensense v3 driver can be in */ +enum invensense3_status_t { + INVENSENSE3_IDLE, + INVENSENSE3_CONFIG, + INVENSENSE3_RUNNING +}; + +/* Different parsers of the invensense v3 driver */ +enum invensense3_parser_t { + INVENSENSE3_PARSER_REGISTERS, + INVENSENSE3_PARSER_FIFO +}; + +enum invensense3_fifo_packet_t { + INVENSENSE3_SAMPLE_SIZE_PK1, + INVENSENSE3_SAMPLE_SIZE_PK2, + INVENSENSE3_SAMPLE_SIZE_PK3, + INVENSENSE3_SAMPLE_SIZE_PK4 +}; + +/* Different device types compatible with the invensense v3 driver */ +enum invensense3_device_t { + INVENSENSE3_UNKOWN, + INVENSENSE3_ICM40605, + INVENSENSE3_ICM40609, + INVENSENSE3_ICM42605, + INVENSENSE3_IIM42652, + INVENSENSE3_ICM42688 +}; + +/* The gyro output data rate configuration */ +enum invensense3_gyro_odr_t { + INVENSENSE3_GYRO_ODR_32KHZ = 1, ///< Not possible for ICM40605 and ICM42605 + INVENSENSE3_GYRO_ODR_16KHZ, ///< Not possible for ICM40605 and ICM42605 + INVENSENSE3_GYRO_ODR_8KHZ, + INVENSENSE3_GYRO_ODR_4KHZ, + INVENSENSE3_GYRO_ODR_2KHZ, + INVENSENSE3_GYRO_ODR_1KHZ, + INVENSENSE3_GYRO_ODR_200HZ, + INVENSENSE3_GYRO_ODR_100HZ, + INVENSENSE3_GYRO_ODR_50HZ, + INVENSENSE3_GYRO_ODR_25HZ, + INVENSENSE3_GYRO_ODR_12_5HZ, + INVENSENSE3_GYRO_ODR_6_25HZ, + INVENSENSE3_GYRO_ODR_3_125HZ, + INVENSENSE3_GYRO_ODR_1_5625HZ, + INVENSENSE3_GYRO_ODR_500HZ +}; + +/* The gyro range in degrees per second(dps) */ +enum invensense3_gyro_range_t { + INVENSENSE3_GYRO_RANGE_2000DPS, + INVENSENSE3_GYRO_RANGE_1000DPS, + INVENSENSE3_GYRO_RANGE_500DPS, + INVENSENSE3_GYRO_RANGE_250DPS, + INVENSENSE3_GYRO_RANGE_125DPS, + INVENSENSE3_GYRO_RANGE_62_5DPS, + INVENSENSE3_GYRO_RANGE_31_25DPS, + INVENSENSE3_GYRO_RANGE_15_625DPS +}; + +/* The accelerometer output data rate configuration */ +enum invensense3_accel_odr_t { + INVENSENSE3_ACCEL_ODR_32KHZ = 1, ///< Not possible for ICM40605 and ICM42605 + INVENSENSE3_ACCEL_ODR_16KHZ, ///< Not possible for ICM40605 and ICM42605 + INVENSENSE3_ACCEL_ODR_8KHZ, + INVENSENSE3_ACCEL_ODR_4KHZ, + INVENSENSE3_ACCEL_ODR_2KHZ, + INVENSENSE3_ACCEL_ODR_1KHZ, + INVENSENSE3_ACCEL_ODR_200HZ, + INVENSENSE3_ACCEL_ODR_100HZ, + INVENSENSE3_ACCEL_ODR_50HZ, + INVENSENSE3_ACCEL_ODR_25HZ, + INVENSENSE3_ACCEL_ODR_12_5HZ, + INVENSENSE3_ACCEL_ODR_6_25HZ, + INVENSENSE3_ACCEL_ODR_3_125HZ, + INVENSENSE3_ACCEL_ODR_1_5625HZ, + INVENSENSE3_ACCEL_ODR_500HZ +}; + +/* The accelerometer range in G */ +enum invensense3_accel_range_t { + INVENSENSE3_ACCEL_RANGE_32G, ///< Only possible for ICM40609 + INVENSENSE3_ACCEL_RANGE_16G, + INVENSENSE3_ACCEL_RANGE_8G, + INVENSENSE3_ACCEL_RANGE_4G, + INVENSENSE3_ACCEL_RANGE_2G +}; + +/* Main invensense v3 device structure */ +struct invensense3_t { + uint8_t abi_id; ///< The ABI id used to broadcast the device measurements + enum invensense3_status_t status; ///< Status of the invensense v3 device + enum invensense3_device_t device; ///< The device type detected + enum invensense3_parser_t parser; ///< Parser of the device + + enum invensense3_bus_t bus; ///< The communication bus used to connect the device SPI/I2C + union { + struct invensense3_spi_t spi; ///< SPI specific configuration + struct invensense3_i2c_t i2c; ///< I2C specific configuration + }; + uint8_t* rx_buffer; + uint8_t* tx_buffer; + uint16_t* rx_length; + + uint8_t register_bank; ///< The current register bank communicating with + uint8_t config_idx; ///< The current configuration index + uint32_t timer; ///< Used to time operations during configuration (samples left during measuring) + + enum invensense3_gyro_odr_t gyro_odr; ///< Gyro Output Data Rate configuration + enum invensense3_gyro_range_t gyro_range; ///< Gyro range configuration + uint16_t gyro_aaf; ///< Gyro Anti-alias filter 3dB Bandwidth configuration [Hz] + uint16_t gyro_aaf_regs[4]; ///< Gyro Anti-alias filter register values + enum invensense3_accel_odr_t accel_odr; ///< Accelerometer Output Data Rate configuration + enum invensense3_accel_range_t accel_range; ///< Accelerometer range configuration + uint16_t accel_aaf; ///< Accelerometer Anti-alias filter 3dB Bandwidth configuration [Hz] + uint16_t accel_aaf_regs[4]; ///< Accelerometer Anti-alias filter register values + enum invensense3_fifo_packet_t sample_size; ///< FIFO packet size + int sample_numbers; ///< expected FIFO packet number, assuming reading at PERIODIC_FREQUENCY + float gyro_samplerate; ///< Sample rate in Hz from the gyro_odr + float accel_samplerate; ///< Sample rate in Hz from the accel_odr +}; + +/* External functions */ +void invensense3_init(struct invensense3_t *inv); +void invensense3_periodic(struct invensense3_t *inv); +void invensense3_event(struct invensense3_t *inv); + +#endif // INVENSENSE3_H diff --git a/sw/airborne/peripherals/invensense3_regs.h b/sw/airborne/peripherals/invensense3_regs.h new file mode 100644 index 0000000000..5238655910 --- /dev/null +++ b/sw/airborne/peripherals/invensense3_regs.h @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2022 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/invensense3_regs.h + * + * Register and address definitions for the Invensense V3 from ardupilot. + */ + +#ifndef INVENSENSE3_REGS_H +#define INVENSENSE3_REGS_H + +#define INV3_BANK0 0x00U +#define INV3_BANK1 0x01U +#define INV3_BANK2 0x02U +#define INV3_BANK3 0x03U + + +#define INV3REG(b, r) ((((uint16_t)b) << 8)|(r)) +#define INV3_READ_FLAG 0x80 + +//Register Map +#define INV3REG_DEVICE_CONFIG INV3REG(INV3_BANK0,0x11U) +# define BIT_DEVICE_CONFIG_SOFT_RESET_CONFIG 0x01 +# define BIT_DEVICE_CONFIG_SPI_MODE 0x10 +#define INV3REG_FIFO_CONFIG INV3REG(INV3_BANK0,0x16U) +# define FIFO_CONFIG_MODE_BYPASS 0x00 +# define FIFO_CONFIG_MODE_STREAM_TO_FIFO 0x01 +# define FIFO_CONFIG_MODE_STOP_ON_FULL 0x02 +# define FIFO_CONFIG_MODE_SHIFT 0x06 +#define INV3REG_TEMP_DATA1 INV3REG(INV3_BANK0,0x1DU) +#define INV3REG_ACCEL_DATA_X1 INV3REG(INV3_BANK0,0x1FU) +#define INV3REG_INT_STATUS INV3REG(INV3_BANK0,0x2DU) +#define INV3REG_FIFO_COUNTH INV3REG(INV3_BANK0,0x2EU) +#define INV3REG_FIFO_COUNTL INV3REG(INV3_BANK0,0x2FU) +#define INV3REG_FIFO_DATA INV3REG(INV3_BANK0,0x30U) +#define INV3REG_SIGNAL_PATH_RESET INV3REG(INV3_BANK0,0x4BU) +# define BIT_SIGNAL_PATH_RESET_FIFO_FLUSH 0x02 +# define BIT_SIGNAL_PATH_RESET_TMST_STROBE 0x04 +# define BIT_SIGNAL_PATH_RESET_ABORT_AND_RESET 0x08 +# define BIT_SIGNAL_PATH_RESET_DMP_MEM_RESET_EN 0x20 +# define BIT_SIGNAL_PATH_RESET_DMP_INIT_EN 0x40 +#define INV3REG_INTF_CONFIG0 INV3REG(INV3_BANK0,0x4CU) +# define UI_SIFS_CFG_SPI_DIS 0x02 +# define UI_SIFS_CFG_I2C_DIS 0x03 +# define UI_SIFS_CFG_SHIFT 0x00 +# define SENSOR_DATA_BIG_ENDIAN 0x10 +# define FIFO_COUNT_BIG_ENDIAN 0x20 +# define FIFO_COUNT_REC 0x40 +# define FIFO_HOLD_LAST_DATA_EN 0x80 +#define INV3REG_PWR_MGMT0 INV3REG(INV3_BANK0,0x4EU) +# define ACCEL_MODE_OFF 0x00 +# define ACCEL_MODE_LN 0x03 +# define ACCEL_MODE_SHIFT 0x00 +# define GYRO_MODE_OFF 0x00 +# define GYRO_MODE_LN 0x03 +# define GYRO_MODE_SHIFT 0x02 +# define BIT_PWR_MGMT_IDLE 0x08 +# define BIT_PWM_MGMT_TEMP_DIS 0x10 +#define INV3REG_GYRO_CONFIG0 INV3REG(INV3_BANK0,0x4FU) +# define GYRO_ODR_32KHZ 0x01 +# define GYRO_ODR_16KHZ 0x02 +# define GYRO_ODR_8KHZ 0x03 +# define GYRO_ODR_4KHZ 0x04 +# define GYRO_ODR_2KHZ 0x05 +# define GYRO_ODR_1KHZ 0x06 +# define GYRO_ODR_200HZ 0x07 +# define GYRO_ODR_100HZ 0x08 +# define GYRO_ODR_50HZ 0x09 +# define GYRO_ODR_25HZ 0x0A +# define GYRO_ODR_12_5HZ 0x0B +# define GYRO_ODR_500HZ 0x0F +# define GYRO_ODR_SHIFT 0x00 +# define GYRO_FS_SEL_2000DPS 0x00 +# define GYRO_FS_SEL_1000DPS 0x01 +# define GYRO_FS_SEL_500DPS 0x02 +# define GYRO_FS_SEL_250DPS 0x03 +# define GYRO_FS_SEL_125DPS 0x04 +# define GYRO_FS_SEL_62_5DPS 0x05 +# define GYRO_FS_SEL_31_25DPS 0x06 +# define GYRO_FS_SEL_15_625DPS 0x07 +# define GYRO_FS_SEL_SHIFT 0x05 +#define INV3REG_ACCEL_CONFIG0 INV3REG(INV3_BANK0,0x50U) +# define ACCEL_ODR_32KHZ 0x01 +# define ACCEL_ODR_16KHZ 0x02 +# define ACCEL_ODR_8KHZ 0x03 +# define ACCEL_ODR_4KHZ 0x04 +# define ACCEL_ODR_2KHZ 0x05 +# define ACCEL_ODR_1KHZ 0x06 +# define ACCEL_ODR_200HZ 0x07 +# define ACCEL_ODR_100HZ 0x08 +# define ACCEL_ODR_50HZ 0x09 +# define ACCEL_ODR_25HZ 0x0A +# define ACCEL_ODR_12_5HZ 0x0B +# define ACCEL_ODR_6_25HZ 0x0C +# define ACCEL_ODR_3_125HZ 0x0D +# define ACCEL_ODR_1_5625HZ 0x0E +# define ACCEL_ODR_500HZ 0x0F +# define ACCEL_ODR_SHIFT 0x00 +# define ACCEL_FS_SEL_16G 0x00 +# define ACCEL_FS_SEL_8G 0x01 +# define ACCEL_FS_SEL_4G 0x02 +# define ACCEL_FS_SEL_2G 0x03 +# define ACCEL_FS_SEL_SHIFT 0x05 +#define INV3REG_GYRO_CONFIG1 INV3REG(INV3_BANK0,0x51U) +#define INV3REG_GYRO_ACCEL_CONFIG0 INV3REG(INV3_BANK0,0x52U) +#define INV3REG_ACCEL_CONFIG1 INV3REG(INV3_BANK0,0x53U) +#define INV3REG_TMST_CONFIG INV3REG(INV3_BANK0,0x54U) +# define BIT_TMST_CONFIG_TMST_EN 0x01 +#define INV3REG_FIFO_CONFIG1 INV3REG(INV3_BANK0,0x5FU) +# define BIT_FIFO_CONFIG1_ACCEL_EN 0x01 +# define BIT_FIFO_CONFIG1_GYRO_EN 0x02 +# define BIT_FIFO_CONFIG1_TEMP_EN 0x04 +# define BIT_FIFO_CONFIG1_TMST_FSYNC_EN 0x08 +# define BIT_FIFO_CONFIG1_HIRES_EN 0x10 +# define BIT_FIFO_CONFIG1_WM_GT_TH 0x20 +# define BIT_FIFO_CONFIG1_RESUME_PARTIAL_RD 0x40 +#define INV3REG_FIFO_CONFIG2 INV3REG(INV3_BANK0,0x60U) +#define INV3REG_FIFO_CONFIG3 INV3REG(INV3_BANK0,0x61U) +#define INV3REG_INT_SOURCE0 INV3REG(INV3_BANK0,0x65U) +#define INV3REG_INT_SOURCE3 INV3REG(INV3_BANK0,0x68U) +# define BIT_FIFO_FULL_INT_EN 0x02 +# define BIT_FIFO_THS_INT_EN 0x04 +# define BIT_UI_DRDY_INT_EN 0x08 +#define INV3REG_INT_CONFIG1 INV3REG(INV3_BANK0,0x64U) +# define BIT_INT_ASYNC_RESET 0x10 +#define INV3REG_WHO_AM_I INV3REG(INV3_BANK0,0x75U) +#define INV3REG_GYRO_CONFIG_STATIC2 INV3REG(INV3_BANK1,0x0BU) +# define BIT_GYRO_NF_DIS 0x01 +# define BIT_GYRO_AAF_DIS 0x02 +#define INV3REG_GYRO_CONFIG_STATIC3 INV3REG(INV3_BANK1,0x0CU) +# define GYRO_AAF_DELT_SHIFT 0x00 +#define INV3REG_GYRO_CONFIG_STATIC4 INV3REG(INV3_BANK1,0x0DU) +# define GYRO_AAF_DELTSQR_LOW_SHIFT 0x00 //[0:7] +#define INV3REG_GYRO_CONFIG_STATIC5 INV3REG(INV3_BANK1,0x0EU) +# define GYRO_AAF_DELTSQR_HIGH_SHIFT 0x00 //[11:8] +# define GYRO_AAF_BITSHIFT_SHIFT 0x04 +#define INV3REG_GYRO_CONFIG_STATIC6 INV3REG(INV3_BANK1,0x0FU) +# define GYRO_X_NF_COSWZ_LOW_SHIFT 0x00 //[0:7] +#define INV3REG_GYRO_CONFIG_STATIC7 INV3REG(INV3_BANK1,0x10U) +# define GYRO_Y_NF_COSWZ_LOW_SHIFT 0x00 //[0:7] +#define INV3REG_GYRO_CONFIG_STATIC8 INV3REG(INV3_BANK1,0x11U) +# define GYRO_Z_NF_COSWZ_LOW_SHIFT 0x00 //[0:7] +#define INV3REG_GYRO_CONFIG_STATIC9 INV3REG(INV3_BANK1,0x12U) +# define GYRO_X_NF_COSWZ_HIGH_SHIFT 0x00 //[8] +# define GYRO_Y_NF_COSWZ_HIGH_SHIFT 0x01 //[8] +# define GYRO_Z_NF_COSWZ_HIGH_SHIFT 0x02 //[8] +# define GYRO_X_NF_COSWZ_SEL_SHIFT 0x03 //[0] +# define GYRO_Y_NF_COSWZ_SEL_SHIFT 0x04 //[0] +# define GYRO_Z_NF_COSWZ_SEL_SHIFT 0x05 //[0] +#define INV3REG_GYRO_CONFIG_STATIC10 INV3REG(INV3_BANK1,0x13U) +# define GYRO_NF_BW_SEL_SHIFT 0x04 +#define INV3REG_ACCEL_CONFIG_STATIC2 INV3REG(INV3_BANK2,0x03U) +# define ACCEL_AAF_DIS 0x01 +# define ACCEL_AAF_DELT_SHIFT 0x01 +#define INV3REG_ACCEL_CONFIG_STATIC3 INV3REG(INV3_BANK2,0x04U) +# define ACCEL_AAF_DELTSQR_LOW_SHIFT 0x00 //[0:7] +#define INV3REG_ACCEL_CONFIG_STATIC4 INV3REG(INV3_BANK2,0x05U) +# define ACCEL_AAF_DELTSQR_HIGH_SHIFT 0x00 //[11:8] +# define ACCEL_AAF_BITSHIFT_SHIFT 0x04 + +#define INV3REG_BANK_SEL 0x76 + +// WHOAMI values +#define INV3_WHOAMI_ICM40605 0x33 +#define INV3_WHOAMI_ICM40609 0x3b +#define INV3_WHOAMI_ICM42605 0x42 +#define INV3_WHOAMI_ICM42688 0x47 +#define INV3_WHOAMI_IIM42652 0x6f +#define INV3_WHOAMI_ICM42670 0x67 + +#endif /* INVENSENSE3_REGS_H */ diff --git a/sw/tools/px4/cube_orangeplus.prototype b/sw/tools/px4/cube_orangeplus.prototype new file mode 100644 index 0000000000..c53da9aace --- /dev/null +++ b/sw/tools/px4/cube_orangeplus.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1063, + "magic": "PX4FWv1", + "description": "Firmware for the CubePilot CubeOrange+ board", + "image": "", + "build_time": 0, + "summary": "CubeOrange+", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} \ No newline at end of file