Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 10:41:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[boards] Add support for the CubePilot - Cube Orange+ (#3017)

Browse Source 
* [imu] Add Invensense V3 driver

* [imu] Add samplerate to raw ABI message

* [boards] Add support for the CubePilot - Cube Orange+
This commit is contained in:
Freek van Tienen
2023-09-20 11:16:01 +02:00
committed by GitHub
parent 0aeb8908c3
commit 528f5d5b10
52 changed files with 1900 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
conf/abi.xml
View File

@@ -212,17 +212,19 @@
</message>
<message name="IMU_GYRO_RAW" id="31">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="gyro" type="struct Int32Rates *"/>
<field name="samples" type="uint8_t"/>
<field name="temp" type="float" unit="deg Celcius"/>
<field name="stamp" type="uint32_t" unit="us">Registered timestamp of the measurement</field>
<field name="gyro" type="struct Int32Rates *">Raw gyro samples measured</field>
<field name="samples" type="uint8_t">Amount of samples</field>
<field name="rate" type="float" unit="Hz">Sampling rate in Hz of the samples</field>
<field name="temp" type="float" unit="deg Celcius">Temperature of the sensor</field>
</message>
<message name="IMU_ACCEL_RAW" id="32">
<field name="stamp" type="uint32_t" unit="us"/>
<field name="accel" type="struct Int32Vect3 *"/>
<field name="samples" type="uint8_t"/>
<field name="temp" type="float" unit="deg Celcius"/>
<field name="stamp" type="uint32_t" unit="us">Registered timestamp of the measurement</field>
<field name="accel" type="struct Int32Vect3 *">Raw accelration samples measured</field>
<field name="samples" type="uint8_t">Amount of samples</field>
<field name="rate" type="float" unit="Hz">Sampling rate in Hz of the samples</field>
<field name="temp" type="float" unit="deg Celcius">Temperature of the sensor</field>
</message>
<message name="IMU_MAG_RAW" id="33">

11
conf/airframes/ENAC/conf_enac.xml
View File

@@ -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"
/>
<aircraft
name="matek"
ac_id="9"
airframe="airframes/examples/matek_h743_slim.xml"
radio="radios/FrSky_X-Lite.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml"
gui_color="red"
/>
</conf>

3
conf/airframes/examples/cube_orange.xml
View File

@@ -14,7 +14,7 @@
<description>Neddrone5</description>
<firmware name="rotorcraft">
<target name="ap" board="cube_orange">
<target name="ap" board="cube_orangeplus">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<!--configure name="RTOS_DEBUG" value="1"/-->
@@ -73,6 +73,7 @@
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
<configure name="MODEM_PORT" value="usb_serial"/>
</module>
<module name="actuators" type="uavcan">

8
conf/airframes/examples/matek_h743_slim.xml
View File

@@ -28,9 +28,11 @@
<module name="telemetry" type="transparent">
<configure name="MODEM_PORT" value="usb_serial"/>
</module>
<module name="imu" type="matek_h743_slim">
<define name="IMU_MPU_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_1000"/>
<define name="IMU_MPU_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_8G"/>
<module name="imu" type="icm42688">
<configure name="IMU_SPI_DEV" value="spi4"/>
<configure name="IMU_SPI_SLAVE_IDX" value="SPI_SLAVE6"/>
<!--define name="IMU_MPU_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_1000"/>
<define name="IMU_MPU_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_8G"/-->
</module>
<module name="gps" type="optitrack"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>

2
conf/boards/cube_orange.makefile
View File

@@ -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

91
conf/boards/cube_orangeplus.makefile Normal file
View File

@@ -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. <configure name="ACTUATORS" value="actuators_ppm/>
# and by setting the correct "driver" attribute in servo section
# e.g. <servo driver="Ppm">
#
ACTUATORS ?= actuators_pwm

18
conf/modules/boards/cube_orangeplus.xml Normal file
View File

@@ -0,0 +1,18 @@
<!DOCTYPE module SYSTEM "../module.dtd">
<module name="cube_orange" dir="boards">
<doc>
<description>
Specific configuration for Cube Orange with ChibiOS
</description>
</doc>
<dep>
<depends>spi_master,baro_ms5611_spi</depends>
</dep>
<makefile target="!sim|nps|fbw">
<define name="USE_RTC_BACKUP" value="TRUE"/>
<configure name="SDLOG_USE_RTC" value="FALSE"/>
<configure name="MS5611_SPI_DEV" value="spi1"/> <!-- spi4 -->
<configure name="MS5611_SLAVE_IDX" value="SPI_SLAVE6"/> <!-- SPI_SLAVE4 -->
</makefile>
</module>

1
conf/modules/imu_apogee.xml
View File

@@ -33,6 +33,7 @@
<define name="DOWNLINK_TRANSPORT" value="pprz_tp"/>
<define name="DOWNLINK_DEVICE" value="uart0"/>
<define name="USE_UART0"/>
<define name="IMU_APOGEE_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_bebop.xml
View File

@@ -43,6 +43,7 @@
<define name="BEBOP_MPU_I2C_DEV" value="i2c2"/>
<define name="USE_I2C2"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_BEBOP_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

29
conf/modules/imu_cube.xml
View File

@@ -28,9 +28,9 @@
<define name="CUBE_IMU1_SPI_SLAVE_IDX" value="$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
<!-- ICM20602 (isolated) -->
<!-- ICM20602 or ICM45686 (isolated) -->
<configure name="CUBE_IMU2_SPI_DEV" default="spi4" case="lower|upper"/>
<configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE3"/>
<configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE3"/> <!-- Overwritten to SPI_SLAVE5 for cube+ -->
<define name="CUBE_IMU2_SPI_DEV" value="$(CUBE_IMU2_SPI_DEV_LOWER)"/>
<define name="USE_$(CUBE_IMU2_SPI_DEV_UPPER)"/>
<define name="CUBE_IMU2_SPI_SLAVE_IDX" value="$(CUBE_IMU2_SPI_SLAVE_IDX)"/>
@@ -50,9 +50,30 @@
<define name="IMU_HEATER_P_GAIN" value="50.0"/>
<define name="IMU_HEATER_I_GAIN" value="0.07"/>
<!-- Different IMU for the cube+ -->
<define name="IMU_CUBE_ORANGEPLUS" value="TRUE" cond="ifeq ($(BOARD_VERSION), orange_plus)"/>
<file name="invensense2.c" dir="peripherals"/>
<file name="mpu60x0.c" dir="peripherals"/>
<file name="mpu60x0_spi.c" dir="peripherals"/>
<file name="invensense3.c" dir="peripherals" cond="ifeq ($(BOARD_VERSION), orange_plus)"/>
<file name="mpu60x0.c" dir="peripherals" cond="ifeq ($(BOARD_VERSION), orange)"/>
<file name="mpu60x0_spi.c" dir="peripherals" cond="ifeq ($(BOARD_VERSION), orange)"/>
<file name="imu_cube.c"/>
<test>
<define name="SPI_MASTER"/>
<define name="CUBE_IMU1_SPI_DEV" value="spi1"/>
<define name="CUBE_IMU1_SPI_SLAVE_IDX" value="0"/>
<define name="CUBE_IMU2_SPI_DEV" value="spi4"/>
<define name="CUBE_IMU2_SPI_SLAVE_IDX" value="1"/>
<define name="CUBE_IMU3_SPI_DEV" value="spi4"/>
<define name="CUBE_IMU3_SPI_SLAVE_IDX" value="2"/>
<define name="USE_SPI1"/>
<define name="USE_SPI4"/>
<define name="USE_SPI_SLAVE0"/>
<define name="USE_SPI_SLAVE2"/>
<define name="USE_SPI_SLAVE2"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_CUBE_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_disco.xml
View File

@@ -38,6 +38,7 @@
<define name="USE_I2C2"/>
<define name="USE_I2C1"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_DISCO_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

52
conf/modules/imu_icm42688.xml Normal file
View File

@@ -0,0 +1,52 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_icm42688" dir="imu" task="sensors">
<doc>
<description>
Driver module for the Invensense v3 IMU ICM42688.
</description>
<configure name="IMU_SPI_DEV" value="spi1" description="SPI device to use for ICM42688"/>
<configure name="IMU_SPI_SLAVE_IDX" value="SPI_SLAVE0" description="Slave index of the IMU AP_CS pin (12)"/>
<configure name="IMU_ODR" default="4KHZ" description="Output data rate of ACCEL and GYRO (invensense3.h:95/126)"/>
<configure name="GYRO_RANGE" default="500DPS" description="Gyro range (invensense3.h:114)"/>
<configure name="ACCEL_RANGE" default="2G" description="Accel range (invensense3.h:145)"/>
<define name="INVENSENSE3_GYRO_AAF" value="977" description="Anti-aliasing filter"/>
<define name="INVENSENSE3_ACCEL_AAF" value="213" description="Anti-aliasing filter"/>
</doc>
<dep>
<depends>i2c,spi_master,imu_common</depends>
<provides>imu</provides>
</dep>
<header>
<file name="imu_icm42688.h"/>
</header>
<init fun="imu_icm42688_init()"/>
<periodic fun="imu_icm42688_periodic()" freq="1000"/>
<event fun="imu_icm42688_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="IMU_SPI_DEV" default="spi1" case="lower|upper"/>
<configure name="IMU_SPI_SLAVE_IDX" default="SPI_SLAVE0"/>
<configure name="IMU_ODR" default="4KHZ"/>
<configure name="GYRO_RANGE" default="500DPS"/>
<configure name="ACCEL_RANGE" default="2G"/>
<define name="IMU_SPI_DEV" value="$(IMU_SPI_DEV_LOWER)"/>
<define name="USE_$(IMU_SPI_DEV_UPPER)"/>
<define name="IMU_SPI_SLAVE_IDX" value="$(IMU_SPI_SLAVE_IDX)"/>
<define name="USE_$(IMU_SPI_SLAVE_IDX)"/>
<define name="INVENSENSE3_GYRO_ODR" value="INVENSENSE3_GYRO_ODR_$(IMU_ODR)"/>
<define name="INVENSENSE3_ACCEL_ODR" value="INVENSENSE3_ACCEL_ODR_$(IMU_ODR)"/>
<define name="INVENSENSE3_GYRO_RANGE" value="INVENSENSE3_GYRO_RANGE_$(GYRO_RANGE)"/>
<define name="INVENSENSE3_ACCEL_RANGE" value="INVENSENSE3_ACCEL_RANGE_$(ACCEL_RANGE)"/>
<define name="INVENSENSE3_GYRO_AAF" value="977"/>
<define name="INVENSENSE3_ACCEL_AAF" value="213"/>
<file name="invensense3.c" dir="peripherals"/>
<file name="imu_icm42688.c"/>
</makefile>
</module>

4
conf/modules/imu_matek_h743_slim.xml
View File

@@ -7,7 +7,9 @@
MPU6000 IMU via SPI.
</description>
</doc>
<autoload name="imu_mpu6000"/>
<dep>
<depends>imu_mpu6000</depends>
</dep>
<makefile target="!sim|nps|fbw">
<configure name="IMU_MPU_SPI_DEV" value="spi1" case="upper|lower"/>
<configure name="IMU_MPU_SPI_SLAVE_IDX" value="SPI_SLAVE5"/>

1
conf/modules/imu_mpu6000.xml
View File

@@ -42,6 +42,7 @@
<define name="IMU_MPU_SPI_SLAVE_IDX" value="0"/>
<define name="USE_SPI_SLAVE0"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_MPU_SPI_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_mpu6000_hmc5883.xml
View File

@@ -67,6 +67,7 @@
<define name="IMU_HMC_I2C_DEV" value="i2c2"/>
<define name="USE_I2C2"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_MPU_HMC_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_mpu9250_i2c.xml
View File

@@ -42,6 +42,7 @@
<test>
<define name="IMU_MPU9250_I2C_DEV" value="i2c1"/>
<define name="USE_I2C1"/>
<define name="IMU_MPU9250_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_mpu9250_spi.xml
View File

@@ -51,6 +51,7 @@
<define name="USE_SPI1"/>
<define name="IMU_MPU9250_SPI_SLAVE_IDX" value="0"/>
<define name="USE_SPI_SLAVE0"/>
<define name="IMU_MPU9250_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_px4fmu_v1.7.xml
View File

@@ -39,6 +39,7 @@
<define name="USE_SPI_SLAVE2"/>
<define name="USE_I2C2"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_PX4FMU_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

1
conf/modules/imu_px4fmu_v2.4.xml
View File

@@ -49,6 +49,7 @@
<define name="USE_SPI_SLAVE1"/>
<define name="USE_I2C2"/>
<define name="PERIODIC_FREQUENCY" value="512"/>
<define name="IMU_PX4_PERIODIC_FREQ" value="512"/>
</test>
</makefile>
</module>

5
sw/airborne/boards/apogee/imu_apogee.c
View File

@@ -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;
}

