From f4a5c71ccd385c5d7ac384de76cb91bb2be59a5b Mon Sep 17 00:00:00 2001 From: Vincentpoont2 Date: Mon, 22 Aug 2022 14:38:36 +0800 Subject: [PATCH] Add Holybro Pixhawk Pi CM4 Baseboard Support --- boards/px4/fmu-v5x/init/rc.board_sensors | 53 ++++++++++++------------ boards/px4/fmu-v5x/src/board_config.h | 3 ++ boards/px4/fmu-v5x/src/manifest.c | 3 ++ boards/px4/fmu-v5x/src/spi.cpp | 48 +++++++++++++++++++++ boards/px4/fmu-v6x/init/rc.board_sensors | 6 +-- boards/px4/fmu-v6x/src/board_config.h | 4 ++ boards/px4/fmu-v6x/src/manifest.c | 4 ++ boards/px4/fmu-v6x/src/spi.cpp | 47 +++++++++++++++++++++ 8 files changed, 139 insertions(+), 29 deletions(-) diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 1dfa208384..6fe7689fa1 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -48,33 +48,11 @@ then fi fi -if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2 +if ver hwtypecmp V5X00 V5X01 V5X02 V5X10 V5X41 then - #SKYNODE base fmu board orientation - - if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 - then - # Internal SPI BMI088 - bmi088 -A -R 2 -s start - bmi088 -G -R 2 -s start - else - # Internal SPI bus ICM20649 - icm20649 -s -R 4 start - fi - - # Internal SPI bus ICM42688p - icm42688p -R 4 -s start - - # Internal SPI bus ICM-20602 (hard-mounted) - icm20602 -R 8 -s start - - # Internal magnetometer on I2c - bmm150 -I -R 6 start - -else #FMUv5Xbase board orientation - if ver hwtypecmp V5X00 V5X01 + if ver hwtypecmp V5X00 V5X01 V5X41 then # Internal SPI BMI088 bmi088 -A -R 4 -s start @@ -93,6 +71,29 @@ else # Internal magnetometer on I2c bmm150 -I start +else + #SKYNODE base fmu board orientation + + # V5X010001 ommited because not representable in this hardware format + if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 V5X80 V5X81 + then + # Internal SPI BMI088 + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start + else + # Internal SPI bus ICM20649 + icm20649 -s -R 4 start + fi + + # Internal SPI bus ICM42688p + icm42688p -R 4 -s start + + # Internal SPI bus ICM-20602 (hard-mounted) + icm20602 -R 8 -s start + + # Internal magnetometer on I2c + bmm150 -I -R 6 start + fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) @@ -104,7 +105,7 @@ ist8310 -X -b 1 -R 10 start if param compare SENS_INT_BARO_EN 1 then bmp388 -I -a 0x77 start - if ver hwtypecmp V5X00 V5X90 V5Xa0 + if ver hwtypecmp V5X00 V5X10 V5X80 V5X90 V5Xa0 then bmp388 -I start else @@ -112,4 +113,4 @@ then fi fi -unset HAVE_PM2 \ No newline at end of file +unset HAVE_PM2 diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 9d3fb6d437..2df2a1f971 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -191,6 +191,9 @@ #define V5X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 #define V5X01 HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388, Rev 1 #define V5X02 HW_VER_REV(0x0,0x2) // FMUV5X, Rev 2 +#define V5X50 HW_VER_REV(0x4,0x0) // FMUV5X, HB CM4 base Rev 0 +#define V5X51 HW_VER_REV(0x4,0x1) // FMUV5X I2C2 BMP388, HB CM4 base Rev 1 +#define V5X52 HW_VER_REV(0x4,0x2) // FMUV5X, HB CM4 base Rev 2 #define V5X50 HW_VER_REV(0x5,0x0) // FMUV5X, HB Mini Rev 0 #define V5X51 HW_VER_REV(0x5,0x1) // FMUV5X I2C2 BMP388, HB Mini Rev 1 #define V5X52 HW_VER_REV(0x5,0x2) // FMUV5X, HB Mini Rev 2 diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c index 91415af45e..251d4c6d37 100644 --- a/boards/px4/fmu-v5x/src/manifest.c +++ b/boards/px4/fmu-v5x/src/manifest.c @@ -163,6 +163,9 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 1 {V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 2 {V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO, Rev 0 + {V5X40, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // FMUV5X, HB CM4 base Rev 0 + {V5X41, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // FMUV5X, HB CM4 base Rev 1 + {V5X42, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)}, // FMUV5X, HB CM4 base Rev 2 {V5X50, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 0 {V5X51, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 1 {V5X52, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X, HB Mini Rev 2 diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index e9383647f0..b64cfa0218 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -106,6 +106,54 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(V5X41, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V5X42, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + }, {GPIO::PortD, GPIO::Pin15}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V5X50, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 4facbe4c05..d1b9e29cfc 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -47,7 +47,7 @@ then fi fi -if ver hwtypecmp V6X04 V6X14 V6X54 +if ver hwtypecmp V6X04 V6X14 V6X44 V6X54 then # Internal SPI bus ICM20649 icm20649 -s -R 6 start @@ -60,7 +60,7 @@ fi # Internal SPI bus ICM42688p icm42688p -R 6 -s start -if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 V6X53 V6X54 +if ver hwtypecmp V6X03 V6X13 V6X33 V6X04 V6X14 V6X53 V6X54 then # Internal SPI bus ICM-42670-P (hard-mounted) icm42670p -R 10 -s start @@ -88,4 +88,4 @@ fi # Baro on I2C3 ms5611 -X start -unset HAVE_PM2 \ No newline at end of file +unset HAVE_PM2 diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index f72f2c134c..7689946b76 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -224,6 +224,10 @@ #define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 #define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 #define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 +#define V6X50 HW_VER_REV(0x4,0x0) // FMUV6X, HB CM4 base Rev 0 +#define V6X51 HW_VER_REV(0x4,0x1) // FMUV6X, BMI388 I2C2 HB CM4 base Rev 1 +#define V6X53 HW_VER_REV(0x4,0x3) // FMUV6X, Sensor Set HB CM4 base Rev 3 +#define V6X54 HW_VER_REV(0x4,0x4) // FMUV6X, Sensor Set HB CM4 base Rev 4 #define V6X50 HW_VER_REV(0x5,0x0) // FMUV6X, HB Mini Rev 0 #define V6X51 HW_VER_REV(0x5,0x1) // FMUV6X, BMI388 I2C2 HB Mini Rev 1 #define V6X53 HW_VER_REV(0x5,0x3) // FMUV6X, Sensor Set HB Mini Rev 3 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index c5a3da2831..9181814308 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -142,6 +142,10 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 + {V6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // HB CM4 base + {V6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2 HB CM4 base + {V6X43, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 + {V6X44, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index e25327001d..b74bef7b5f 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -84,6 +84,53 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIHWVersion(V6X43, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V6X44, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X50, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),