diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0e0041223a5d..49b062909d06 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -170,7 +170,7 @@ else param select-backup $PARAM_BACKUP_FILE fi - if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X PX4_FMU_V6XRT + if mft query -q -k MFT -s MFT_ETHERNET -v 1 then netman update -i eth0 fi diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 87451b0c52ea..52d098101df1 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -22,12 +22,12 @@ param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_I 0.025 #param set-default SENS_IMU_TEMP_P 1.0 -if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +if ver hwtypecmp ARKV6X000 then param set-default SENS_TEMP_ID 2818058 fi -if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +if ver hwtypecmp ARKV6X001 then param set-default SENS_TEMP_ID 3014666 fi diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index 20f0c228d59b..a3e32486df80 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -5,7 +5,7 @@ set HAVE_PM2 yes set HAVE_PM3 yes -if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no set HAVE_PM3 no @@ -66,7 +66,7 @@ then fi fi -if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +if ver hwtypecmp ARKV6X000 then # Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz iim42652 -R 3 -s -b 1 -C 32051 start @@ -78,7 +78,7 @@ then icm42688p -R 6 -s -b 3 -C 32051 start fi -if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +if ver hwtypecmp ARKV6X001 then # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz #iim42653 -R 3 -s -b 1 -C 32051 start diff --git a/boards/ark/fmu-v6x/src/CMakeLists.txt b/boards/ark/fmu-v6x/src/CMakeLists.txt index 3135751c666e..78b8222f19d8 100644 --- a/boards/ark/fmu-v6x/src/CMakeLists.txt +++ b/boards/ark/fmu-v6x/src/CMakeLists.txt @@ -55,7 +55,6 @@ else() init.c led.c mtd.cpp - manifest.c sdio.c spi.cpp spix_sync.c diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 4782fc7efdd8..04b84631c760 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2024 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 @@ -204,25 +204,17 @@ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "ARKV6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Base/FMUM -#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Sensor Set Rev 0 -#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, Sensor Set Rev 1 -//#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3 -//#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4 -#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Sensor Set Rev 0 -#define ARKV6X11 HW_VER_REV(0x1,0x1) // NO PX4IO, Sensor Set Rev 1 -#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 -#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 -#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 -#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped +#define ARKV6X_0 HW_FMUM_ID(0x0) // ARKV6X, Sensor Set Rev 0 +#define ARKV6X_1 HW_FMUM_ID(0x1) // ARKV6X, Sensor Set Rev 1 #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/ark/fmu-v6x/src/manifest.c b/boards/ark/fmu-v6x/src/manifest.c deleted file mode 100644 index f4e012bfb068..000000000000 --- a/boards/ark/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,216 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// 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, - }, -}; - -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_v0640[] = { - { - // 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_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 - {ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 0 - {ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 1 - {ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 0 - {ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 1 - {ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 - {ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 - {ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 - {ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/ark/fmu-v6x/src/mtd.cpp b/boards/ark/fmu-v6x/src/mtd.cpp index 80d41e0b78a3..6e6004ad30bc 100644 --- a/boards/ark/fmu-v6x/src/mtd.cpp +++ b/boards/ark/fmu-v6x/src/mtd.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 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 @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -86,10 +89,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/ark/fmu-v6x/src/spi.cpp b/boards/ark/fmu-v6x/src/spi.cpp index f83a5a91cd4b..fc60153efedf 100644 --- a/boards/ark/fmu-v6x/src/spi.cpp +++ b/boards/ark/fmu-v6x/src/spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * Copyright (C) 2024 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 @@ -36,7 +36,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(ARKV6X00, { + initSPIFmumID(ARKV6X_0, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -59,145 +59,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(ARKV6X01, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, 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(ARKV6X10, { - initSPIBus(SPI::Bus::SPI1, { - 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_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 - // 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(ARKV6X11, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, 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(ARKV6X40, { - initSPIBus(SPI::Bus::SPI1, { - 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_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 - // 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(ARKV6X41, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, 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(ARKV6X50, { - initSPIBus(SPI::Bus::SPI1, { - 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_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 - // 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(ARKV6X51, { + initSPIFmumID(ARKV6X_1, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index cf152b948eda..83596bc9b209 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 +if ver hwbasecmp 008 009 00a 010 then # Skynode: use the "custom participant" config for uxrce_dds_client param set-default UXRCE_DDS_PTCFG 2 @@ -25,12 +25,12 @@ fi safety_button start # GPIO Expander driver on external I2C3 -if ver hwtypecmp V5X009000 V5X009001 +if ver hwbasecmp 009 then # No USB mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10 fi -if ver hwtypecmp V5X00a000 V5X00a001 V5X008000 V5X008001 +if ver hwbasecmp 00a 008 then mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 fi diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index ae59f79dffa1..8d1d7c1e38ab 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -3,7 +3,7 @@ # board specific MAVLink startup script. #------------------------------------------------------------------------------ -if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 +if ver hwbasecmp 008 009 00a 010 then # Start MAVLink on the UART connected to the mission computer mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index d6b91f4505fe..e14364c9508e 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -5,7 +5,7 @@ set HAVE_PM2 yes -if ver hwtypecmp V5X005000 V5X005001 V5X005002 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -49,54 +49,53 @@ then fi fi -if ver hwtypecmp V5X000000 V5X000001 V5X000002 V5X001000 V5X004000 V5X004001 V5X004002 V5X005001 V5X005002 +if ver hwbasecmp 008 009 00a 010 then - #FMUv5Xbase board orientation + #SKYNODE base fmu board orientation - if ver hwtypecmp V5X000000 V5X000001 V5X004000 V5X004001 V5X005001 + if ver hwtypecmp V5X000 V5X001 then # Internal SPI BMI088 - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start else # Internal SPI bus ICM20649 - icm20649 -s -R 6 start + icm20649 -s -R 4 start fi # Internal SPI bus ICM42688p - icm42688p -R 6 -s start + icm42688p -R 4 -s start # Internal SPI bus ICM-20602 (hard-mounted) - icm20602 -R 10 -s start + icm20602 -R 8 -s start # Internal magnetometer on I2c - bmm150 -I start + bmm150 -I -R 6 start + + # Auto start power monitors + pm_selector_auterion start else - #SKYNODE base fmu board orientation + #FMUv5Xbase board orientation - if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001 + if ver hwtypecmp V5X000 V5X001 then # Internal SPI BMI088 - bmi088 -A -R 2 -s start - bmi088 -G -R 2 -s start + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start else # Internal SPI bus ICM20649 - icm20649 -s -R 4 start + icm20649 -s -R 6 start fi # Internal SPI bus ICM42688p - icm42688p -R 4 -s start + icm42688p -R 6 -s start # Internal SPI bus ICM-20602 (hard-mounted) - icm20602 -R 8 -s start + icm20602 -R 10 -s start # Internal magnetometer on I2c - bmm150 -I -R 6 start - - # Auto start power monitors - pm_selector_auterion start - + bmm150 -I 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) @@ -108,7 +107,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 V5X000000 V5X001000 V5X008000 V5X009000 V5X00a000 + if ver hwtypecmp V5X000 then bmp388 -I start else diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index 684b11736eda..311b7c9be8c9 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -36,7 +36,6 @@ add_library(drivers_board i2c.cpp init.cpp led.c - manifest.c mtd.cpp sdio.c spi.cpp diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 67cac1acd35f..2fe1654b65f5 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -179,31 +179,17 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) #define GPIO_HW_VER_SENSE /* PF4 */ ADC3_GPIO(14) #define HW_INFO_INIT_PREFIX "V5X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7 -// 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 V5X40 HW_VER_REV(0x4,0x0) // FMUV5X, HB CM4 base Rev 0 -#define V5X41 HW_VER_REV(0x4,0x1) // FMUV5X I2C2 BMP388, HB CM4 base Rev 1 -#define V5X42 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 -#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 V5X101 HW_VER_REV(0x10,0x1) // NO USB (Q) I2C2 BMP388, Rev 1 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 + +#define V5X_0 HW_FMUM_ID(0x0) // FMUV5X, Auterion FMUv5x RC13 (baro2 BMP388 on I2C4) Sensor Set Rev 0 +#define V5X_1 HW_FMUM_ID(0x1) // FMUV5X, Auterion, HB FMUv5x RC15 (baro2 BMP388 on I2C2) Sensor Set Rev 1 +#define V5X_2 HW_FMUM_ID(0x2) // FMUV5X, HB FMUv5x Sensor Set Rev 2 #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c deleted file mode 100644 index 56f0009f5000..000000000000 --- a/boards/px4/fmu-v5x/src/manifest.c +++ /dev/null @@ -1,222 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2021 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// 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, - }, -}; - -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[] = { -// ver_rev - {V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, Rev 0 - {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 - {V5X41, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, HB CM4 base Rev 1 - {V5X42, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X, HB CM4 base Rev 2 - {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 - {V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q), Rev 0 - {V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1 - {V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2 - {V5X101, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388, Rev 1 -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/px4/fmu-v5x/src/mtd.cpp b/boards/px4/fmu-v5x/src/mtd.cpp index c0139d123279..8e57555eacad 100644 --- a/boards/px4/fmu-v5x/src/mtd.cpp +++ b/boards/px4/fmu-v5x/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -114,10 +117,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index 8fa378ee5d4d..dd6bfbb25e1e 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(V5X00, { + initSPIFmumID(V5X_0, { 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}), @@ -60,7 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X01, { + initSPIFmumID(V5X_1, { 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 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V5X02, { + initSPIFmumID(V5X_2, { 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}), @@ -107,99 +107,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - 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(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}), - 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(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}), - 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}), - }), - }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index 93acc0fbd294..6500693173a5 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 0 -if ver hwtypecmp V6X009010 V6X010010 +if ver hwbasecmp 009 010 then # Skynode: use the "custom participant" config for uxrce_dds_client param set-default UXRCE_DDS_PTCFG 2 diff --git a/boards/px4/fmu-v6x/init/rc.board_mavlink b/boards/px4/fmu-v6x/init/rc.board_mavlink index 6001553a76f3..4fef2264da97 100644 --- a/boards/px4/fmu-v6x/init/rc.board_mavlink +++ b/boards/px4/fmu-v6x/init/rc.board_mavlink @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # if skynode base board is detected start Mavlink on Telem2 -if ver hwtypecmp V6X009010 V6X010010 +if ver hwbasecmp 009 010 then mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 6a47d72f0e77..854fe7e07f30 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -5,7 +5,7 @@ set HAVE_PM2 yes set INA_CONFIGURED no -if ver hwtypecmp V6X005000 V6X005001 V6X005003 V6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -55,7 +55,7 @@ then fi #Start Auterion Power Module selector for Skynode boards -if ver hwtypecmp V6X009010 V6X010010 +if ver hwbasecmp 009 010 then pm_selector_auterion start else @@ -70,25 +70,25 @@ else fi fi -if ver hwtypecmp V6X000006 V6X004006 V6X005006 +if ver hwtypecmp V6X006 then # Internal SPI bus ICM45686 icm45686 -s -R 10 start iim42652 -s -R 6 start adis16470 -s -R 0 start else - if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 + if ver hwtypecmp V6X004 then # Internal SPI bus ICM20649 icm20649 -s -R 6 start else # Internal SPI BMI088 - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 then bmi088 -A -R 6 -s start bmi088 -G -R 6 -s start else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X010 then bmi088 -A -R 0 -s start bmi088 -G -R 0 -s start @@ -100,11 +100,11 @@ else fi # Internal SPI bus ICM42688p - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 then icm42688p -R 12 -s start else - if ver hwtypecmp V6X000010 + if ver hwtypecmp V6X010 then icm42688p -R 14 -s start else @@ -112,12 +112,12 @@ else fi fi - if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 + if ver hwtypecmp V6X003 V6X004 then # Internal SPI bus ICM-42670-P (hard-mounted) icm42670p -R 10 -s start else - if ver hwtypecmp V6X009010 V6X010010 + if ver hwbasecmp 009 010 then icm20602 -R 6 -s start else @@ -128,7 +128,7 @@ else fi # Internal magnetometer on I2c -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then rm3100 -I -b 4 start else @@ -142,7 +142,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006 + if ver hwtypecmp V6X001 V6X006 then icp201xx -I -a 0x64 start else @@ -151,7 +151,7 @@ then fi #external baro -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then icp201xx -X start else diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index d4a16ff4f303..a120ebe33623 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -55,7 +55,6 @@ else() init.c led.c mtd.cpp - manifest.c sdio.c spi.cpp timer_config.cpp diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index ff0676fab7ee..6d79c1b1518e 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -208,40 +208,22 @@ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 // Base/FMUM -#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 V6X06 HW_VER_REV(0x0,0x6) // FMUV6X, Sensor Set Rev 6 -#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 V6X16 HW_VER_REV(0x1,0x6) // NO PX4IO, Sensor Set Rev 6 -#define V6X21 HW_VER_REV(0x2,0x1) // FMUV6X, CUAV Sensor Set -#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 V6X46 HW_VER_REV(0x4,0x6) // FMUV6X, Sensor Set HB CM4 base Rev 6 -#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 -#define V6X56 HW_VER_REV(0x5,0x6) // FMUV6X, Sensor Set HB Mini Rev 6 -#define V6X90 HW_VER_REV(0x9,0x0) // Rev 0 -#define V6X0910 HW_VER_REV(0x9,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver9 -#define V6X1010 HW_VER_REV(0x10,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver10 - - +#define V6X_0 HW_FMUM_ID(0x0) // FMUV6X, Auterion,HB Sensor Set Rev 0 +#define V6X_1 HW_FMUM_ID(0x1) // FMUV6X, CUAV Sensor Set Rev 1 +#define V6X_3 HW_FMUM_ID(0x3) // FMUV6X, HB Sensor Set Rev 3 +#define V6X_4 HW_FMUM_ID(0x4) // FMUV6X, HB Sensor Set Rev 4 +#define V6X_6 HW_FMUM_ID(0x6) // FMUV6X, HB Sensor Set Rev 6 +#define V6X_8 HW_FMUM_ID(0x8) // FMUV6X, HB Sensor Set Rev 8 +#define V6X_16 HW_FMUM_ID(0x10) // FMUV6X, Auterion Sensor Set Rev 16 from EEPROM #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c deleted file mode 100644 index bda800b74919..000000000000 --- a/boards/px4/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,208 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2021 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// 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, - }, -}; - -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)}, - {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 - {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 - {V6X06, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 - {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 - {V6X46, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 - {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 - {V6X56, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 6 - {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 - {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 - {V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver9 - {V6X1010, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver10 -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/px4/fmu-v6x/src/mtd.cpp b/boards/px4/fmu-v6x/src/mtd.cpp index c0139d123279..8e57555eacad 100644 --- a/boards/px4/fmu-v6x/src/mtd.cpp +++ b/boards/px4/fmu-v6x/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include // KiB BS nB @@ -114,10 +117,16 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index 8b002e157efa..c0150720838f 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/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(V6X00, { + initSPIFmumID(V6X_0, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -60,54 +60,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X03, { - 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(V6X06, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, 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(V6X21, { + initSPIFmumID(V6X_1, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -131,7 +84,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X43, { + initSPIFmumID(V6X_3, { 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}), @@ -155,7 +108,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X44, { + initSPIFmumID(V6X_4, { 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}), @@ -178,7 +131,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X46, { + initSPIFmumID(V6X_6, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -201,101 +154,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X50, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, 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(V6X53, { - 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(V6X54, { - 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(V6X56, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), - }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, 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(V6X0910, { + initSPIFmumID(V6X_16, { 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}), @@ -319,29 +179,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X1010, { - 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::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}), - }, {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}), - }), - }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6xrt/init/rc.board_mavlink b/boards/px4/fmu-v6xrt/init/rc.board_mavlink index 633dc58eb813..546949911863 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_mavlink +++ b/boards/px4/fmu-v6xrt/init/rc.board_mavlink @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # if skynode base board is detected start Mavlink on Telem2 -if ver hwtypecmp V6XRT009000 V6XRT010000 +if ver hwbasecmp 009 010 then mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z diff --git a/boards/px4/fmu-v6xrt/init/rc.board_sensors b/boards/px4/fmu-v6xrt/init/rc.board_sensors index a2c5f2a3d89e..64779648a4b2 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_sensors +++ b/boards/px4/fmu-v6xrt/init/rc.board_sensors @@ -1,6 +1,6 @@ #!/bin/sh # -# PX4 FMUv6X-RT specific board sensors init +# PX4 FMUv6xrt specific board sensors init #------------------------------------------------------------------------------ # # UART mapping on PX4 FMU-V6XRT: @@ -18,7 +18,7 @@ set HAVE_PM2 yes -if ver hwtypecmp V6XRT005000 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi diff --git a/boards/px4/fmu-v6xrt/src/CMakeLists.txt b/boards/px4/fmu-v6xrt/src/CMakeLists.txt index 91833ede24c1..e34b9a134c72 100644 --- a/boards/px4/fmu-v6xrt/src/CMakeLists.txt +++ b/boards/px4/fmu-v6xrt/src/CMakeLists.txt @@ -66,7 +66,6 @@ else() i2c.cpp init.c led.c - manifest.c mtd.cpp sdhc.c spi.cpp diff --git a/boards/px4/fmu-v6xrt/src/board_config.h b/boards/px4/fmu-v6xrt/src/board_config.h index 1ea636d8807c..a2edff8dabc8 100644 --- a/boards/px4/fmu-v6xrt/src/board_config.h +++ b/boards/px4/fmu-v6xrt/src/board_config.h @@ -288,7 +288,7 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_SPLIT_VERSIONING #define HW_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_SLEW_FAST) @@ -297,11 +297,9 @@ #define GPIO_HW_VER_SENSE /* GPIO_AD_23 GPIO9 Pin 22 */ ADC_GPIO(5, 22) #define HW_INFO_INIT_PREFIX "V6XRT" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0 on HB Base and T1 Base -// Base/FMUM -#define V6XRT_00 HW_VER_REV(0x0,0x0) // First Release -#define V6XRT_30 HW_VER_REV(0x3,0x0) // T1 Base w/o PX4IO -#define V6XRT_50 HW_VER_REV(0x5,0x0) // HB Mini Rev 0 +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 & 1 +#define V6XRT_0 HW_FMUM_ID(0x0) // First Release +#define V6XRT_1 HW_FMUM_ID(0x1) // Next Release #define BOARD_I2C_LATEINIT 1 /* See Note about SE550 Eanable */ diff --git a/boards/px4/fmu-v6xrt/src/manifest.c b/boards/px4/fmu-v6xrt/src/manifest.c deleted file mode 100644 index 9ebaddf406d3..000000000000 --- a/boards/px4/fmu-v6xrt/src/manifest.c +++ /dev/null @@ -1,208 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018, 2021, 2024 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file manifest.c - * - * This module supplies the interface to the manifest of hardware that is - * optional and dependent on the HW REV and HW VER IDs - * - * The manifest allows the system to know whether a hardware option - * say for example the PX4IO is an no-pop option vs it is broken. - * - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include -#include - -#include -#include -#include - -#include "systemlib/px4_macros.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -typedef struct { - uint32_t hw_ver_rev; /* the version and revision */ - const px4_hw_mft_item_t *mft; /* The first entry */ - uint32_t entries; /* the lenght of the list */ -} px4_hw_mft_list_entry_t; - -typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; -#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 - -static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; - -// List of components on a specific board configuration -// The index of those components is given by the enum (px4_hw_mft_item_id_t) -// declared in board_common.h - -static const px4_hw_mft_item_t hw_mft_list_V00[] = { // HB Base - { - // 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, - }, - { - // PX4_MFT_CAN3 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_V30[] = { // NXP T1 Base - { - // 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, - }, - { - // PX4_MFT_CAN3 - .present = 1, - .mandatory = 1, - .connection = px4_hw_con_onboard, - }, -}; - -static const px4_hw_mft_item_t hw_mft_list_V50[] = { // HB Mini - { - // 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, - }, - { - // PX4_MFT_CAN3 - .present = 0, - .mandatory = 0, - .connection = px4_hw_con_unknown, - }, -}; - - -static px4_hw_mft_list_entry_t mft_lists[] = { - {V6XRT_00, hw_mft_list_V00, arraySize(hw_mft_list_V00)}, // HB Base - {V6XRT_30, hw_mft_list_V30, arraySize(hw_mft_list_V30)}, // NXP T1 Base - {V6XRT_50, hw_mft_list_V50, arraySize(hw_mft_list_V50)}, // HB Mini -}; - -/************************************************************************************ - * Name: board_query_manifest - * - * Description: - * Optional returns manifest item. - * - * Input Parameters: - * manifest_id - the ID for the manifest item to retrieve - * - * Returned Value: - * 0 - item is not in manifest => assume legacy operations - * pointer to a manifest item - * - ************************************************************************************/ - -__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) -{ - static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - uint32_t ver_rev = board_get_hw_version() << 16; - ver_rev |= board_get_hw_revision(); - - for (unsigned i = 0; i < arraySize(mft_lists); i++) { - if (mft_lists[i].hw_ver_rev == ver_rev) { - boards_manifest = &mft_lists[i]; - break; - } - } - - if (boards_manifest == px4_hw_mft_list_uninitialized) { - syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev); - } - } - - px4_hw_mft_item rv = &device_unsupported; - - if (boards_manifest != px4_hw_mft_list_uninitialized && - id < boards_manifest->entries) { - rv = &boards_manifest->mft[id]; - } - - return rv; -} diff --git a/boards/px4/fmu-v6xrt/src/mtd.cpp b/boards/px4/fmu-v6xrt/src/mtd.cpp index 9cd1795a519a..38fa63a7eda3 100644 --- a/boards/px4/fmu-v6xrt/src/mtd.cpp +++ b/boards/px4/fmu-v6xrt/src/mtd.cpp @@ -31,6 +31,9 @@ * ****************************************************************************/ +#include +#include + #include #include @@ -120,10 +123,15 @@ static const px4_mft_entry_s mtd_mft = { .pmft = (void *) &board_mtd_config, }; +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; static const px4_mft_s mft = { - .nmft = 1, + .nmft = 2, .mfts = { - &mtd_mft + &mtd_mft, + &mft_mft, } }; diff --git a/boards/px4/fmu-v6xrt/src/spi.cpp b/boards/px4/fmu-v6xrt/src/spi.cpp index 6b2ffd32d56d..d7e05735cd5a 100644 --- a/boards/px4/fmu-v6xrt/src/spi.cpp +++ b/boards/px4/fmu-v6xrt/src/spi.cpp @@ -42,7 +42,7 @@ #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { - initSPIHWVersion(V6XRT_00, { + initSPIFmumID(V6XRT_0, { initSPIBus(SPI::Bus::LPSPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 @@ -62,26 +62,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6XRT_30, { - initSPIBus(SPI::Bus::LPSPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ - }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 - - initSPIBus(SPI::Bus::LPSPI2, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port3, GPIO::Pin24}, SPI::DRDY{GPIO::Port2, GPIO::Pin7}), /* GPIO_AD_25 GPIO3_IO24, GPIO_EMC_B1_39 GPIO2_IO07 */ - }, {GPIO::Port1, GPIO::Pin22}), // Power GPIO_EMC_B1_22 GPIO1_IO22 - - initSPIBus(SPI::Bus::LPSPI3, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}, SPI::DRDY{GPIO::Port2, GPIO::Pin28}), /* GPIO_EMC_B2_08 GPIO2_IO18, GPIO_EMC_B2_18 GPIO2_IO28 */ - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin15}), /* GPIO_EMC_B2_05 GPIO2_IO15 */ - }, {GPIO::Port1, GPIO::Pin14}), // Power GPIO_EMC_B1_14 GPIO1_IO14 - - initSPIBusExternal(SPI::Bus::LPSPI6, { - initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin9}, SPI::DRDY{GPIO::Port1, GPIO::Pin5}), /* GPIO_LPSR_09 GPIO6_IO09 GPIO_EMC_B1_05 GPIO1_IO05*/ - initSPIConfigExternal(SPI::CS{GPIO::Port6, GPIO::Pin8}, SPI::DRDY{GPIO::Port1, GPIO::Pin7}), /* GPIO_LPSR_08 GPIO6_IO08 GPIO_EMC_B1_07 GPIO1_IO07*/ - }), - }), - initSPIHWVersion(V6XRT_50, { + initSPIFmumID(V6XRT_1, { initSPIBus(SPI::Bus::LPSPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */ }, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33 GPIO2_IO01 diff --git a/boards/sky-drones/smartap-airlink/src/board_config.h b/boards/sky-drones/smartap-airlink/src/board_config.h index ac21a1686cb2..5b957e0694cc 100644 --- a/boards/sky-drones/smartap-airlink/src/board_config.h +++ b/boards/sky-drones/smartap-airlink/src/board_config.h @@ -168,7 +168,7 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_HW_VERSIONING // migrate to Split #define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) #define GPIO_HW_REV_SENSE /* PF5 */ ADC3_GPIO(15) diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index c6109decf628..a0bafe7d59de 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -51,6 +51,7 @@ add_library(px4_platform STATIC px4_cli.cpp shutdown.cpp spi.cpp + pab_manifest.c ${SRCS} ) target_link_libraries(px4_platform prebuild_targets px4_work_queue) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 6b316c31d8b4..970c47ec9e08 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -269,6 +269,18 @@ # define HW_VER_REV(v,r) ((uint32_t)((v) & 0xffff) << 16) | ((uint32_t)(r) & 0xffff) #endif +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +typedef uint16_t hw_fmun_id_t; +typedef uint16_t hw_base_id_t; +// Original Signals GPIO_HW_REV_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the FMUM +// Original Signals GPIO_HW_VER_SENSE/GPIO_HW_VER_REV_DRIVE is used to ID the BASE +# define BOARD_HAS_VERSIONING 1 +# define HW_FMUM_ID(rev) ((hw_fmun_id_t)(rev) & 0xffff) +# define HW_BASE_ID(ver) ((hw_base_id_t)(ver) & 0xffff) +# define GET_HW_FMUM_ID() (HW_FMUM_ID(board_get_hw_revision())) +# define GET_HW_BASE_ID() (HW_BASE_ID(board_get_hw_version())) +#endif + #define HW_INFO_REV_DIGITS 3 #define HW_INFO_VER_DIGITS 3 @@ -668,20 +680,51 @@ bool board_booted_by_px4(void); ************************************************************************************/ typedef enum { - PX4_MFT_PX4IO = 0, - PX4_MFT_USB = 1, - PX4_MFT_CAN2 = 2, - PX4_MFT_CAN3 = 3, + PX4_MFT_PX4IO = 0, + PX4_MFT_USB = 1, + PX4_MFT_CAN2 = 2, + PX4_MFT_CAN3 = 3, + PX4_MFT_PM2 = 4, + PX4_MFT_ETHERNET = 5, + PX4_MFT_T1_ETH = 6, + PX4_MFT_T100_ETH = 7, + PX4_MFT_T1000_ETH = 8, } px4_hw_mft_item_id_t; +typedef int (*system_query_func_t)(const char *sub, const char *val, void *out); + +#define PX4_MFT_MFT_TYPES { \ + PX4_MFT_PX4IO, \ + PX4_MFT_USB, \ + PX4_MFT_CAN2, \ + PX4_MFT_CAN3, \ + PX4_MFT_PM2, \ + PX4_MFT_ETHERNET, \ + PX4_MFT_T1_ETH, \ + PX4_MFT_T100_ETH, \ + PX4_MFT_T1000_ETH } + +#define PX4_MFT_MFT_STR_TYPES { \ + "MFT_PX4IO", \ + "MFT_USB", \ + "MFT_CAN2", \ + "MFT_CAN3", \ + "MFT_PM2", \ + "MFT_ETHERNET", \ + "MFT_T1_ETH", \ + "MFT_T100_ETH", \ + "MFT_T1000_ETH", \ + "MFT_T1000_ETH"} + typedef enum { - px4_hw_con_unknown = 0, - px4_hw_con_onboard = 1, + px4_hw_con_unknown = 0, + px4_hw_con_onboard = 1, px4_hw_con_connector = 3, } px4_hw_connection_t; typedef struct { + unsigned int id: 16; /* The id px4_hw_mft_item_id_t */ unsigned int present: 1; /* 1 if this board have this item */ unsigned int mandatory: 1; /* 1 if this item has to be present and working */ unsigned int connection: 2; /* See px4_hw_connection_t */ @@ -693,7 +736,7 @@ typedef const px4_hw_mft_item_t *px4_hw_mft_item; #if defined(BOARD_HAS_VERSIONING) __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id); - +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out); # define PX4_MFT_HW_SUPPORTED(ID) (board_query_manifest((ID))->present) # define PX4_MFT_HW_REQUIRED(ID) (board_query_manifest((ID))->mandatory) # define PX4_MFT_HW_IS_ONBOARD(ID) (board_query_manifest((ID))->connection == px4_hw_con_onboard) @@ -766,6 +809,26 @@ __EXPORT const char *board_get_hw_type_name(void); #define board_get_hw_type_name() "" #endif +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the HW type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This may be a 0 length string "" + * + ************************************************************************************/ + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +__EXPORT const char *board_get_hw_base_type_name(void); +#else +#define board_get_hw_base_type_name() "" +#endif + /************************************************************************************ * Name: board_get_hw_version * diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index 66644098228f..a2e4682b98cd 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -73,7 +73,11 @@ struct px4_spi_bus_t { struct px4_spi_bus_all_hw_t { px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS]; - int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + hw_fmun_id_t board_hw_fmun_id {USHRT_MAX}; +#else + int board_hw_version_revision {-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused +#endif }; #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c new file mode 100644 index 000000000000..d0570708a7ca --- /dev/null +++ b/platforms/common/pab_manifest.c @@ -0,0 +1,404 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pab_manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW_BASE_ID + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + +typedef struct { + hw_base_id_t hw_base_id; /* The ID of the Base */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific base board configuration +// The ids of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h + +// BASE ID 0 Auterion vXx base board +static const px4_hw_mft_item_t base_configuration_0[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 1 vXx base without px4io +static const px4_hw_mft_item_t base_configuration_1[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 2 Modal AI Alaised to ID 0 + +// BASE ID 3 NXP T1 PHY +static const px4_hw_mft_item_t base_configuration_3[] = { + { + .id = PX4_MFT_PX4IO, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T1_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 4 HB CM4 Alaised to ID 0 + +// BASE ID 5 HB min +static const px4_hw_mft_item_t base_configuration_5[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN2, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, +}; + +// BASE ID 6 Not allocated +// BASE ID 7 Read from EEPROM +// BASE ID 8 Skynode QS with USB - Alaised to ID 0 + +// BASE ID 9 Auterion Skynode base RC9 & older (no usb +static const px4_hw_mft_item_t base_configuration_9[] = { + { + .id = PX4_MFT_PX4IO, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_USB, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_CAN2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_CAN3, + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_PM2, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, + { + .id = PX4_MFT_ETHERNET, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_connector, + }, + { + .id = PX4_MFT_T100_ETH, + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +// BASE ID 10 Skynode QS no USB Alaised to ID 9 +// BASE ID 16 Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0 + + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO + {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO + {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base + {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY + {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base + {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini + {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 + {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 + {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 + {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 +}; + +/************************************************************************************ + * Name: base_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + hw_base_id_t hw_base_id = GET_HW_BASE_ID(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_base_id == hw_base_id) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %04" PRIx16 " is not supported!\n", hw_base_id); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized) + for (unsigned int ndx = 0; ndx < boards_manifest->entries; ndx++) { + if (boards_manifest->mft[ndx].id == id) { + rv = &boards_manifest->mft[id]; + break; + } + } + + return rv; +} + +__EXPORT int system_query_manifest(const char *sub, const char *val, void *out) +{ + static const char *keys[] = PX4_MFT_MFT_STR_TYPES; + static const px4_hw_mft_item_id_t item_ids[] = PX4_MFT_MFT_TYPES; + px4_hw_mft_item rv = &device_unsupported; + int id = -1; + int intval = atoi(val); + + for (unsigned int k = 0; k < arraySize(keys); k++) { + if (!strcmp(keys[k], sub)) { + id = item_ids[k]; + break; + } + } + + if (id != -1) { + // In case we have to filter when a FMUM is mounted to a BASE + // For now just use the board + rv = board_query_manifest(id); + return rv->present == intval ? OK : -ENXIO; + } + + return -ENOENT; +} +#endif diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp index adda7b4b5795..963372bdbe0f 100644 --- a/platforms/common/spi.cpp +++ b/platforms/common/spi.cpp @@ -44,12 +44,26 @@ #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1 void px4_set_spi_buses_from_hw_version() { -#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) +# if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + int hw_fmun_id = GET_HW_FMUM_ID(); + + for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { + if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_fmun_id == 0) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + + if (px4_spi_buses_all_hw[i].board_hw_fmun_id == hw_fmun_id) { + px4_spi_buses = px4_spi_buses_all_hw[i].buses; + } + } + +# else + +# if defined(BOARD_HAS_SIMPLE_HW_VERSIONING) int hw_version_revision = board_get_hw_version(); -#else +# else int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision()); -#endif - +# endif for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) { if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) { @@ -61,6 +75,8 @@ void px4_set_spi_buses_from_hw_version() } } +# endif + if (!px4_spi_buses) { // fallback px4_spi_buses = px4_spi_buses_all_hw[0].buses; } diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h index 95e99ffd46d5..b0864d45c0ec 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h @@ -36,6 +36,10 @@ __BEGIN_DECLS #define HW_INFO_SUFFIX "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x%0" STRINGIFY(HW_INFO_REV_DIGITS) "x" +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +# define HW_INFO_FMUM_SUFFIX "%0" STRINGIFY(HW_INFO_REV_DIGITS) "x" +# define HW_INFO_BASE_SUFFIX "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x" +#endif /************************************************************************************ * Name: board_determine_hw_info * diff --git a/platforms/nuttx/src/px4/common/px4_manifest.cpp b/platforms/nuttx/src/px4/common/px4_manifest.cpp index d7402215abdd..f3dc9a5e60bc 100644 --- a/platforms/nuttx/src/px4/common/px4_manifest.cpp +++ b/platforms/nuttx/src/px4/common/px4_manifest.cpp @@ -104,6 +104,13 @@ __EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type, break; case MFT: + if (mft->mfts[m]->pmft != nullptr) { + system_query_func_t query = (system_query_func_t) mft->mfts[m]->pmft; + return query(sub, val, nullptr); + } + + break; + default: rv = -ENODATA; break; diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c index 127332feb6f3..e8cfeff32881 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c @@ -50,7 +50,7 @@ #include #include -#if defined(BOARD_HAS_HW_VERSIONING) +#if defined(BOARD_HAS_HW_VERSIONING) || defined(BOARD_HAS_HW_SPLIT_VERSIONING) # if defined(GPIO_HW_VER_REV_DRIVE) # define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE @@ -66,6 +66,9 @@ static int hw_version = 0; static int hw_revision = 0; static char hw_info[HW_INFO_SIZE] = {0}; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static char hw_base_info[HW_INFO_SIZE] = {0}; +#endif /**************************************************************************** * Protected Functions @@ -279,6 +282,27 @@ __EXPORT const char *board_get_hw_type_name() return (const char *) hw_info; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the base type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This my be a 0 length string "" + * + ************************************************************************************/ + +__EXPORT const char *board_get_hw_base_type_name() +{ + return (const char *) hw_base_info; +} +#endif + /************************************************************************************ * Name: board_get_hw_version * @@ -380,7 +404,12 @@ int board_determine_hw_info() } if (rv == OK) { +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_FMUM_SUFFIX, GET_HW_FMUM_ID()); + snprintf(hw_base_info, sizeof(hw_info), HW_INFO_BASE_SUFFIX, GET_HW_BASE_ID()); +#else snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); +#endif } return rv; diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h index 37e04a1840ad..64cad11a3b1b 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/spi_hw_description.h @@ -186,6 +186,20 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI struct px4_spi_bus_array_t { px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; }; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static inline constexpr px4_spi_bus_all_hw_t initSPIFmumID(hw_fmun_id_t hw_fmun_id, + const px4_spi_bus_array_t &bus_items) +{ + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_fmun_id = hw_fmun_id; + return ret; +} +#else static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, const px4_spi_bus_array_t &bus_items) { @@ -198,6 +212,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_rev ret.board_hw_version_revision = hw_version_revision; return ret; } +#endif constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS]) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 54c8a836c916..6d484c0a826f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -51,7 +51,7 @@ #include #include -#if defined(BOARD_HAS_HW_VERSIONING) +#if defined(BOARD_HAS_HW_VERSIONING) || defined(BOARD_HAS_HW_SPLIT_VERSIONING) # if defined(GPIO_HW_VER_REV_DRIVE) # define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE @@ -67,7 +67,9 @@ static int hw_version = 0; static int hw_revision = 0; static char hw_info[HW_INFO_SIZE] = {0}; - +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static char hw_base_info[HW_INFO_SIZE] = {0}; +#endif /**************************************************************************** * Protected Functions ****************************************************************************/ @@ -366,6 +368,27 @@ __EXPORT const char *board_get_hw_type_name() return (const char *) hw_info; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/************************************************************************************ + * Name: board_get_hw_base_type_name + * + * Description: + * Optional returns a 0 terminated string defining the base type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This my be a 0 length string "" + * + ************************************************************************************/ + +__EXPORT const char *board_get_hw_base_type_name() +{ + return (const char *) hw_base_info; +} +#endif + /************************************************************************************ * Name: board_get_hw_version * @@ -467,7 +490,12 @@ int board_determine_hw_info() } if (rv == OK) { +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_FMUM_SUFFIX, GET_HW_FMUM_ID()); + snprintf(hw_base_info, sizeof(hw_info), HW_INFO_BASE_SUFFIX, GET_HW_BASE_ID()); +#else snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); +#endif } return rv; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h index 40cc40854b7c..3594d1d82011 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/spi_hw_description.h @@ -136,6 +136,21 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI struct px4_spi_bus_array_t { px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; }; + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static inline constexpr px4_spi_bus_all_hw_t initSPIFmumID(hw_fmun_id_t hw_fmun_id, + const px4_spi_bus_array_t &bus_items) +{ + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_fmun_id = hw_fmun_id; + return ret; +} +#else static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, const px4_spi_bus_array_t &bus_items) { @@ -148,6 +163,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_rev ret.board_hw_version_revision = hw_version_revision; return ret; } +#endif constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]); constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS]) diff --git a/src/lib/version/version.h b/src/lib/version/version.h index d7d471417e67..451393f835ed 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -88,6 +88,16 @@ static inline int px4_board_hw_revision(void) return board_get_hw_revision(); } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +/** + * get the base board type + */ +static inline const char *px4_board_base_type(void) +{ + return board_get_hw_base_type_name(); +} +#endif + /** * get the build URI (used for crash logging) */ diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index ab75009d1ec5..a2b5c1885a5c 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -51,6 +51,9 @@ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_hwtypecmp_str[] = "hwtypecmp"; +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) +static const char sz_ver_hwbasecmp_str[] = "hwbasecmp"; +#endif static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_buri_str[] = "uri"; @@ -84,6 +87,11 @@ static void usage(const char *reason) PRINT_MODULE_USAGE_COMMAND_DESCR("hwtypecmp", "Compare hardware type (returns 0 on match)"); PRINT_MODULE_USAGE_ARG(" []", "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + PRINT_MODULE_USAGE_COMMAND_DESCR("hwbasecmp", "Compare hardware base (returns 0 on match)"); + PRINT_MODULE_USAGE_ARG(" []", + "Hardware type to compare against (eg. V2). An OR comparison is used if multiple are specified", false); +#endif } extern "C" __EXPORT int ver_main(int argc, char *argv[]) @@ -129,12 +137,50 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) return 1; } +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + + if (!strncmp(argv[1], sz_ver_hwbasecmp_str, sizeof(sz_ver_hwbasecmp_str))) { + if (argc >= 3 && argv[2] != nullptr) { + const char *board_type = px4_board_base_type(); + + for (int i = 2; i < argc; ++i) { + if (strcmp(board_type, argv[i]) == 0) { + return 0; // if one of the arguments match, return success + } + } + + } else { + PX4_ERR("Not enough arguments, try 'ver hwbasecmp {000...999}[1:*]'"); + } + + return 1; + } + +#endif /* check if we want to show all */ bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { PX4_INFO_RAW("HW arch: %s\n", px4_board_name()); -#if defined(BOARD_HAS_VERSIONING) + +#if defined(BOARD_HAS_HW_SPLIT_VERSIONING) + char sbase[14] = "NA"; + char sfmum[14] = "NA"; + int base = GET_HW_BASE_ID(); + int fmu = GET_HW_FMUM_ID(); + + if (base >= 0) { + snprintf(sbase, sizeof(sbase), "0x%0" STRINGIFY(HW_INFO_VER_DIGITS) "X", base); + } + + if (fmu >= 0) { + snprintf(sfmum, sizeof(sfmum), "0x%0" STRINGIFY(HW_INFO_REV_DIGITS) "X", fmu); + } + + PX4_INFO_RAW("HW type: %s\n", strlen(HW_INFO_INIT_PREFIX) ? HW_INFO_INIT_PREFIX : "NA"); + PX4_INFO_RAW("HW FMUM ID: %s\n", sfmum); + PX4_INFO_RAW("HW BASE ID: %s\n", sbase); +#elif defined(BOARD_HAS_VERSIONING) char vb[14] = "NA"; char rb[14] = "NA"; int v = px4_board_hw_version();