4
sw/airborne/boards/ardrone/navdata.c
View File

@@ -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,

4
sw/airborne/modules/ahrs/ahrs_vectornav.c
View File

@@ -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);
}
}

4
sw/airborne/modules/core/abi_sender_ids.h
View File

@@ -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

36
sw/airborne/modules/imu/imu.c
View File

@@ -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

5
sw/airborne/modules/imu/imu_aspirin.c
View File

@@ -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;
}

5
sw/airborne/modules/imu/imu_aspirin_2_spi.c
View File

@@ -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

5
sw/airborne/modules/imu/imu_aspirin_i2c.c
View File

@@ -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;
}

5
sw/airborne/modules/imu/imu_bebop.c
View File

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

4
sw/airborne/modules/imu/imu_bmi088_i2c.c
View File

@@ -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);
}
}

44
sw/airborne/modules/imu/imu_cube.c
View File

@@ -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);
}

5
sw/airborne/modules/imu/imu_disco.c
View File

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

4
sw/airborne/modules/imu/imu_heater.c
View File

@@ -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;

77
sw/airborne/modules/imu/imu_icm42688.c Normal file
View File

@@ -0,0 +1,77 @@
/*
* Copyright (C) 2022 Freek van tieen <freek.v.tienen@gmail.com>
*
* 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);
}

36
sw/airborne/modules/imu/imu_icm42688.h Normal file
View File

@@ -0,0 +1,36 @@
/*
* Copyright (C) 2022 Jesús Bautista Villar <jesbauti20@gmail.com>
*
* 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 */

5
sw/airborne/modules/imu/imu_mpu6000.c
View File

@@ -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);
}
}

5
sw/airborne/modules/imu/imu_mpu6000_hmc5883.c
View File

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

5
sw/airborne/modules/imu/imu_mpu60x0_i2c.c
View File

@@ -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;
}
}

5
sw/airborne/modules/imu/imu_mpu9250_i2c.c
View File

@@ -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

5
sw/airborne/modules/imu/imu_mpu9250_spi.c
View File

@@ -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);
}
}

4
sw/airborne/modules/imu/imu_nps.c
View File

@@ -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) {

5
sw/airborne/modules/imu/imu_px4fmu.c
View File

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

5
sw/airborne/modules/imu/imu_px4fmu_v2.4.c
View File

@@ -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

4
sw/airborne/modules/imu/imu_vectornav.c
View File

@@ -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);
}

8
sw/airborne/modules/ins/imu_xsens.c
View File

@@ -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) {

48
sw/airborne/peripherals/bmi088.c
View File

@@ -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:

3
sw/airborne/peripherals/bmi088.h
View File

@@ -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);

6
sw/airborne/peripherals/invensense2.c
View File

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

2
sw/airborne/peripherals/invensense2.h
View File

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

990
sw/airborne/peripherals/invensense3.c Normal file
View File

File diff suppressed because it is too large Load Diff

192
sw/airborne/peripherals/invensense3.h Normal file
View File

@@ -0,0 +1,192 @@
/*
* Copyright (C) 2022 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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

Some files were not shown because too many files have changed in this diff Show More