From 9353c15e8ab30b7fb8eb9b3478312e32f60f9d97 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 Apr 2022 09:46:48 -0700 Subject: [PATCH] px4_fmu-v5X:Added Holybro mini base board --- boards/px4/fmu-v5x/init/rc.board_sensors | 23 ++++++- boards/px4/fmu-v5x/src/board_config.h | 29 +++++---- boards/px4/fmu-v5x/src/can.c | 21 +++++- boards/px4/fmu-v5x/src/manifest.c | 48 ++++++++++++++ boards/px4/fmu-v5x/src/spi.cpp | 82 ++++++++++++++++++++++-- 5 files changed, 181 insertions(+), 22 deletions(-) diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 48ba01eeb1..1dfa208384 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -3,6 +3,12 @@ # PX4 FMUv5X specific board sensors init #------------------------------------------------------------------------------ +set HAVE_PM2 yes + +if ver hwtypecmp V5X50 V5X51 V5X52 +then + set HAVE_PM2 no +fi if param compare -s ADC_ADS1115_EN 1 then ads1115 start -X @@ -15,21 +21,31 @@ if param compare SENS_EN_INA226 1 then # Start Digital power monitors ina226 -X -b 1 -t 1 -k start - ina226 -X -b 2 -t 2 -k start + + if [ $HAVE_PM2 = yes ] + then + ina226 -X -b 2 -t 2 -k start + fi fi if param compare SENS_EN_INA228 1 then # Start Digital power monitors ina228 -X -b 1 -t 1 -k start - ina228 -X -b 2 -t 2 -k start + if [ $HAVE_PM2 = yes ] + then + ina228 -X -b 2 -t 2 -k start + fi fi if param compare SENS_EN_INA238 1 then # Start Digital power monitors ina238 -X -b 1 -t 1 -k start - ina238 -X -b 2 -t 2 -k start + if [ $HAVE_PM2 = yes ] + then + ina238 -X -b 2 -t 2 -k start + fi fi if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2 @@ -96,3 +112,4 @@ then fi fi +unset HAVE_PM2 \ No newline at end of file diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index bdb8b2b5fe..d1055b7692 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -185,18 +185,23 @@ #define HW_INFO_INIT {'V','5','X','x', 'x',0} #define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ #define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 // Base FMUM -#define V5X00 HW_VER_REV(0x0,0x0) // FMUV5X, Rev 0 -#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 V5X90 HW_VER_REV(0x9,0x0) // NO USB, Rev 0 -#define V5X91 HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388, Rev 1 -#define V5X92 HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388, Rev 2 -#define V5Xa0 HW_VER_REV(0xa,0x0) // NO USB (Q), Rev 0 -#define V5Xa1 HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 -#define V5Xa2 HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2 +#define V5X00 HW_VER_REV(0x0,0x0) // FMUV5X, Rev 0 +#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(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 +#define V5X90 HW_VER_REV(0x9,0x0) // NO USB, Rev 0 +#define V5X91 HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388, Rev 1 +#define V5X92 HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388, Rev 2 +#define V5Xa0 HW_VER_REV(0xa,0x0) // NO USB (Q), Rev 0 +#define V5Xa1 HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 +#define V5Xa2 HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2 + +#define UAVCAN_NUM_IFACES_RUNTIME 1 /* HEATER * PWM in future diff --git a/boards/px4/fmu-v5x/src/can.c b/boards/px4/fmu-v5x/src/can.c index 1a2775d698..a383a56edd 100644 --- a/boards/px4/fmu-v5x/src/can.c +++ b/boards/px4/fmu-v5x/src/can.c @@ -37,7 +37,26 @@ * Board-specific CAN functions. */ -#ifdef CONFIG_CAN +#if !defined(CONFIG_CAN) + +#include + +#include "board_config.h" + + +__EXPORT +uint16_t board_get_can_interfaces(void) +{ + uint16_t enabled_interfaces = 0x3; + + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_CAN2)) { + enabled_interfaces &= ~(1 << 1); + } + + return enabled_interfaces; +} + +#else #include #include diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c index ac566ac410..91415af45e 100644 --- a/boards/px4/fmu-v5x/src/manifest.c +++ b/boards/px4/fmu-v5x/src/manifest.c @@ -75,24 +75,61 @@ static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; // declared in board_common.h static const px4_hw_mft_item_t hw_mft_list_v0500[] = { { + // PX4_MFT_PX4IO .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, { + // PX4_MFT_USB .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, + { + // PX4_MFT_CAN2 + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0550[] = { + { + // PX4_MFT_PX4IO + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, }; static const px4_hw_mft_item_t hw_mft_list_v0510[] = { { + // PX4_MFT_PX4IO .present = 0, .mandatory = 0, .connection = px4_hw_con_unknown, }, { + // PX4_MFT_USB + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + // PX4_MFT_CAN2 .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, @@ -101,15 +138,23 @@ static const px4_hw_mft_item_t hw_mft_list_v0510[] = { static const px4_hw_mft_item_t hw_mft_list_v0509[] = { { + // PX4_MFT_PX4IO .present = 1, .mandatory = 1, .connection = px4_hw_con_onboard, }, { + // PX4_MFT_USB .present = 0, .mandatory = 0, .connection = px4_hw_con_unknown, }, + { + // PX4_MFT_CAN2 + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, }; static px4_hw_mft_list_entry_t mft_lists[] = { @@ -118,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 + {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 {V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB, Rev 0 {V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 {V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 2 diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index 83d7962604..e9383647f0 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(HW_VER_REV(0, 0), { + initSPIHWVersion(V5X00, { 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}), @@ -47,10 +47,80 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION 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 + // 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(V5X01, { + 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(V5X02, { + 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}), + // }, {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}), + }, {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}) }), @@ -60,7 +130,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(HW_VER_REV(0, 1), { + initSPIHWVersion(V5X51, { 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}), @@ -84,7 +154,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(HW_VER_REV(0, 2), { + initSPIHWVersion(V5X52, { 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}),