From a142804278ccf740ac55188c21d75e9b48662843 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 27 Sep 2022 19:46:23 -0600 Subject: [PATCH] Fix ARKV6X control allocator with base boards --- boards/ark/fmu-v6x/src/board_config.h | 10 +++++ boards/ark/fmu-v6x/src/manifest.c | 56 ++++++++++++++++++++++++++- boards/ark/fmu-v6x/src/mtd.cpp | 6 +-- boards/ark/fmu-v6x/src/spi.cpp | 5 +-- 4 files changed, 67 insertions(+), 10 deletions(-) diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 14827405b459..3db9c7288369 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -218,8 +218,18 @@ #define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 #define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 #define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 +#define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 #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 V6X40 HW_VER_REV(0x4,0x0) // FMUV6X, HB CM4 base Rev 0 +#define V6X41 HW_VER_REV(0x4,0x1) // FMUV6X, BMI388 I2C2 HB CM4 base Rev 1 +#define V6X43 HW_VER_REV(0x4,0x3) // FMUV6X, Sensor Set HB CM4 base Rev 3 +#define V6X44 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 +#define V6X54 HW_VER_REV(0x5,0x4) // FMUV6X, Sensor Set HB Mini Rev 4 /* HEATER diff --git a/boards/ark/fmu-v6x/src/manifest.c b/boards/ark/fmu-v6x/src/manifest.c index 3a4fc93bff48..8369c02d67e8 100644 --- a/boards/ark/fmu-v6x/src/manifest.c +++ b/boards/ark/fmu-v6x/src/manifest.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 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 @@ -75,6 +75,19 @@ 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_v0600[] = { { + // 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, @@ -83,25 +96,64 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = { static const px4_hw_mft_item_t hw_mft_list_v0610[] = { { + // 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, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0650[] = { + { + // PX4_MFT_PX4IO + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_unknown, + }, + { + // 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 px4_hw_mft_list_entry_t mft_lists[] = { // ver_rev {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO {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_v0600, arraySize(hw_mft_list_v0600)}, // HB CM4 base + {V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 HB CM4 base + {V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 + {V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // 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 + {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 + {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 + {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 + {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 }; /************************************************************************************ diff --git a/boards/ark/fmu-v6x/src/mtd.cpp b/boards/ark/fmu-v6x/src/mtd.cpp index ce047df328fc..0d32267f31de 100644 --- a/boards/ark/fmu-v6x/src/mtd.cpp +++ b/boards/ark/fmu-v6x/src/mtd.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * Copyright (C) 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 @@ -42,10 +42,6 @@ static const px4_mft_device_t i2c3 = { // 24LC64T on Base 8K 32 X 2 .bus_type = px4_mft_device_t::I2C, .devid = PX4_MK_I2C_DEVID(3, 0x51) }; -static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 256 - .bus_type = px4_mft_device_t::I2C, - .devid = PX4_MK_I2C_DEVID(4, 0x50) -}; static const px4_mtd_entry_t fmum_fram = { diff --git a/boards/ark/fmu-v6x/src/spi.cpp b/boards/ark/fmu-v6x/src/spi.cpp index 0e08f86bec59..063aeb57219d 100644 --- a/boards/ark/fmu-v6x/src/spi.cpp +++ b/boards/ark/fmu-v6x/src/spi.cpp @@ -61,14 +61,13 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIHWVersion(HW_VER_REV(0, 3), { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, 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}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices