dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fmu-v5: Add support for ICM-42688-P

This commit is contained in:
CUAVmengxiao 2022-03-24 18:12:37 +08:00 committed by Daniel Agar
parent 7cd57b4415
commit c0d5ae2f9f
6 changed files with 88 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/px4/fmu-v5/default.px4board
View File

@ -21,6 +21,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_IRLOCK=y
CONFIG_COMMON_LIGHT=y
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y

4
boards/px4/fmu-v5/init/rc.board_defaults
View File

@ -9,9 +9,9 @@ param set-default BAT2_V_DIV 18.1
param set-default BAT1_A_PER_V 36.367515152
param set-default BAT2_A_PER_V 36.367515152
if ver hwtypecmp V550 V560
if ver hwtypecmp V550 V552 V560 V562
then
# CUAV V5+ (V550) and V5nano (V560) have 3 IMUs
# CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs
# Multi-EKF (IMUs only)
param set-default EKF2_MULTI_IMU 3
param set-default SENS_IMU_MODE 0

10
boards/px4/fmu-v5/init/rc.board_sensors
View File

@ -5,8 +5,14 @@
board_adc start
# Internal SPI bus ICM-20602
icm20602 -s -R 2 -q start
if ver hwtypecmp V552 V562
then
# Internal SPI bus ICM-42688-P
icm42688p -s -R 2 -q start
else
# Internal SPI bus ICM-20602
icm20602 -s -R 2 -q start
fi
# Internal SPI bus ICM-20689
icm20689 -s -R 2 start

2
boards/px4/fmu-v5/src/board_config.h
View File

@ -191,6 +191,8 @@
#define HW_INFO_INIT {'V','5','x', 'x',0}
#define HW_INFO_INIT_VER 2
#define HW_INFO_INIT_REV 3
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
/* CAN Silence
*
* Silent mode control \ ESC Mux select

2
boards/px4/fmu-v5/src/manifest.c
View File

@ -160,6 +160,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
{0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
{0x0105, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5 R:5 V:1
{0x0500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:0 V:5
{0x0502, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:2 V:5 ICM42688P
{0x0602, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5nano R:2 V:6 ICM42688P
{0x0400, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // HolyBro mini no can 2,3
{0x0600, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // CUAV V5nano R:0 V:6 with can 2
};

94
boards/px4/fmu-v5/src/spi.cpp
View File

@ -35,29 +35,81 @@
#include <drivers/drv_sensor.h>
#include <nuttx/spi/spi.h>
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
}, {GPIO::PortE, GPIO::Pin3}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
initSPIHWVersion(HW_VER_REV(0, 0), {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortF, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
}, {GPIO::PortE, GPIO::Pin3}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
}),
initSPIBus(SPI::Bus::SPI4, {
initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
}),
initSPIBusExternal(SPI::Bus::SPI5, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
}),
}),
initSPIBus(SPI::Bus::SPI4, {
initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
initSPIHWVersion(HW_VER_REV(5, 2), {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortF, GPIO::Pin11}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
}, {GPIO::PortE, GPIO::Pin3}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
}),
initSPIBus(SPI::Bus::SPI4, {
initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
}),
initSPIBusExternal(SPI::Bus::SPI5, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
}),
}),
initSPIBusExternal(SPI::Bus::SPI5, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
initSPIHWVersion(HW_VER_REV(6, 2), {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortF, GPIO::Pin11}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}),
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}),
}, {GPIO::PortE, GPIO::Pin3}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5})
}),
initSPIBus(SPI::Bus::SPI4, {
initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}),
}),
initSPIBusExternal(SPI::Bus::SPI5, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin11})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})
}),
}),
};
static constexpr bool unused = validateSPIConfig(px4_spi_buses);
static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);