Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 3 commits into from
Mar 29, 2022
Merged

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fda5e9a
fmu-v5: Add support for ICM-42688-P
mxiaogit Mar 24, 2022
23ce05a
spi: get the correct version revision
mxiaogit Mar 24, 2022
02bdd53
fmu-v5: add macro definitions for different version revisions
mxiaogit Mar 29, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions boards/px4/fmu-v5/default.px4board
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions boards/px4/fmu-v5/init/rc.board_defaults
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
10 changes: 8 additions & 2 deletions boards/px4/fmu-v5/init/rc.board_sensors
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
9 changes: 9 additions & 0 deletions boards/px4/fmu-v5/src/board_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,15 @@
#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
#define V500 HW_VER_REV(0x0,0x0) // FMUV5, Rev 0
#define V515 HW_VER_REV(0x1,0x5) // CUAV V5, Rev 5
#define V540 HW_VER_REV(0x4,0x0) // HolyBro mini no can 2,3, Rev 0
#define V550 HW_VER_REV(0x5,0x0) // CUAV V5+, Rev 0
#define V552 HW_VER_REV(0x5,0x2) // CUAV V5+ ICM42688P, Rev 2
#define V560 HW_VER_REV(0x6,0x0) // CUAV V5nano with can 2, Rev 0
#define V562 HW_VER_REV(0x6,0x2) // CUAV V5nano ICM42688P, Rev 2

/* CAN Silence
*
* Silent mode control \ ESC Mux select
Expand Down
12 changes: 7 additions & 5 deletions boards/px4/fmu-v5/src/manifest.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,11 +157,13 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {


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
{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
{V500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
{V515, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5 R:5 V:1
{V540, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // HolyBro mini no can 2,3
{V550, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:0 V:5
{V552, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5+ R:2 V:5 ICM42688P
{V560, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // CUAV V5nano R:0 V:6 with can 2
{V562, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // Alias for CUAV V5nano R:2 V:6 ICM42688P
};

/************************************************************************************
Expand Down
94 changes: 73 additions & 21 deletions boards/px4/fmu-v5/src/spi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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(V500, {
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}),
}),
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})

initSPIHWVersion(V552, {
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::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin6}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin7}),
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin8})

initSPIHWVersion(V562, {
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);
2 changes: 1 addition & 1 deletion platforms/common/spi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
int hw_version_revision = board_get_hw_version();
#else
int hw_version_revision = board_get_hw_revision();
int hw_version_revision = (board_get_hw_version() << 8) | board_get_hw_revision();
#endif


Expand Down