Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[imu/board] Add support for ICM45686 and LTS release of the Cube Orange+ (#3519)

Browse Source 
* Add invensense3_456 driver for ICM45686 IMU

* Add support for 9th SPI slave

* Add SPI_SLAVE9 with PG01 as Chip Select on Cube Orange

Used in the Cube Orange Plus LTS / "shiny" release for the non-isolated IMU (on SPI bus 1)

* Add cube_orangeplus_lts board with new ICM45686 IMUs
This commit is contained in:
lthoma5
2025-08-15 15:35:06 +02:00
committed by GitHub
parent e018eeed47
commit af2be6f092
12 changed files with 971 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/boards/cube_orangeplus_lts.makefile
+97
View File
@@ -0,0 +1,97 @@
# 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_lts
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
GPS_PORT ?= UART4
GPS_BAUD ?= B57600
# The GPS2 port
GPS2_PORT ?= UART8
GPS2_BAUD ?= B57600
# InterMCU port connected to the IO processor
INTERMCU_PORT ?= UART6
INTERMCU_BAUD ?= B1500000
# Overwrite default Cube IMU slave IDs for the triple ICM45686 configuration
CUBE_IMU1_SPI_SLAVE_IDX ?= SPI_SLAVE9
# IMU2 is correct @ SPI_SLAVE3
CUBE_IMU3_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
conf/modules/boards/cube_orangeplus_lts.xml
+19
View File
@@ -0,0 +1,19 @@
<!DOCTYPE module SYSTEM "../module.dtd">
<module name="cube_orange_plus_lts" dir="boards">
<doc>
<description>
Specific configuration for Cube Orange+ with triple ICM45686 IMU
</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"/>
<configure name="MS5611_SLAVE_IDX" value="SPI_SLAVE6"/>
</makefile>
</module>
conf/modules/imu_cube.xml
+13 -7
View File
@@ -20,26 +20,26 @@
<periodic fun="imu_cube_periodic()"/>
<event fun="imu_cube_event()"/>
<makefile target="!sim|nps|fbw">
<!-- ICM20948 (non-isolated) -->
<!-- ICM20948 (non-isolated) or ICM45686 (non-isolated, Cube+ LTS) -->
<configure name="CUBE_IMU1_SPI_DEV" default="spi1" case="lower|upper"/>
<configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE2"/>
<configure name="CUBE_IMU1_SPI_SLAVE_IDX" default="SPI_SLAVE2"/> <!-- Overwritten to SPI_SLAVE9 for the cube+ LTS -->
<define name="CUBE_IMU1_SPI_DEV" value="$(CUBE_IMU1_SPI_DEV_LOWER)"/>
<define name="USE_$(CUBE_IMU1_SPI_DEV_UPPER)"/>
<define name="CUBE_IMU1_SPI_SLAVE_IDX" value="$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU1_SPI_SLAVE_IDX)"/>
<!-- ICM20602 or ICM45686 (isolated) -->
<!-- ICM20602 or ICM45686 (isolated) or ICM45686 (isolated, Cube+ LTS) -->
<configure name="CUBE_IMU2_SPI_DEV" default="spi4" case="lower|upper"/>
<configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE3"/> <!-- Overwritten to SPI_SLAVE5 for cube+ -->
<configure name="CUBE_IMU2_SPI_SLAVE_IDX" default="SPI_SLAVE3"/> <!-- Overwritten to SPI_SLAVE5 for cube+ (non-LTS) -->
<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)"/>
<define name="USE_$(CUBE_IMU2_SPI_SLAVE_IDX)"/>
<!-- ICM20649 (isolated) -->
<!-- ICM20649 (isolated) or ICM45686 (isolated, Cube+ LTS) -->
<configure name="CUBE_IMU3_SPI_DEV" default="spi4" case="lower|upper"/>
<configure name="CUBE_IMU3_SPI_SLAVE_IDX" default="SPI_SLAVE8"/>
<define name="CUBE_IMU3_SPI_DEV" value="$(CUBE_IMU3_SPI_DEV_LOWER)"/>
<define name="CUBE_IMU3_SPI_DEV" value="$(CUBE_IMU3_SPI_DEV_LOWER)"/> <!-- Overwritten to SPI_SLAVE5 for the cube+ LTS -->
<define name="USE_$(CUBE_IMU3_SPI_DEV_UPPER)"/>
<define name="CUBE_IMU3_SPI_SLAVE_IDX" value="$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
<define name="USE_$(CUBE_IMU3_SPI_SLAVE_IDX)"/>
@@ -53,10 +53,16 @@
<!-- Different IMU for the cube+ -->
<define name="IMU_CUBE_ORANGEPLUS" value="TRUE" cond="ifeq ($(BOARD_VERSION), orange_plus)"/>
<file name="invensense2.c" dir="peripherals"/>
<!-- Triple ICM45686 for the Cube+ LTS / "shiny" cube" -->
<define name="IMU_CUBE_ORANGEPLUS_LTS" value="TRUE" cond="ifeq ($(BOARD_VERSION), orange_plus_lts)"/>
<file name="invensense2.c" dir="peripherals" cond="ifneq ($(BOARD_VERSION), orange_plus_lts)"/>
<file name="invensense3.c" dir="peripherals" cond="ifeq ($(BOARD_VERSION), orange_plus)"/>
<file name="invensense3_456.c" dir="peripherals" cond="ifeq ($(BOARD_VERSION), orange_plus_lts)"/>
<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>
sw/airborne/arch/chibios/common_board.h
+5
View File
@@ -753,6 +753,11 @@
#define SPI_SELECT_SLAVE8_PIN PAL_PAD(LINE_SPI_SLAVE8)
#endif
#if defined(LINE_SPI_SLAVE9)
#define SPI_SELECT_SLAVE9_PORT PAL_PORT(LINE_SPI_SLAVE9)
#define SPI_SELECT_SLAVE9_PIN PAL_PAD(LINE_SPI_SLAVE9)
#endif
/**
* SDIO
*/
sw/airborne/arch/chibios/mcu_periph/spi_arch.c
+25
View File
@@ -120,6 +120,11 @@ static inline ioportid_t spi_resolve_slave_port(uint8_t slave)
return SPI_SELECT_SLAVE8_PORT;
break;
#endif //USE_SPI_SLAVE8
#if USE_SPI_SLAVE9
case 9:
return SPI_SELECT_SLAVE9_PORT;
break;
#endif //USE_SPI_SLAVE9
default:
return 0;
break;
@@ -182,6 +187,11 @@ static inline uint16_t spi_resolve_slave_pin(uint8_t slave)
return SPI_SELECT_SLAVE8_PIN;
break;
#endif //USE_SPI_SLAVE8
#if USE_SPI_SLAVE9
case 9:
return SPI_SELECT_SLAVE9_PIN;
break;
#endif //USE_SPI_SLAVE9
default:
return 0;
break;
@@ -613,6 +623,11 @@ void spi_slave_select(uint8_t slave)
gpio_clear(SPI_SELECT_SLAVE8_PORT, SPI_SELECT_SLAVE8_PIN);
break;
#endif //USE_SPI_SLAVE8
#if USE_SPI_SLAVE9
case 9:
gpio_clear(SPI_SELECT_SLAVE9_PORT, SPI_SELECT_SLAVE9_PIN);
break;
#endif //USE_SPI_SLAVE9
default:
break;
}
@@ -670,6 +685,11 @@ void spi_slave_unselect(uint8_t slave)
gpio_set(SPI_SELECT_SLAVE8_PORT, SPI_SELECT_SLAVE8_PIN);
break;
#endif //USE_SPI_SLAVE8
#if USE_SPI_SLAVE9
case 9:
gpio_set(SPI_SELECT_SLAVE9_PORT, SPI_SELECT_SLAVE9_PIN);
break;
#endif //USE_SPI_SLAVE9
default:
break;
}
@@ -754,4 +774,9 @@ void spi_init_slaves(void)
gpio_setup_output(SPI_SELECT_SLAVE8_PORT, SPI_SELECT_SLAVE8_PIN);
spi_slave_unselect(8);
#endif
#if USE_SPI_SLAVE9
gpio_setup_output(SPI_SELECT_SLAVE9_PORT, SPI_SELECT_SLAVE9_PIN);
spi_slave_unselect(9);
#endif
}
sw/airborne/boards/cube/orange/board.cfg
+2
View File
@@ -180,6 +180,8 @@ PE13 SERVO2 PWM AF:TIM1_CH3 ()
PE14 SERVO1 PWM AF:TIM1_CH4 ()
PE15 VDD_5V_PERIPH_OC PASSIVE # INV
PG01 SPI_SLAVE9 OUTPUT PUSHPULL SPEED_HIGH FLOATING LEVEL_HIGH # ICM45686_CS
# GROUPS
GROUP ENERGY_SAVE_INPUTS %NAME=~/^SERVO[0-9]+|LED[0-9]+|SPI_SLAVE[0-9]+$/
GROUP ENERGY_SAVE_LOWS %NAME=~/^VDD_3V3_SENSORS_EN|VDD_5V_PERIPH_EN|PWM_VOLT_SEL|ALARM$/
sw/airborne/boards/cube/orange/board.h
+23 -13
View File
@@ -183,7 +183,7 @@
#define PF15 15U
#define PG00 0U
#define PG01 1U
#define PG01_SPI_SLAVE9 1U
#define PG02 2U
#define PG03 3U
#define PG04 4U
@@ -353,6 +353,8 @@
#define LINE_SERVO1 PAL_LINE(GPIOE, 14U)
#define LINE_VDD_5V_PERIPH_OC PAL_LINE(GPIOE, 15U)
#define LINE_SPI_SLAVE9 PAL_LINE(GPIOG, 1U)
#define LINE_OSC_IN PAL_LINE(GPIOH, 0U)
#define LINE_OSC_OUT PAL_LINE(GPIOH, 1U)
@@ -541,10 +543,10 @@
PIN_PUPDR_PULLDOWN(PB05_VDD_BRICK_VALID) | \
PIN_PUPDR_FLOATING(PB06_CAN2_TX) | \
PIN_PUPDR_PULLDOWN(PB07_VDD_BACKUP_VALID) | \
PIN_PUPDR_PULLUP(PB08_I2C1_SCL) | \
PIN_PUPDR_PULLUP(PB09_I2C1_SDA) | \
PIN_PUPDR_PULLUP(PB10_I2C2_SCL) | \
PIN_PUPDR_PULLUP(PB11_I2C2_SDA) | \
PIN_PUPDR_FLOATING(PB08_I2C1_SCL) | \
PIN_PUPDR_FLOATING(PB09_I2C1_SDA) | \
PIN_PUPDR_FLOATING(PB10_I2C2_SCL) | \
PIN_PUPDR_FLOATING(PB11_I2C2_SDA) | \
PIN_PUPDR_FLOATING(PB12_CAN2_RX) | \
PIN_PUPDR_FLOATING(PB13_SPI2_SCK) | \
PIN_PUPDR_FLOATING(PB14_SPI2_MISO) | \
@@ -998,7 +1000,7 @@
PIN_AFIO_AF(PF15, 0))
#define VAL_GPIOG_MODER (PIN_MODE_INPUT(PG00) | \
PIN_MODE_INPUT(PG01) | \
PIN_MODE_OUTPUT(PG01_SPI_SLAVE9) | \
PIN_MODE_INPUT(PG02) | \
PIN_MODE_INPUT(PG03) | \
PIN_MODE_INPUT(PG04) | \
@@ -1015,7 +1017,7 @@
PIN_MODE_INPUT(PG15))
#define VAL_GPIOG_OTYPER (PIN_OTYPE_PUSHPULL(PG00) | \
PIN_OTYPE_PUSHPULL(PG01) | \
PIN_OTYPE_PUSHPULL(PG01_SPI_SLAVE9) | \
PIN_OTYPE_PUSHPULL(PG02) | \
PIN_OTYPE_PUSHPULL(PG03) | \
PIN_OTYPE_PUSHPULL(PG04) | \
@@ -1032,7 +1034,7 @@
PIN_OTYPE_PUSHPULL(PG15))
#define VAL_GPIOG_OSPEEDR (PIN_OSPEED_SPEED_VERYLOW(PG00) | \
PIN_OSPEED_SPEED_VERYLOW(PG01) | \
PIN_OSPEED_SPEED_HIGH(PG01_SPI_SLAVE9) | \
PIN_OSPEED_SPEED_VERYLOW(PG02) | \
PIN_OSPEED_SPEED_VERYLOW(PG03) | \
PIN_OSPEED_SPEED_VERYLOW(PG04) | \
@@ -1049,7 +1051,7 @@
PIN_OSPEED_SPEED_VERYLOW(PG15))
#define VAL_GPIOG_PUPDR (PIN_PUPDR_PULLDOWN(PG00) | \
PIN_PUPDR_PULLDOWN(PG01) | \
PIN_PUPDR_FLOATING(PG01_SPI_SLAVE9) | \
PIN_PUPDR_PULLDOWN(PG02) | \
PIN_PUPDR_PULLDOWN(PG03) | \
PIN_PUPDR_PULLDOWN(PG04) | \
@@ -1066,7 +1068,7 @@
PIN_PUPDR_PULLDOWN(PG15))
#define VAL_GPIOG_ODR (PIN_ODR_LEVEL_LOW(PG00) | \
PIN_ODR_LEVEL_LOW(PG01) | \
PIN_ODR_LEVEL_HIGH(PG01_SPI_SLAVE9) | \
PIN_ODR_LEVEL_LOW(PG02) | \
PIN_ODR_LEVEL_LOW(PG03) | \
PIN_ODR_LEVEL_LOW(PG04) | \
@@ -1083,7 +1085,7 @@
PIN_ODR_LEVEL_LOW(PG15))
#define VAL_GPIOG_AFRL (PIN_AFIO_AF(PG00, 0) | \
PIN_AFIO_AF(PG01, 0) | \
PIN_AFIO_AF(PG01_SPI_SLAVE9, 0) | \
PIN_AFIO_AF(PG02, 0) | \
PIN_AFIO_AF(PG03, 0) | \
PIN_AFIO_AF(PG04, 0) | \
@@ -1691,8 +1693,16 @@
LINE_SERVO3, \
LINE_LED1, \
LINE_SERVO2, \
LINE_SERVO1
#define ENERGY_SAVE_INPUTS_SIZE 16
LINE_SERVO1, \
LINE_SPI_SLAVE9
#define ENERGY_SAVE_INPUTS_SIZE 17
#define ENERGY_SAVE_LOWS \
LINE_VDD_5V_PERIPH_EN, \
LINE_ALARM, \
LINE_PWM_VOLT_SEL, \
LINE_VDD_3V3_SENSORS_EN
#define ENERGY_SAVE_LOWS_SIZE 4
#if !defined(_FROM_ASM_)
#ifdef __cplusplus
sw/airborne/mcu_periph/spi.h
+1
View File
@@ -195,6 +195,7 @@ struct spi_periph {
#define SPI_SLAVE6 6
#define SPI_SLAVE7 7
#define SPI_SLAVE8 8
#define SPI_SLAVE9 9
/// @todo SPI error struct
//extern uint8_t spi_nb_ovrn;
sw/airborne/modules/imu/imu_cube.c
+67 -1
View File
@@ -32,20 +32,73 @@
#include "peripherals/invensense3.h"
#include "peripherals/mpu60x0_spi.h"
#if IMU_CUBE_ORANGEPLUS_LTS
#include "peripherals/invensense3_456.h"
#endif // IMU_CUBE_ORANGEPLUS_LTS
#if IMU_CUBE_ORANGEPLUS_LTS
static struct invensense3_456_t imu1;
static struct invensense3_456_t imu2;
static struct invensense3_456_t imu3;
#else
static struct invensense2_t imu1;
#if IMU_CUBE_ORANGEPLUS
static struct invensense3_t imu2;
#else
static struct Mpu60x0_Spi imu2;
#endif
#endif // IMU_CUBE_ORANGEPLUS
static struct invensense2_t imu3;
#endif // IMU_CUBE_ORANGEPLUS_LTS
void imu_cube_init(void)
{
struct Int32RMat rmat;
struct Int32Eulers eulers;
#if IMU_CUBE_ORANGEPLUS_LTS
/* IMU 1 (ICM45686, non-isolated) */
imu1.abi_id = IMU_CUBE1_ID;
imu1.spi.p = &CUBE_IMU1_SPI_DEV;
imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
imu1.imu_odr = INVENSENSE3_456_ODR_6_4KHZ;
invensense3_456_init(&imu1);
// Rotation (ROTATION_ROLL_180_YAW_135)
eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180));
eulers.theta = ANGLE_BFP_OF_REAL(0);
eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(135));
int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE1_ID, &rmat, NULL, NULL);
imu_set_defaults_accel(IMU_CUBE1_ID, &rmat, NULL, NULL);
/* IMU 2 (ICM45686, isolated) */
imu2.abi_id = IMU_CUBE2_ID;
imu2.spi.p = &CUBE_IMU2_SPI_DEV;
imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;
imu2.imu_odr = INVENSENSE3_456_ODR_6_4KHZ;
invensense3_456_init(&imu2);
// Rotation (GYRO_EXT_CS -> icm42688_ext2 -> ROTATION_PITCH_180_YAW_90)
eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(0));
eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
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);
/* IMU 3 (ICM45686, isolated) */
imu3.abi_id = IMU_CUBE3_ID;
imu3.spi.p = &CUBE_IMU3_SPI_DEV;
imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
imu3.imu_odr = INVENSENSE3_456_ODR_6_4KHZ;
invensense3_456_init(&imu3);
// Rotation (ROTATION_YAW_90)
eulers.phi = ANGLE_BFP_OF_REAL(0);
eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(0));
eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90));
int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
#else
/* IMU 1 (ICM20649 not isolated) */
imu1.abi_id = IMU_CUBE1_ID;
imu1.bus = INVENSENSE2_SPI;
@@ -126,10 +179,16 @@ void imu_cube_init(void)
int32_rmat_of_eulers(&rmat, &eulers);
imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
#endif // IMU_CUBE_ORANGEPLUS_LTS
}
void imu_cube_periodic(void)
{
#if IMU_CUBE_ORANGEPLUS_LTS
invensense3_456_periodic(&imu1);
invensense3_456_periodic(&imu2);
invensense3_456_periodic(&imu3);
#else
invensense2_periodic(&imu1);
#if IMU_CUBE_ORANGEPLUS
invensense3_periodic(&imu2);
@@ -137,10 +196,16 @@ void imu_cube_periodic(void)
mpu60x0_spi_periodic(&imu2);
#endif
invensense2_periodic(&imu3);
#endif // IMU_CUBE_ORANGEPLUS_LTS
}
void imu_cube_event(void)
{
#if IMU_CUBE_ORANGEPLUS_LTS
invensense3_456_event(&imu1);
invensense3_456_event(&imu2);
invensense3_456_event(&imu3);
#else
invensense2_event(&imu1);
#if IMU_CUBE_ORANGEPLUS
@@ -171,4 +236,5 @@ void imu_cube_event(void)
#endif
invensense2_event(&imu3);
#endif // IMU_CUBE_ORANGEPLUS_LTS
}
sw/airborne/peripherals/invensense3_456.c
+519
View File
File diff suppressed because it is too large Load Diff
sw/airborne/peripherals/invensense3_456.h
+123
View File
@@ -0,0 +1,123 @@
/*
* 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_456.h
*
* Driver for the Invensense 456XY IMUs:
* - ICM45686
*
* This driver is largely based on the original Invense v3 driver by Freek van Tienen,
* the Arupilot AP_InertialSensor_Invensensev3 with some inspiration from the px4 one
* as well. Split from the "main" invensensev3 driver as the WHO_AM_I register is different,
* and we operate this driver in high-resolution (20-bit) mode which is not used by any
* of the existing drivers. TL;DR: the drivers are quite different (even sensor ODR's),
* so we would be using if statements everywhere anyway. This seemed like the cleaner
* solution.
*/
#ifndef INVENSENSE3_456_H
#define INVENSENSE3_456_H
#include "std.h"
#include "mcu_periph/spi.h"
// Hold 22 measurements + 3 for the register address and length
#define INVENSENSE3_456_FIFO_BUFFER_LEN 22
#define INVENSENSE3_456_BUFFER_SIZE INVENSENSE3_456_FIFO_BUFFER_LEN * sizeof(struct FIFODataHighRes) + 3
// REF: https://github.com/ArduPilot/ardupilot/blob/85a8a55611a95d4c59052f79f56744a93a9c5a63/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp#L160C1-L167C3
struct __attribute__((packed)) FIFODataHighRes {
uint8_t header;
uint8_t accel[6];
uint8_t gyro[6];
int16_t temperature;
uint16_t timestamp;
uint8_t gx : 4, ax : 4, gy : 4, ay : 4, gz : 4, az : 4;
};
/* Invensense v3 SPI peripheral */
/* Modified to have a 4-bit tx buffer to deal with the different way of accessing register banks */
struct invensense3_456_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[4]; ///< Transmit buffer
uint8_t rx_buf[INVENSENSE3_456_BUFFER_SIZE]; ///< Receive buffer
};
/* Different states the invensense v3 driver can be in */
enum invensense3_456_status_t {
INVENSENSE3_456_IDLE,
INVENSENSE3_456_CONFIG,
INVENSENSE3_456_RUNNING
};
/* Different device types compatible with the invensense v3 - 456xy driver */
enum invensense3_456_device_t {
INVENSENSE3_456_UNKOWN,
INVENSENSE3_456_ICM45686
};
/* Output data rate configuration - for this initial driver version,
we operate the accelerometer/gyro at the same ODR */
enum invensense3_456_odr_t {
INVENSENSE3_456_ODR_6_4KHZ = 0b0011, // LN only
INVENSENSE3_456_ODR_3_2KHZ = 0b0100, // LN only
INVENSENSE3_456_ODR_1_6KHZ = 0b0101, // LN only
INVENSENSE3_456_ODR_800HZ = 0b0110, // LN only
INVENSENSE3_456_ODR_400HZ = 0b0111, // LP / LN
INVENSENSE3_456_ODR_200HZ = 0b1000, // LP / LN
INVENSENSE3_456_ODR_100HZ = 0b1001, // LP / LN
INVENSENSE3_456_ODR_50HZ = 0b1010, // LP / LN
INVENSENSE3_456_ODR_25HZ = 0b1011, // LP / LN
// INVENSENSE3_456_ODR_12_5HZ = 0b1100, // LP / LN - NOT USED
// INVENSENSE3_456_ODR_6_25HZ = 0b1101, // LP only - NOT USED
// INVENSENSE3_456_ODR_3_125HZ = 0b1110, // LP only - NOT USED
// INVENSENSE3_456_ODR_1_5625HZ = 0b1111, // LP only - NOT USED
};
/* Main invensense 456 device structure */
struct invensense3_456_t {
uint8_t abi_id; ///< The ABI id used to broadcast the device measurements
enum invensense3_456_status_t status; ///< Status of the invensense device
enum invensense3_456_device_t device; ///< The device type detected
struct invensense3_456_spi_t spi; ///< SPI specific configuration (support SPI only for now)
uint8_t* rx_buffer;
uint8_t* tx_buffer;
uint16_t* rx_length;
uint8_t config_idx; ///< The current configuration index (used for setup/initial configuration)
uint32_t timer; ///< Used to time operations during configuration (samples left during measuring)
enum invensense3_456_odr_t imu_odr; ///< Accelerometer / gyro Output Data Rate configuration
int sample_numbers; ///< expected FIFO packet number, assuming reading at PERIODIC_FREQUENCY
float imu_samplerate; ///< Sample rate in Hz from the imu_odr
};
/* External functions */
void invensense3_456_init(struct invensense3_456_t *inv);
void invensense3_456_periodic(struct invensense3_456_t *inv);
void invensense3_456_event(struct invensense3_456_t *inv);
#endif // INVENSENSE3_456_H
sw/airborne/peripherals/invensense3_456_regs.h
+77
View File
@@ -0,0 +1,77 @@
/*
* 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_456_regs.h
*
* Register and address definitions for the Invensense V3 ICM456xy sensors from ardupilot.
*/
#ifndef INVENSENSE3_456_REGS_H
#define INVENSENSE3_456_REGS_H
#define INV3_456_READ_FLAG 0x80
#define ACCEL_ODR_SHIFT 0x00
#define GYRO_ODR_SHIFT 0x00
#define GYRO_FS_SEL_SHIFT 0x04
#define ACCEL_FS_SEL_SHIFT 0x04
// WHOAMI values
#define INV3_456_WHOAMI_ICM45686 0xE9 // REF: https://invensense.tdk.com/wp-content/uploads/documentation/DS-000577_ICM-45686.pdf
#define INV3REG_456_WHOAMI 0x72
#define INV3REG_456_PWR_MGMT0 0x10
#define INV3REG_456_INT1_STATUS0 0x19
#define INV3REG_456_ACCEL_CONFIG0 0x1B
#define INV3REG_456_GYRO_CONFIG0 0x1C
#define INV3REG_456_FIFO_CONFIG0 0x1D
#define INV3REG_456_FIFO_CONFIG2 0x20
#define INV3REG_456_FIFO_CONFIG3 0x21
#define INV3REG_456_FIFO_CONFIG4 0x22
#define INV3REG_456_RTC_CONFIG 0x26
#define INV3REG_456_FIFO_COUNTH 0x12
#define INV3REG_456_FIFO_COUNTL 0x13
#define INV3REG_456_FIFO_DATA 0x14
#define INV3REG_456_INTF_CONFIG0 0x2C
#define INV3REG_456_IOC_PAD_SCENARIO 0x2F
#define INV3REG_456_IOC_PAD_SCENARIO_AUX_OVRD 0x30
#define INV3REG_456_IOC_PAD_SCENARIO_OVRD 0x31
#define INV3REG_456_PWR_MGMT_AUX1 0x54
#define INV3REG_456_IREG_ADDRH 0x7C
#define INV3REG_456_IREG_ADDRL 0x7D
#define INV3REG_456_IREG_DATA 0x7E
#define INV3REG_456_REG_MISC2 0x7F
#define INV3REG_456_SREG_CTRL 0x63
#define INV3BANK_456_IMEM_SRAM_ADDR 0x0000
#define INV3BANK_456_IPREG_BAR_ADDR 0xA000
#define INV3BANK_456_IPREG_TOP1_ADDR 0xA200
#define INV3BANK_456_IPREG_SYS1_ADDR 0xA400
#define INV3BANK_456_IPREG_SYS2_ADDR 0xA500
#define INV_456_BASE_FIFO3_CONFIG_VALUE (1U<<3 | 1U<<2 | 1U<<1) // FIFO_HIRES_EN | FIFO_GYRO_EN | FIFO_ACCEL_EN
#define INV_456_FIFO_IF_EN (1U<<0) // INV_456_FIFO_IF_EN
#endif /* INVENSENSE3_456_REGS_H */