diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index d9ff5cce2e9b..05d73d96f78f 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -45,6 +45,7 @@ jobs: matek_h743-slim, modalai_fc-v1, modalai_fc-v2, + mro_ctrl-zero-classic, mro_ctrl-zero-f7, mro_ctrl-zero-f7-oem, mro_ctrl-zero-h7, diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6bad6ee9ed07..61ef182f3ad0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -183,7 +183,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 @@ -287,12 +287,9 @@ else . $FCONFIG fi - # - # Start IO for PWM output or RC input if enabled - # - if param compare -s SYS_USE_IO 1 + if px4io supported then - # Check if PX4IO present and update firmware if needed. + # Check if PX4IO present and update firmware if needed. if [ -f $IOFW ] then if ! px4io checkcrc ${IOFW} @@ -314,12 +311,12 @@ else tune_control stop fi fi - fi - if ! px4io start - then - echo "PX4IO start failed" - set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + if ! px4io start + then + echo "PX4IO start failed" + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + fi fi fi diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 7a26ee01c8ee..52d098101df1 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -22,21 +22,14 @@ 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 -if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007 -then - param set-default SYS_USE_IO 0 -else - param set-default SYS_USE_IO 1 -fi - safety_button start diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index a2b993f928d1..e120ba6017d2 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ set HAVE_PM2 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 fi @@ -47,7 +47,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 @@ -59,7 +59,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 b77c240f15ba..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 // migrate to Split +#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 @@ -247,7 +239,6 @@ /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -#define BOARD_PWM_FREQ 1024000 #define GPIO_FMU_CH1 /* PI0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0) #define GPIO_FMU_CH2 /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12) 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 3ece10aeca4f..6e89d3d1f89a 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 @@ -92,10 +95,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/ark/fmu-v6x/src/spix_sync.c b/boards/ark/fmu-v6x/src/spix_sync.c index 2bda38696267..056e38e75f74 100644 --- a/boards/ark/fmu-v6x/src/spix_sync.c +++ b/boards/ark/fmu-v6x/src/spix_sync.c @@ -82,9 +82,7 @@ #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) #define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) -#if !defined(BOARD_PWM_FREQ) -#define BOARD_PWM_FREQ 1000000 -#endif +#define BOARD_SPIX_SYNC_PWM_FREQ 1024000 unsigned spix_sync_timer_get_period(unsigned timer) @@ -129,11 +127,11 @@ static void spix_sync_timer_init_timer(unsigned timer, unsigned rate) * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. */ - rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; + rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1; /* configure the timer to update at the desired rate */ - rARR(timer) = (BOARD_PWM_FREQ / rate) - 1; + rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1; /* generate an update event; reloads the counter and all registers */ rEGR(timer) = GTIM_EGR_UG; diff --git a/boards/cubepilot/cubeorange/init/rc.board_defaults b/boards/cubepilot/cubeorange/init/rc.board_defaults index 8ae2ed458b0e..7f961cd33e71 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_defaults +++ b/boards/cubepilot/cubeorange/init/rc.board_defaults @@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0 param set-default -s SENS_TEMP_ID 2621474 -param set-default SYS_USE_IO 1 - set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_defaults b/boards/cubepilot/cubeorangeplus/init/rc.board_defaults index 8ae2ed458b0e..7f961cd33e71 100644 --- a/boards/cubepilot/cubeorangeplus/init/rc.board_defaults +++ b/boards/cubepilot/cubeorangeplus/init/rc.board_defaults @@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0 param set-default -s SENS_TEMP_ID 2621474 -param set-default SYS_USE_IO 1 - set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/cubepilot/cubeyellow/init/rc.board_defaults b/boards/cubepilot/cubeyellow/init/rc.board_defaults index 54705e650d4f..6f39beb0c8ec 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_defaults +++ b/boards/cubepilot/cubeyellow/init/rc.board_defaults @@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 17 # Disable IMU thermal control param set-default SENS_EN_THERMAL 0 -param set-default SYS_USE_IO 1 - set IOFW "/etc/extras/cubepilot_io-v2_default.bin" diff --git a/boards/holybro/durandal-v1/init/rc.board_defaults b/boards/holybro/durandal-v1/init/rc.board_defaults index 2a3b2c0a7b38..49254efb15a4 100644 --- a/boards/holybro/durandal-v1/init/rc.board_defaults +++ b/boards/holybro/durandal-v1/init/rc.board_defaults @@ -11,5 +11,3 @@ param set-default BAT2_A_PER_V 36.367515152 # Enable IMU thermal control param set-default SENS_EN_THERMAL 1 - -param set-default SYS_USE_IO 1 diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 713c736d5066..a9f38ba78c98 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7/init/rc.board_defaults b/boards/holybro/kakuteh7/init/rc.board_defaults index fa014c6b485b..47dea71be617 100644 --- a/boards/holybro/kakuteh7/init/rc.board_defaults +++ b/boards/holybro/kakuteh7/init/rc.board_defaults @@ -22,13 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281 # Select the Generic 250 Racer by default param set-default SYS_AUTOSTART 4050 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 - +# use EKF2 without mag param set-default SYS_HAS_MAG 0 +# and enable gravity fusion +param set-default EKF2_IMU_CONTROL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 diff --git a/boards/holybro/kakuteh7/src/spi.cpp b/boards/holybro/kakuteh7/src/spi.cpp index 1ff5b7f35c1c..e0f4db0edd6e 100644 --- a/boards/holybro/kakuteh7/src/spi.cpp +++ b/boards/holybro/kakuteh7/src/spi.cpp @@ -43,7 +43,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), }), initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), }), diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index b019b753f90f..3310e79fbb6a 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -32,6 +32,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index d326a787978c..9c060d844224 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/pix32v5/init/rc.board_defaults b/boards/holybro/pix32v5/init/rc.board_defaults index 94c582821480..c2300a50a770 100644 --- a/boards/holybro/pix32v5/init/rc.board_defaults +++ b/boards/holybro/pix32v5/init/rc.board_defaults @@ -9,7 +9,5 @@ param set-default BAT2_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 param set-default BAT2_A_PER_V 36.367515152 -param set-default SYS_USE_IO 1 - rgbled_pwm start safety_button start diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index f88c94880fda..c16095b6a096 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -1,9 +1,11 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m7" -CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" -CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" -CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" -CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0" CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y @@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y @@ -96,4 +99,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_defaults b/boards/mro/ctrl-zero-classic/init/rc.board_defaults index 32117f4a5886..0495329e1a47 100644 --- a/boards/mro/ctrl-zero-classic/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-classic/init/rc.board_defaults @@ -6,4 +6,5 @@ param set-default BAT1_V_DIV 10.1 param set-default BAT1_A_PER_V 17 -safety_button start +param set-default GPS_2_CONFIG 202 +param set-default TEL_FRSKY_CONFIG 103 diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_sensors b/boards/mro/ctrl-zero-classic/init/rc.board_sensors index 111e3eb0440b..46f488794516 100644 --- a/boards/mro/ctrl-zero-classic/init/rc.board_sensors +++ b/boards/mro/ctrl-zero-classic/init/rc.board_sensors @@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start # Interal DPS310 (barometer) dps310 -s -b 2 start + +safety_button start diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig index c8a5905f548c..241e130b4cf2 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig @@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H743ZI=y CONFIG_ARCH_CHIP_STM32H7=y CONFIG_ARCH_INTERRUPTSTACK=768 CONFIG_ARMV7M_BASEPRI_WAR=y diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h index 9b1ae6342d33..0df441c6a778 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h @@ -222,6 +222,9 @@ /* UART/USART */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */ +#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */ + #define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ #define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ #define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ @@ -235,9 +238,6 @@ #define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ #define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ -#define GPIO_USART6_TX 0 /* USART6 is RX-only */ -#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ - #define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ #define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ @@ -268,13 +268,14 @@ #define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ #define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ +#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */ +#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */ +#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */ + /* I2C */ #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ -#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ -#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ - -#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ -#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig index e5169e78ede7..df9639f35251 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig @@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H743ZI=y CONFIG_ARCH_CHIP_STM32H7=y CONFIG_ARCH_INTERRUPTSTACK=768 CONFIG_ARCH_STACKDUMP=y @@ -190,7 +190,6 @@ CONFIG_STM32H7_DMA2=y CONFIG_STM32H7_DMACAPABLE=y CONFIG_STM32H7_FLOWCONTROL_BROKEN=y CONFIG_STM32H7_I2C1=y -CONFIG_STM32H7_I2C3=y CONFIG_STM32H7_I2C4=y CONFIG_STM32H7_I2C_DYNTIMEO=y CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 @@ -210,17 +209,20 @@ CONFIG_STM32H7_SPI2=y CONFIG_STM32H7_SPI5=y CONFIG_STM32H7_SPI5_DMA=y CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI6=y +CONFIG_STM32H7_TIM15=y +CONFIG_STM32H7_TIM16=y CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM2=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y CONFIG_STM32H7_UART4=y CONFIG_STM32H7_UART7=y CONFIG_STM32H7_UART8=y -CONFIG_STM32H7_USART2=y -CONFIG_STM32H7_USART3=y -CONFIG_STM32H7_USART6=y CONFIG_STM32H7_USART_BREAKS=y CONFIG_STM32H7_USART_INVERT=y CONFIG_STM32H7_USART_SINGLEWIRE=y @@ -250,9 +252,6 @@ CONFIG_USART3_IFLOWCONTROL=y CONFIG_USART3_OFLOWCONTROL=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=3000 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_TXBUFSIZE=1500 CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/mro/ctrl-zero-classic/src/board_config.h b/boards/mro/ctrl-zero-classic/src/board_config.h index dfb0dfcb1a78..d40eb1e56ffc 100644 --- a/boards/mro/ctrl-zero-classic/src/board_config.h +++ b/boards/mro/ctrl-zero-classic/src/board_config.h @@ -45,95 +45,111 @@ #include /* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ -#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) -#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) +#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) #define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) +#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) + #define BOARD_HAS_CONTROL_STATUS_LEDS 1 #define BOARD_OVERLOAD_LED LED_RED #define BOARD_ARMED_STATE_LED LED_BLUE /* ADC channels */ #define PX4_ADC_GPIO \ - /* PA2 */ GPIO_ADC12_INP14, \ + /* PC4 */ GPIO_ADC12_INP4, \ /* PA3 */ GPIO_ADC12_INP15, \ /* PA4 */ GPIO_ADC12_INP18, \ - /* PC1 */ GPIO_ADC123_INP11 + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC5 */ GPIO_ADC12_INP8, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PB1 */ GPIO_ADC12_INP5 /* Define Channel numbers must match above GPIO pins */ -#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */ #define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ #define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ -#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ +#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */ +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */ +#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */ +#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */ #define ADC_CHANNELS \ ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ (1 << ADC_SCALED_V5_CHANNEL) | \ - (1 << ADC_RC_RSSI_CHANNEL)) + (1 << ADC_RC_RSSI_CHANNEL) | \ + (1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \ + (1 << ADC_AUX1_VOLTAGE_CHANNEL) | \ + (1 << ADC_AUX2_VOLTAGE_CHANNEL)) /* HW has to large of R termination on ADC todo:change when HW value is chosen */ #define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) /* CAN Silence: Silent mode control */ -#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) -#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) +#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11) +#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 12 /* Power supply control and monitoring GPIOs */ -#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ #define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + /* Define True logic Power Control in arch agnostic form */ #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) #define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) /* Tone alarm output */ -#define TONE_ALARM_TIMER 2 /* timer 2 */ -#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ +#define TONE_ALARM_TIMER 16 /* timer 16 */ +#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */ -#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) +#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6) #define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 -#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 +#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2 /* USB OTG FS */ #define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ #define HRT_TIMER 3 /* use timer3 for the HRT */ -#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ +#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */ -#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ -#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 +#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */ +#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3 /* RC Serial port */ -#define RC_SERIAL_PORT "/dev/ttyS3" +#define RC_SERIAL_PORT "/dev/ttyS5" -#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) /* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ -#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) -/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */ #define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ +/* No PX4IO processor present */ +#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0 + /* Power switch controls ******************************************************/ #define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) /* * Board has a separate RC_IN * - * GPIO PPM_IN on PB0 T3CH3 + * GPIO PPM_IN on PC7 T3CH2 * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output */ -#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) #define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ #define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) @@ -156,7 +172,7 @@ #define BOARD_HAS_STATIC_MANIFEST 1 -#define BOARD_NUM_IO_TIMERS 5 +#define BOARD_NUM_IO_TIMERS 6 #define BOARD_ENABLE_CONSOLE_BUFFER @@ -171,9 +187,12 @@ GPIO_CAN2_SILENT_S0, \ GPIO_nPOWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_LEVEL_SHIFTER_OE, \ GPIO_TONE_ALARM_IDLE, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_OTGFS_VBUS, \ + GPIO_BTN_SAFETY, \ + GPIO_LED_SAFETY, \ } __BEGIN_DECLS diff --git a/boards/mro/ctrl-zero-classic/src/i2c.cpp b/boards/mro/ctrl-zero-classic/src/i2c.cpp index 1b8927c69939..49c9ea1c7efd 100644 --- a/boards/mro/ctrl-zero-classic/src/i2c.cpp +++ b/boards/mro/ctrl-zero-classic/src/i2c.cpp @@ -35,6 +35,5 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusExternal(1), - initI2CBusExternal(3), initI2CBusExternal(4), }; diff --git a/boards/mro/ctrl-zero-classic/src/spi.cpp b/boards/mro/ctrl-zero-classic/src/spi.cpp index 4a4c3502bbd9..674ae3f09b2f 100644 --- a/boards/mro/ctrl-zero-classic/src/spi.cpp +++ b/boards/mro/ctrl-zero-classic/src/spi.cpp @@ -37,7 +37,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}), initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), }, {GPIO::PortE, GPIO::Pin3}), initSPIBus(SPI::Bus::SPI2, { @@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { }), initSPIBus(SPI::Bus::SPI5, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}), }), }; diff --git a/boards/mro/ctrl-zero-classic/src/timer_config.cpp b/boards/mro/ctrl-zero-classic/src/timer_config.cpp index 23bb7a1decc7..50760e387bba 100644 --- a/boards/mro/ctrl-zero-classic/src/timer_config.cpp +++ b/boards/mro/ctrl-zero-classic/src/timer_config.cpp @@ -33,6 +33,28 @@ #include +/* Timer allocation + * + * TIM1_CH4 T FMU_CH1 + * TIM1_CH3 T FMU_CH2 + * TIM1_CH2 T FMU_CH3 + * TIM1_CH1 T FMU_CH4 + * + * TIM4_CH2 T FMU_CH5 + * TIM4_CH3 T FMU_CH6 + * TIM2_CH3 T FMU_CH7 + * TIM2_CH1 T FMU_CH8 + * + * TIM2_CH4 T FMU_CH9 + * TIM15_CH1 T FMU_CH10 + * + * TIM8_CH1 T FMU_CH11 + * + * TIM4_CH4 T FMU_CH12 + * + * TIM16_CH1 T BUZZER - Driven by other driver + */ + constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { initIOTimer(Timer::Timer1, DMA{DMA::Index1}), initIOTimer(Timer::Timer4, DMA{DMA::Index1}), diff --git a/boards/mro/x21-777/init/rc.board_defaults b/boards/mro/x21-777/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/mro/x21-777/init/rc.board_defaults +++ b/boards/mro/x21-777/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/mro/x21/init/rc.board_defaults b/boards/mro/x21/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/mro/x21/init/rc.board_defaults +++ b/boards/mro/x21/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v2/init/rc.board_defaults b/boards/px4/fmu-v2/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/px4/fmu-v2/init/rc.board_defaults +++ b/boards/px4/fmu-v2/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v3/init/rc.board_defaults b/boards/px4/fmu-v3/init/rc.board_defaults index e9fecd78a833..5d576dd74711 100644 --- a/boards/px4/fmu-v3/init/rc.board_defaults +++ b/boards/px4/fmu-v3/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v4pro/init/rc.board_defaults b/boards/px4/fmu-v4pro/init/rc.board_defaults index 42378a19d727..8dd9df28d5a2 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_defaults +++ b/boards/px4/fmu-v4pro/init/rc.board_defaults @@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 26.4 param set-default EKF2_MULTI_IMU 2 param set-default SENS_IMU_MODE 0 -param set-default SYS_USE_IO 1 - set LOGGER_BUF 64 diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults index 83c1976b74be..d7f8b05bd811 100644 --- a/boards/px4/fmu-v5/init/rc.board_defaults +++ b/boards/px4/fmu-v5/init/rc.board_defaults @@ -9,13 +9,6 @@ param set-default BAT2_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 param set-default BAT2_A_PER_V 36.367515152 -if ver hwtypecmp V5004000 V5006000 -then - param set-default SYS_USE_IO 0 -else - param set-default SYS_USE_IO 1 -fi - if ver hwtypecmp V5005000 V5005002 V5006000 V5006002 then # CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index b9a040d9a3a0..2cfeeacb659e 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -16,8 +16,6 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -param set-default SYS_USE_IO 1 - safety_button start set LOGGER_BUF 8 diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index 0d69e36794f6..f51fd6eb2a03 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 4f7c1d078de5..a54eab13ed4a 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,51 +49,50 @@ then fi fi -if ver hwtypecmp V5X000000 V5X000001 V5X000002 V5X001000 V5X004000 V5X004001 V5X004002 V5X005001 V5X005002 +if ver hwbasecmp 008 009 00a 010 011 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 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 - + 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) @@ -105,7 +104,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 def8804acf95..a129a7cad1f4 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -41,7 +41,6 @@ add_library(drivers_board can.c init.cpp led.c - manifest.c mtd.cpp sdio.c timer_config.cpp diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 22079f018d2a..1a02ff778b48 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -180,31 +180,17 @@ /* HW Version and Revision drive signals Default to 1 to detect */ -#define BOARD_HAS_HW_VERSIONING // migrate to Split +#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 fc1e97d2c8b1..d92a4d7aacdf 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 @@ -120,10 +123,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-v6c/init/rc.board_defaults b/boards/px4/fmu-v6c/init/rc.board_defaults index 6418e5836d8b..f7dd26972135 100644 --- a/boards/px4/fmu-v6c/init/rc.board_defaults +++ b/boards/px4/fmu-v6c/init/rc.board_defaults @@ -9,5 +9,3 @@ param set-default BAT2_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 param set-default BAT2_A_PER_V 36.367515152 - -param set-default SYS_USE_IO 1 diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index cb9b676b6856..67cf0ce0b80a 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -19,22 +19,25 @@ CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_COMMON_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index ba812e4b7bfd..f6e71a14caee 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -16,6 +16,5 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 -param set-default SYS_USE_IO 1 safety_button start diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index e59fb0c938f5..06554a688de8 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ set HAVE_PM2 yes -if ver hwtypecmp V6X005000 V6X005001 V6X005003 V6X005004 +if mft query -q -k MFT -s MFT_PM2 -v 0 then set HAVE_PM2 no fi @@ -48,57 +48,74 @@ then fi -if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 +# Keep nesting shallow +if ver hwtypecmp V6X006 V6X008 then - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start + if ver hwtypecmp V6X006 + then + # Internal SPI bus ICM45686 + adis16470 -s -R 0 start + iim42652 -s -R 6 start + icm45686 -s -R 10 start + else + # Internal SPI bus 3x ICM45686 + icm45686 -b 3 -s -R 0 start + icm45686 -b 2 -s -R 0 start + icm45686 -b 1 -s -R 10 start + fi else - # Internal SPI BMI088 - if ver hwtypecmp V6X009010 V6X010010 + if ver hwtypecmp V6X004 then - bmi088 -A -R 6 -s start - bmi088 -G -R 6 -s start + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start else - if ver hwtypecmp V6X000010 + # Internal SPI BMI088 + if ver hwbasecmp 009 010 then - bmi088 -A -R 0 -s start - bmi088 -G -R 0 -s start + bmi088 -A -R 6 -s start + bmi088 -G -R 6 -s start else - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + if ver hwtypecmp V6X010 + then + bmi088 -A -R 0 -s start + bmi088 -G -R 0 -s start + else + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + fi fi fi -fi -# Internal SPI bus ICM42688p -if ver hwtypecmp V6X009010 V6X010010 -then - icm42688p -R 12 -s start -else - if ver hwtypecmp V6X000010 + # Internal SPI bus ICM42688p + if ver hwbasecmp 009 010 then - icm42688p -R 14 -s start + icm42688p -R 12 -s start else - icm42688p -R 6 -s start + if ver hwtypecmp V6X010 + then + icm42688p -R 14 -s start + else + icm42688p -R 6 -s start + fi fi -fi -if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 -then - # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 10 -s start -else - if ver hwtypecmp V6X009010 V6X010010 + if ver hwtypecmp V6X003 V6X004 then - icm20602 -R 6 -s start + # Internal SPI bus ICM-42670-P (hard-mounted) + icm42670p -R 10 -s start else - # Internal SPI bus ICM-20649 (hard-mounted) - icm20649 -R 14 -s start + if ver hwbasecmp 009 010 + then + icm20602 -R 6 -s start + else + # Internal SPI bus ICM-20649 (hard-mounted) + icm20649 -R 14 -s start + fi fi fi # Internal magnetometer on I2c -if ver hwtypecmp V6X002001 +if ver hwtypecmp V6X001 then rm3100 -I -b 4 start else @@ -112,7 +129,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 + if ver hwtypecmp V6X001 V6X006 V6X008 then icp201xx -I -a 0x64 start else @@ -121,7 +138,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/nuttx-config/include/board.h b/boards/px4/fmu-v6x/nuttx-config/include/board.h index eb3004183b02..cd1f7120bec3 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board.h @@ -380,7 +380,9 @@ #define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ #define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ // GPIO_UART5_RTS no remap /* PC8 */ -// GPIO_UART5_CTS No remap /* PC9 */ +#undef GPIO_UART5_CTS +#define GPIO_UART5_CTS ((GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9) | GPIO_PULLDOWN) /* PC9 */ + #define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ #define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ 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 fecd7efc9c96..7cad5497ed2b 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -208,36 +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 11 // Rev 0 and Rev 3,4 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 7 // 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 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 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 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 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 313d49f9bf24..000000000000 --- a/boards/px4/fmu-v6x/src/manifest.c +++ /dev/null @@ -1,205 +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 - {V6X40, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // HB CM4 base - {V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 HB CM4 base - {V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 - {V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 - {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini - {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini - {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 - {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 - {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO - {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 - {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 - {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 fc1e97d2c8b1..d92a4d7aacdf 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 @@ -120,10 +123,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 d3bbdb9315f8..8c633dd78f14 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,31 +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(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}), @@ -108,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}), @@ -132,31 +108,7 @@ 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(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}), @@ -179,84 +131,15 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - // never shipped - //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(V6X04, { - 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(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, { + initSPIFmumID(V6X_6, { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + initSPIDevice(DRV_IMU_DEVTYPE_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_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + 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_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + 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 @@ -270,16 +153,16 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), - initSPIHWVersion(V6X0910, { + + initSPIFmumID(V6X_8, { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + 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_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, 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}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices @@ -293,7 +176,9 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), - initSPIHWVersion(V6X1010, { + + + 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}), @@ -316,6 +201,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/sky-drones/smartap-airlink/init/rc.board_defaults b/boards/sky-drones/smartap-airlink/init/rc.board_defaults index c581f8c741f9..212c72657f44 100644 --- a/boards/sky-drones/smartap-airlink/init/rc.board_defaults +++ b/boards/sky-drones/smartap-airlink/init/rc.board_defaults @@ -25,8 +25,6 @@ param set-default BAT_V_OFFS_CURR 0.413 # Disable safety switch param set-default CBRK_IO_SAFETY 22027 -param set-default SYS_USE_IO 1 - safety_button start set LOGGER_BUF 32 diff --git a/boards/thepeach/k1/init/rc.board_defaults b/boards/thepeach/k1/init/rc.board_defaults index 5d8a3c182ba1..f22fa838f49e 100644 --- a/boards/thepeach/k1/init/rc.board_defaults +++ b/boards/thepeach/k1/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 - -param set-default SYS_USE_IO 1 diff --git a/boards/thepeach/r1/init/rc.board_defaults b/boards/thepeach/r1/init/rc.board_defaults index 5d8a3c182ba1..f22fa838f49e 100644 --- a/boards/thepeach/r1/init/rc.board_defaults +++ b/boards/thepeach/r1/init/rc.board_defaults @@ -5,5 +5,3 @@ param set-default BAT1_V_DIV 18.1 param set-default BAT1_A_PER_V 36.367515152 - -param set-default SYS_USE_IO 1 diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c index d0570708a7ca..ba4835a5b13a 100644 --- a/platforms/common/pab_manifest.c +++ b/platforms/common/pab_manifest.c @@ -315,19 +315,19 @@ static const px4_hw_mft_item_t base_configuration_9[] = { // 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 + {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 + {HW_BASE_ID(0x100), base_configuration_0, arraySize(base_configuration_0)}, // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0 }; /************************************************************************************ 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/drivers/imu/invensense/icm45686/ICM45686.cpp b/src/drivers/imu/invensense/icm45686/ICM45686.cpp index 07fdf6633a86..1bd2a1283450 100644 --- a/src/drivers/imu/invensense/icm45686/ICM45686.cpp +++ b/src/drivers/imu/invensense/icm45686/ICM45686.cpp @@ -522,7 +522,7 @@ void ICM45686::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); } else { - accel.dt = FIFO_TIMESTAMP_SCALING; + accel.dt = FIFO_SAMPLE_DT; } // 20 bit hires mode @@ -634,7 +634,7 @@ void ICM45686::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); } else { - gyro.dt = FIFO_TIMESTAMP_SCALING; + gyro.dt = FIFO_SAMPLE_DT; } // 20 bit hires mode diff --git a/src/drivers/imu/invensense/icm45686/ICM45686.hpp b/src/drivers/imu/invensense/icm45686/ICM45686.hpp index f370808772e1..efcc485b7152 100644 --- a/src/drivers/imu/invensense/icm45686/ICM45686.hpp +++ b/src/drivers/imu/invensense/icm45686/ICM45686.hpp @@ -70,7 +70,7 @@ class ICM45686 : public device::SPI, public I2CSPIDriver void exit_and_cleanup() override; // Sensor Configuration - static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured + static constexpr float FIFO_SAMPLE_DT{1e6f / 6400.f}; // 6400 Hz accel & gyro ODR configured static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; diff --git a/src/drivers/imu/invensense/icm45686/icm45686_main.cpp b/src/drivers/imu/invensense/icm45686/icm45686_main.cpp index 8cf88e059754..1951acd9895e 100644 --- a/src/drivers/imu/invensense/icm45686/icm45686_main.cpp +++ b/src/drivers/imu/invensense/icm45686/icm45686_main.cpp @@ -38,7 +38,7 @@ void ICM45686::print_usage() { - PRINT_MODULE_USAGE_NAME("icm42688p", "driver"); + PRINT_MODULE_USAGE_NAME("icm45686", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7b9d29970a99..19cec0be7fe1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -340,8 +340,7 @@ class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleIn (ParamInt) _param_rc_rssi_pwm_max, (ParamInt) _param_rc_rssi_pwm_min, (ParamInt) _param_sens_en_themal, - (ParamInt) _param_sys_hitl, - (ParamInt) _param_sys_use_io + (ParamInt) _param_sys_hitl ) }; @@ -472,7 +471,7 @@ int PX4IO::init() } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) { + if (_param_sys_hitl.get() <= 0) { _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); } @@ -1561,6 +1560,10 @@ int PX4IO::custom_command(int argc, char *argv[]) { const char *verb = argv[0]; + if (!strcmp(verb, "supported")) { + return 0; + } + if (!strcmp(verb, "checkcrc")) { if (is_running()) { PX4_ERR("io must be stopped"); @@ -1766,7 +1769,7 @@ Output driver communicating with the IO co-processor. extern "C" __EXPORT int px4io_main(int argc, char *argv[]) { if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_PX4IO)) { - PX4_ERR("PX4IO Not Supported"); + PX4_INFO("PX4IO Not Supported"); return -1; } return PX4IO::main(argc, argv); diff --git a/src/drivers/px4io/px4io_params.c b/src/drivers/px4io/px4io_params.c index ef1c1ee6ab92..b390bdb18a29 100644 --- a/src/drivers/px4io/px4io_params.c +++ b/src/drivers/px4io/px4io_params.c @@ -42,18 +42,6 @@ #include #include -/** - * Set usage of IO board - * - * Can be used to use a configure the use of the IO board. - * - * @value 0 IO PWM disabled (RC only) - * @value 1 IO enabled (RC & PWM) - * @reboot_required true - * @group System - */ -PARAM_DEFINE_INT32(SYS_USE_IO, 0); - /** * S.BUS out * diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 58473f866634..3349cb52e628 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2023 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 @@ -62,6 +62,7 @@ add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) +add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) add_subdirectory(systemlib EXCLUDE_FROM_ALL) @@ -71,6 +72,7 @@ add_subdirectory(terrain_estimation EXCLUDE_FROM_ALL) add_subdirectory(timesync EXCLUDE_FROM_ALL) add_subdirectory(tinybson EXCLUDE_FROM_ALL) add_subdirectory(tunes EXCLUDE_FROM_ALL) +add_subdirectory(variable_length_ringbuffer EXCLUDE_FROM_ALL) add_subdirectory(version EXCLUDE_FROM_ALL) add_subdirectory(weather_vane EXCLUDE_FROM_ALL) add_subdirectory(wind_estimator EXCLUDE_FROM_ALL) diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index 207d21fdd9ea..da995a420b2c 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -35,6 +35,8 @@ class Slice assert(y0 + Q <= N); } + Slice(const Slice &other) = default; + const Type &operator()(size_t i, size_t j) const { assert(i < P); @@ -52,6 +54,12 @@ class Slice return (*_data)(_x0 + i, _y0 + j); } + // Separate function needed otherwise the default copy constructor matches before the deep copy implementation + Slice &operator=(const Slice &other) + { + return this->operator=(other); + } + template Slice &operator=(const Slice &other) { diff --git a/src/lib/matrix/test/MatrixSliceTest.cpp b/src/lib/matrix/test/MatrixSliceTest.cpp index 7849259c65e5..9240e6b5a8c2 100644 --- a/src/lib/matrix/test/MatrixSliceTest.cpp +++ b/src/lib/matrix/test/MatrixSliceTest.cpp @@ -262,3 +262,12 @@ TEST(MatrixSliceTest, Slice) float O_check_data_12 [4] = {2.5, 3, 4, 5}; EXPECT_EQ(res_12, (SquareMatrix(O_check_data_12))); } + +TEST(MatrixSliceTest, XYAssignmentTest) +{ + Vector3f a(1, 2, 3); + Vector3f b(4, 5, 6); + // Assign first two elements from b to first two slot of a + a.xy() = b.xy(); + EXPECT_EQ(a, Vector3f(4, 5, 3)); +} diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp index c9369addd526..7301776113b6 100644 --- a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp +++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp @@ -61,12 +61,17 @@ class FunctionActuatorSet : public FunctionProviderBase int index = (int)(vehicle_command.param7 + 0.5f); if (index == 0) { - _data[0] = vehicle_command.param1; - _data[1] = vehicle_command.param2; - _data[2] = vehicle_command.param3; - _data[3] = vehicle_command.param4; - _data[4] = vehicle_command.param5; - _data[5] = vehicle_command.param6; + if (PX4_ISFINITE(vehicle_command.param1)) { _data[0] = vehicle_command.param1; } + + if (PX4_ISFINITE(vehicle_command.param2)) {_data[1] = vehicle_command.param2; } + + if (PX4_ISFINITE(vehicle_command.param3)) {_data[2] = vehicle_command.param3; } + + if (PX4_ISFINITE(vehicle_command.param4)) {_data[3] = vehicle_command.param4; } + + if (PX4_ISFINITE(vehicle_command.param5)) {_data[4] = vehicle_command.param5; } + + if (PX4_ISFINITE(vehicle_command.param6)) {_data[5] = vehicle_command.param6; } } } } diff --git a/src/lib/motion_planning/PositionSmoothing.cpp b/src/lib/motion_planning/PositionSmoothing.cpp index a5c68f04d173..4b3a427a217b 100644 --- a/src/lib/motion_planning/PositionSmoothing.cpp +++ b/src/lib/motion_planning/PositionSmoothing.cpp @@ -134,21 +134,20 @@ const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, co } // Get the crossing point using L1-style guidance - auto l1_point = _getL1Point(position, waypoints); - return {l1_point(0), l1_point(1), target(2)}; + return _getL1Point(position, waypoints); } -const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const +const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const { - const Vector2f pos_traj(_trajectory[0].getCurrentPosition(), - _trajectory[1].getCurrentPosition()); - const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero(); - const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0])); - const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); - const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest; + const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(), + _trajectory[2].getCurrentPosition()); + const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero(); + const Vector3f prev_to_pos(pos_traj - waypoints[0]); + const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target)); + const Vector3f closest_pt = waypoints[0] + prev_to_closest; // Compute along-track error using L1 distance and cross-track error - const float crosstrack_error = Vector2f(closest_pt - pos_traj).length(); + const float crosstrack_error = (closest_pt - pos_traj).length(); const float l1 = math::max(_target_acceptance_radius, 5.f); float alongtrack_error = 0.f; @@ -159,9 +158,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve } // Position of the point on the line where L1 intersect the line between the two waypoints - const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target; - - return l1_point; + return closest_pt + alongtrack_error * u_prev_to_target; } const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp index 73886d0124ed..61070d9dabd4 100644 --- a/src/lib/motion_planning/PositionSmoothing.hpp +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -438,7 +438,7 @@ class PositionSmoothing const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3], bool is_single_waypoint, const Vector3f &feedforward_velocity_setpoint); - const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; + const Vector3f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const; const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const; float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const; float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const; diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 5a5603d4cf3d..c201e0df5a5d 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -210,8 +210,6 @@ bool param_modify_on_import(bson_node_t node) } } - return false; - //2023-02-08: translate L1 parameters after removing l1 control { if (strcmp("RWTO_L1_PERIOD", node->name) == 0) { @@ -232,4 +230,6 @@ bool param_modify_on_import(bson_node_t node) return true; } } + + return false; } diff --git a/src/lib/ringbuffer/CMakeLists.txt b/src/lib/ringbuffer/CMakeLists.txt new file mode 100644 index 000000000000..78a26de16664 --- /dev/null +++ b/src/lib/ringbuffer/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +px4_add_library(ringbuffer + Ringbuffer.cpp +) + +target_include_directories(ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC RingbufferTest.cpp LINKLIBS ringbuffer) diff --git a/src/lib/ringbuffer/Ringbuffer.cpp b/src/lib/ringbuffer/Ringbuffer.cpp new file mode 100644 index 000000000000..64fc33d8361d --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + + + + +#include "Ringbuffer.hpp" + +#include +#include +#include + + +Ringbuffer::~Ringbuffer() +{ + deallocate(); +} + +bool Ringbuffer::allocate(size_t buffer_size) +{ + assert(_ringbuffer == nullptr); + + _size = buffer_size; + _ringbuffer = new uint8_t[_size]; + return _ringbuffer != nullptr; +} + +void Ringbuffer::deallocate() +{ + delete[] _ringbuffer; + _ringbuffer = nullptr; + _size = 0; +} + +size_t Ringbuffer::space_available() const +{ + if (_start > _end) { + return _start - _end - 1; + + } else { + return _start - _end - 1 + _size; + } +} + +size_t Ringbuffer::space_used() const +{ + if (_start <= _end) { + return _end - _start; + + } else { + // Potential wrap around. + return _end - _start + _size; + } +} + + +bool Ringbuffer::push_back(const uint8_t *buf, size_t buf_len) +{ + if (buf_len == 0 || buf == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + if (_start > _end) { + // Add after end up to start, no wrap around. + + // Leave one byte free so that start don't end up the same + // which signals empty. + const size_t available = _start - _end - 1; + + if (available < buf_len) { + return false; + } + + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + + } else { + // Add after end, maybe wrap around. + const size_t available = _start - _end - 1 + _size; + + if (available < buf_len) { + return false; + } + + const size_t remaining_packet_len = _size - _end; + + if (buf_len > remaining_packet_len) { + memcpy(&_ringbuffer[_end], buf, remaining_packet_len); + _end = 0; + + memcpy(&_ringbuffer[_end], buf + remaining_packet_len, buf_len - remaining_packet_len); + _end += buf_len - remaining_packet_len; + + } else { + memcpy(&_ringbuffer[_end], buf, buf_len); + _end += buf_len; + } + } + + return true; +} + +size_t Ringbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + if (_start == _end) { + // Empty + return 0; + } + + if (_start < _end) { + + // No wrap around. + size_t to_copy_len = math::min(_end - _start, buf_max_len); + + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + + return to_copy_len; + + } else { + // Potential wrap around. + size_t to_copy_len = _end - _start + _size; + + if (to_copy_len > buf_max_len) { + to_copy_len = buf_max_len; + } + + const size_t remaining_buf_len = _size - _start; + + if (to_copy_len > remaining_buf_len) { + + memcpy(buf, &_ringbuffer[_start], remaining_buf_len); + _start = 0; + memcpy(buf + remaining_buf_len, &_ringbuffer[_start], to_copy_len - remaining_buf_len); + _start += to_copy_len - remaining_buf_len; + + } else { + memcpy(buf, &_ringbuffer[_start], to_copy_len); + _start += to_copy_len; + } + + return to_copy_len; + } +} diff --git a/src/lib/ringbuffer/Ringbuffer.hpp b/src/lib/ringbuffer/Ringbuffer.hpp new file mode 100644 index 000000000000..541f322db947 --- /dev/null +++ b/src/lib/ringbuffer/Ringbuffer.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + + + +#pragma once + +#include +#include + + +// FIFO ringbuffer implementation. +// +// The ringbuffer can store up 1 byte less than allocated as +// start and end marker need to be one byte apart when the buffer +// is full, otherwise it would suddenly be empty. +// +// The buffer is not thread-safe. + +class Ringbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + Ringbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~Ringbuffer(); + + /* @brief Allocate ringbuffer + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Space available to copy bytes into + * + * @returns number of free bytes. + */ + size_t space_available() const; + + /* + * @brief Space used to copy data from + * + * @returns number of used bytes. + */ + size_t space_used() const; + + /* + * @brief Copy data into ringbuffer + * + * @param buf Pointer to buffer to copy from. + * @param buf_len Number of bytes to copy. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *buf, size_t buf_len); + + /* + * @brief Get data from ringbuffer + * + * @param buf Pointer to buffer where data can be copied into. + * @param max_buf_len Max number of bytes to copy. + * + * @returns 0 if buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + size_t _size {0}; + uint8_t *_ringbuffer {nullptr}; + size_t _start{0}; + size_t _end{0}; +}; diff --git a/src/lib/ringbuffer/RingbufferTest.cpp b/src/lib/ringbuffer/RingbufferTest.cpp new file mode 100644 index 000000000000..e5e0a7a57f8a --- /dev/null +++ b/src/lib/ringbuffer/RingbufferTest.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +#include +#include +#include + + +#include "Ringbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(Ringbuffer, AllocateAndDeallocate) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(Ringbuffer, PushATooBigMessage) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(Ringbuffer, PushAndPopOne) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + EXPECT_EQ(buf.space_used(), 20); + EXPECT_EQ(buf.space_available(), 79); + + // Get everything + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + // Nothing remaining + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(Ringbuffer, PushAndPopSeveral) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{90}; + data.paint(); + + // 9 little chunks in + for (unsigned i = 0; i < 9; ++i) { + EXPECT_TRUE(buf.push_back(data.buf() + i * 10, 10)); + } + + // 10 won't because of overhead inside the buffer + EXPECT_FALSE(buf.push_back(data.buf(), 10)); + + TempData out{90}; + // Take it back out in 2 big steps + EXPECT_EQ(buf.pop_front(out.buf(), 50), 50); + EXPECT_EQ(buf.pop_front(out.buf() + 50, 40), 40); + EXPECT_EQ(data, out); +} + +TEST(Ringbuffer, PushAndTryToPopMore) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData out1{80}; + EXPECT_EQ(buf.pop_front(out1.buf(), out1.size()), data1.size()); +} + +TEST(Ringbuffer, PushAndPopSeveralInterleaved) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(50); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + TempData out12{80}; + EXPECT_EQ(buf.pop_front(out12.buf(), out12.size()), out12.size()); + + TempData out12_ref{80}; + out12_ref.paint(); + EXPECT_EQ(out12_ref, out12); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), out3.size()), data3.size()); + EXPECT_EQ(data3, out3); +} + +TEST(Ringbuffer, PushEmpty) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(Ringbuffer, PopWithoutBuffer) +{ + Ringbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(Ringbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + Ringbuffer buf; + // Allocate 1 bytes more than the packet, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(21)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 38ed565a055d..8353d60fdd8b 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -52,10 +52,17 @@ using namespace time_literals; static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);} -void TECSAirspeedFilter::initialize(const float equivalent_airspeed) +void TECSAirspeedFilter::initialize(const float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available) { - _airspeed_state.speed = equivalent_airspeed; - _airspeed_state.speed_rate = 0.0f; + if (airspeed_sensor_available && PX4_ISFINITE(equivalent_airspeed)) { + _airspeed_state.speed = equivalent_airspeed; + + } else { + _airspeed_state.speed = equivalent_airspeed_trim; + } + + _airspeed_state.speed_rate = 0.f; } void TECSAirspeedFilter::update(const float dt, const Input &input, const Param ¶m, @@ -353,7 +360,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag) { - if (!flag.detect_underspeed_enabled) { + if (!flag.detect_underspeed_enabled || !flag.airspeed_enabled) { _ratio_undersped = 0.0f; return; } @@ -412,6 +419,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON); _pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment, _pitch_setpoint + pitch_increment); + _pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max); //Debug Output _debug_output.energy_balance_rate_estimate = seb_rate.estimate; @@ -654,7 +662,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo TECSAltitudeReferenceModel::AltitudeReferenceState current_state{.alt = altitude, .alt_rate = altitude_rate}; _altitude_reference_model.initialize(current_state); - _airspeed_filter.initialize(equivalent_airspeed); + _airspeed_filter.initialize(equivalent_airspeed, _airspeed_filter_param.equivalent_airspeed_trim, + _control_flag.airspeed_enabled); TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 5673342772e1..0d15b8a6c41f 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -88,8 +88,11 @@ class TECSAirspeedFilter * @brief Initialize filter * * @param[in] equivalent_airspeed is the equivalent airspeed in [m/s]. + * @param[in] equivalent_airspeed_trim is the equivalent airspeed trim (vehicle setting) in [m/s]. + * @param[in] airspeed_sensor_available boolean if the airspeed sensor is available. */ - void initialize(float equivalent_airspeed); + void initialize(float equivalent_airspeed, const float equivalent_airspeed_trim, + const bool airspeed_sensor_available); /** * @brief Update filter @@ -696,9 +699,9 @@ class TECS .max_climb_rate = 5.0f, .vert_accel_limit = 0.0f, .equivalent_airspeed_trim = 15.0f, - .tas_min = 3.0f, - .pitch_max = 5.0f, - .pitch_min = -5.0f, + .tas_min = 10.0f, + .pitch_max = 0.5f, + .pitch_min = -0.5f, .throttle_trim = 0.0f, .throttle_trim_adjusted = 0.f, .throttle_max = 1.0f, diff --git a/src/lib/variable_length_ringbuffer/CMakeLists.txt b/src/lib/variable_length_ringbuffer/CMakeLists.txt new file mode 100644 index 000000000000..2d2bf4706731 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +px4_add_library(variable_length_ringbuffer + VariableLengthRingbuffer.cpp +) + +target_link_libraries(variable_length_ringbuffer PRIVATE ringbuffer) + +target_include_directories(variable_length_ringbuffer PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_unit_gtest(SRC VariableLengthRingbufferTest.cpp LINKLIBS variable_length_ringbuffer) diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp new file mode 100644 index 000000000000..9b607e208b07 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + + + + +#include "VariableLengthRingbuffer.hpp" + +#include +#include + + +VariableLengthRingbuffer::~VariableLengthRingbuffer() +{ + deallocate(); +} + +bool VariableLengthRingbuffer::allocate(size_t buffer_size) +{ + return _ringbuffer.allocate(buffer_size); +} + +void VariableLengthRingbuffer::deallocate() +{ + _ringbuffer.deallocate(); +} + +bool VariableLengthRingbuffer::push_back(const uint8_t *packet, size_t packet_len) +{ + if (packet_len == 0 || packet == nullptr) { + // Nothing to add, we better don't try. + return false; + } + + size_t space_required = packet_len + sizeof(Header); + + if (space_required > _ringbuffer.space_available()) { + return false; + } + + Header header{static_cast(packet_len)}; + bool result = _ringbuffer.push_back(reinterpret_cast(&header), sizeof(header)); + assert(result); + + result = _ringbuffer.push_back(packet, packet_len); + assert(result); + + // In case asserts are commented out to prevent unused warnings. + (void)result; + + return true; +} + +size_t VariableLengthRingbuffer::pop_front(uint8_t *buf, size_t buf_max_len) +{ + if (buf == nullptr) { + // User needs to supply a valid pointer. + return 0; + } + + // Check next header + Header header; + + if (_ringbuffer.pop_front(reinterpret_cast(&header), sizeof(header)) < sizeof(header)) { + return 0; + } + + // We can't fit the packet into the user supplied buffer. + // This should never happen as the user has to supply a big // enough buffer. + assert(static_cast(header.len) <= buf_max_len); + + size_t bytes_read = _ringbuffer.pop_front(buf, header.len); + assert(bytes_read == header.len); + + return bytes_read; +} diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp new file mode 100644 index 000000000000..89f92eb24670 --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbuffer.hpp @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + + + +#pragma once + +#include +#include + + +// FIFO ringbuffer implementation for packets of variable length. +// +// The variable length is implemented using a 4 byte header +// containing a the length. +// +// The buffer is not thread-safe. + +class VariableLengthRingbuffer +{ +public: + /* @brief Constructor + * + * @note Does not allocate automatically. + */ + VariableLengthRingbuffer() = default; + + /* + * @brief Destructor + * + * Automatically calls deallocate. + */ + ~VariableLengthRingbuffer(); + + /* @brief Allocate ringbuffer + * + * @note The variable length requires 4 bytes + * of overhead per packet. + * + * @param buffer_size Number of bytes to allocate on heap. + * + * @returns false if allocation fails. + */ + bool allocate(size_t buffer_size); + + /* + * @brief Deallocate ringbuffer + * + * @note only required to deallocate and reallocate again. + */ + void deallocate(); + + /* + * @brief Copy packet into ringbuffer + * + * @param packet Pointer to packet to copy from. + * @param packet_len Length of packet. + * + * @returns true if packet could be copied into buffer. + */ + bool push_back(const uint8_t *packet, size_t packet_len); + + /* + * @brief Get packet from ringbuffer + * + * @note max_buf_len needs to be bigger equal to any pushed packet. + * + * @param buf Pointer to where next packet can be copied into. + * @param max_buf_len Max size of buf + * + * @returns 0 if packet is bigger than max_len or buffer is empty. + */ + size_t pop_front(uint8_t *buf, size_t max_buf_len); + +private: + struct Header { + uint32_t len; + }; + + Ringbuffer _ringbuffer {}; +}; diff --git a/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp new file mode 100644 index 000000000000..958f36947eda --- /dev/null +++ b/src/lib/variable_length_ringbuffer/VariableLengthRingbufferTest.cpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +#include +#include +#include + + +#include "VariableLengthRingbuffer.hpp" + +class TempData +{ +public: + TempData(size_t len) + { + _size = len; + _buf = new uint8_t[_size]; + } + + ~TempData() + { + delete[] _buf; + _buf = nullptr; + } + + uint8_t *buf() const + { + return _buf; + } + + size_t size() const + { + return _size; + } + + void paint(unsigned offset = 0) + { + for (size_t i = 0; i < _size; ++i) { + _buf[i] = (uint8_t)((i + offset) % UINT8_MAX); + } + } + +private: + uint8_t *_buf {nullptr}; + size_t _size{0}; + +}; + +bool operator==(const TempData &lhs, const TempData &rhs) +{ + if (lhs.size() != rhs.size()) { + return false; + } + + return memcmp(lhs.buf(), rhs.buf(), lhs.size()) == 0; +} + + +TEST(VariableLengthRingbuffer, AllocateAndDeallocate) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + buf.deallocate(); + + ASSERT_TRUE(buf.allocate(1000)); + // The second time we forget to clean up, but we expect no leak. +} + +TEST(VariableLengthRingbuffer, PushATooBigMessage) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{200}; + + // A message that doesn't fit should get rejected. + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); +} + +TEST(VariableLengthRingbuffer, PushAndPopOne) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + // Out buffer is the same size + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 20); + EXPECT_EQ(data, out); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + // Out buffer is supposedly bigger + TempData out2{20}; + EXPECT_EQ(buf.pop_front(out2.buf(), 21), 20); + EXPECT_EQ(data, out2); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + // Disabled because it doesn't work reliably. + // For some reason the abort works when tests are filtered using TESTFILTER + // but not when all tests are run. + // + // Out buffer is too small + // Asserts are disabled in release build + //TempData out3{19}; + //EXPECT_DEATH(buf.pop_front(out3.buf(), out3.size()), ".*"); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveral) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data{20}; + data.paint(); + + // 4 should fit + for (unsigned i = 0; i < 4; ++i) { + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + } + + // 5 won't because of overhead + EXPECT_FALSE(buf.push_back(data.buf(), data.size())); + + // Take 4 back out + for (unsigned i = 0; i < 4; ++i) { + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), data.size()); + EXPECT_EQ(data, out); + } + + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushAndPopSeveralVariableSize) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + TempData data1{50}; + data1.paint(); + EXPECT_TRUE(buf.push_back(data1.buf(), data1.size())); + + TempData data2{30}; + data2.paint(42); + EXPECT_TRUE(buf.push_back(data2.buf(), data2.size())); + + // Supposedly more space + TempData out1{50}; + EXPECT_EQ(buf.pop_front(out1.buf(), 100), data1.size()); + EXPECT_EQ(data1, out1); + + TempData data3{50}; + data3.paint(33); + EXPECT_TRUE(buf.push_back(data3.buf(), data3.size())); + + // Supposedly more space + TempData out2{30}; + EXPECT_EQ(buf.pop_front(out2.buf(), 100), data2.size()); + EXPECT_EQ(data2, out2); + + // Supposedly more space + TempData out3{50}; + EXPECT_EQ(buf.pop_front(out3.buf(), 100), data3.size()); + EXPECT_EQ(data3, out3); + + TempData out4{100}; + EXPECT_EQ(buf.pop_front(out4.buf(), out4.size()), 0); +} + +TEST(VariableLengthRingbuffer, PushEmpty) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); +} + +TEST(VariableLengthRingbuffer, PopWithoutBuffer) +{ + VariableLengthRingbuffer buf; + ASSERT_TRUE(buf.allocate(100)); + + EXPECT_FALSE(buf.push_back(nullptr, 0)); + + TempData data{50}; + data.paint(); + + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + + + EXPECT_EQ(buf.pop_front(nullptr, 50), 0); +} + +TEST(VariableLengthRingbuffer, EmptyAndNoSpaceForHeader) +{ + // Addressing a corner case where start and end are at the end + // and the same. + + VariableLengthRingbuffer buf; + // Allocate 4+1 bytes more than the packet, 4 for the header, 1 for the start/end logic. + ASSERT_TRUE(buf.allocate(25)); + + { + TempData data{20}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{20}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } + + { + TempData data{10}; + data.paint(); + EXPECT_TRUE(buf.push_back(data.buf(), data.size())); + TempData out{10}; + EXPECT_EQ(buf.pop_front(out.buf(), out.size()), out.size()); + EXPECT_EQ(data, out); + } +} diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1895a945c88b..a2a14c7fa0f4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1367,6 +1367,10 @@ Commander::handle_command(const vehicle_command_s &cmd) answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); break; + case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + break; + case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: @@ -1403,7 +1407,6 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW: case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: - case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE: case vehicle_command_s::VEHICLE_CMD_DO_WINCH: case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 37a0cf1624fa..aac00da78bde 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -229,6 +229,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; } if (_saturation_flags.pitch_pos) { @@ -236,6 +239,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; } if (_saturation_flags.yaw_pos) { @@ -243,6 +249,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; } if (_saturation_flags.thrust_pos) { @@ -250,5 +259,8 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index 0d8daee7dea9..453da560b421 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -37,8 +37,12 @@ #include "range_finder_consistency_check.hpp" -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us) +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us) { + if (horizontal_motion) { + _time_last_horizontal_motion = time_us; + } + const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; if ((_time_last_update_us == 0) @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { + if (fabsf(vz) < _min_vz_for_valid_consistency) { + // We can only check consistency if there is vertical motion + return; + } + if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { - _is_kinematically_consistent = false; - _time_last_inconsistent_us = time_us; + if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { + _is_kinematically_consistent = false; + _time_last_inconsistent_us = time_us; + } } else { - if (fabsf(vz) > _min_vz_for_valid_consistency && _test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { + if (_test_ratio < 1.f + && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { _is_kinematically_consistent = true; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp index 1498a4aa6cce..fa185f662955 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -47,7 +47,7 @@ class RangeFinderConsistencyCheck final RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; - void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us); + void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us); void setGate(float gate) { _gate = gate; } @@ -71,6 +71,7 @@ class RangeFinderConsistencyCheck final bool _is_kinematically_consistent{true}; uint64_t _time_last_inconsistent_us{}; + uint64_t _time_last_horizontal_motion{}; static constexpr float _signed_test_ratio_tau = 2.f; diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 88a9cb719711..fa2a365f356a 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion() const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - // Run the kinematic consistency check when not moving horizontally - if (_control_status.flags.in_air && !_control_status.flags.fixed_wing - && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + if (_control_status.flags.in_air) { + const bool horizontal_motion = _control_status.flags.fixed_wing + || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P(4, 4) + P(5, 5), 0.1f)); const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); const float var = sq(_params.range_noise) + dist_dependant_var; _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _time_delayed_us); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), horizontal_motion, _time_delayed_us); } } else { diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 657b989f4882..f368e1f615c9 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -199,7 +199,6 @@ void Ekf::resetHaglRng() _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; _time_last_hagl_fuse = _time_delayed_us; - _time_last_healthy_rng_data = 0; } void Ekf::stopHaglRngFusion() diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 4a47965592a3..243058d0ab44 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,5 +1,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 @@ -132,7 +132,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 12990000,0.7,0.0012,-0.014,0.71,-0.0078,0.0067,-0.03,-0.0011,0.0013,-3.7e+02,-1.2e-05,-5.9e-05,-2.3e-06,6.8e-06,9.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.062,0.062,0.025,0.049,0.049,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0 13090000,0.7,0.0012,-0.014,0.71,-0.0084,0.0069,-0.03,-0.0019,0.002,-3.7e+02,-1.2e-05,-5.8e-05,-2.8e-06,6.4e-06,9.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00021,0.00021,9.3e-05,0.071,0.071,0.025,0.057,0.057,0.052,3.8e-09,3.9e-09,2e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0 13190000,0.7,0.00095,-0.014,0.71,0.00044,0.0063,-0.027,0.0043,0.0013,-3.7e+02,-1.1e-05,-5.9e-05,-2.8e-06,5.1e-06,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.058,0.058,0.025,0.049,0.049,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 -13290000,0.7,0.00095,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 +13290000,0.7,0.00096,-0.014,0.71,0.00026,0.0072,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,3.9e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,1.9e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2e-06,2.6e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0 13490000,0.7,0.00084,-0.014,0.71,0.0006,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-1.7e-06,1.8e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.0002,0.0002,9.3e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,1.8e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0 13590000,0.7,0.00078,-0.014,0.71,0.0008,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-1.9e-06,1.7e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,0.00019,0.00019,9.3e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,1.8e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index 88b4777030ae..cddd19097efb 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,5 +1,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-3.3e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 190000,0.98,-0.0092,-0.013,0.21,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.5e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00081,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 290000,0.98,-0.0092,-0.013,0.21,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.6e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00068,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 @@ -347,5 +347,5 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0096,-0.09,0.071,-0.011,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00017,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.053,0.053,0.0059,0.051,0.051,0.032,2.6e-11,2.6e-11,8.1e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 34590000,0.98,-0.0069,-0.013,0.18,-0.014,-0.0053,0.71,0.073,-0.0089,-0.081,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3e-05,4.2e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 34690000,0.98,-0.0068,-0.012,0.18,-0.017,-0.0032,1.7,0.071,-0.0093,0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3.1e-05,4.2e-05,0.047,0.047,0.006,0.052,0.052,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -34790000,0.98,-0.0068,-0.012,0.18,-0.018,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 +34790000,0.98,-0.0068,-0.012,0.18,-0.019,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0039,3.6,0.07,-0.0065,0.5,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.043,0.043,0.0061,0.052,0.052,0.032,2.6e-11,2.6e-11,7.8e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 90d5dccb1feb..3508d6b7701b 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -127,7 +127,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if (stick_input || too_fast || !PX4_ISFINITE(_dist_to_bottom)) { // Stop using distance to ground _terrain_hold = false; - _terrain_follow = false; // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_dist_to_ground_lock) && PX4_ISFINITE(_dist_to_bottom)) { @@ -144,7 +143,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if (!stick_input && not_moving && PX4_ISFINITE(_dist_to_bottom)) { // Start using distance to ground _terrain_hold = true; - _terrain_follow = true; // Adjust the setpoint to maintain the same height error to reduce control transients if (PX4_ISFINITE(_position_setpoint(2))) { @@ -155,7 +153,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } - if ((_param_mpc_alt_mode.get() == 1 || _terrain_follow) && PX4_ISFINITE(_dist_to_bottom)) { + if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { // terrain following _terrainFollowing(apply_brake, stopped); // respect maximum altitude diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 0b9fbe72a3fb..be9eb278d38e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -75,6 +75,7 @@ class FlightTaskManualAltitude : public FlightTask StickYaw _stick_yaw{this}; bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data + bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_hold_max_z, @@ -121,8 +122,6 @@ class FlightTaskManualAltitude : public FlightTask void setGearAccordingToSwitch(); uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ - bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ - bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ float _min_distance_to_ground{(float)(-INFINITY)}; /**< min distance to ground constraint */ float _max_distance_to_ground{(float)INFINITY}; /**< max distance to ground constraint */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index dd7ee8225ef6..2f6128508e2d 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -106,5 +106,15 @@ void FlightTaskManualAltitudeSmoothVel::_setOutputState() _jerk_setpoint(2) = _smoothing.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing.getCurrentVelocity(); - _position_setpoint(2) = _smoothing.getCurrentPosition(); + + if (!_terrain_hold) { + if (_terrain_hold_previous) { + // Reset position setpoint to current position when switching from terrain hold to non-terrain hold + _smoothing.setCurrentPosition(_position(2)); + } + + _position_setpoint(2) = _smoothing.getCurrentPosition(); + } + + _terrain_hold_previous = _terrain_hold; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index 45e013fb7c82..468388c031b9 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -67,4 +67,7 @@ class FlightTaskManualAltitudeSmoothVel : public FlightTaskManualAltitude (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max ) + +private: + bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 14527ba712d2..a89283e14619 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -171,5 +171,15 @@ void FlightTaskManualPositionSmoothVel::_setOutputStateZ() _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); + + if (!_terrain_hold) { + if (_terrain_hold_previous) { + // Reset position setpoint to current position when switching from terrain hold to non-terrain hold + _smoothing_z.setCurrentPosition(_position(2)); + } + + _position_setpoint(2) = _smoothing_z.getCurrentPosition(); + } + + _terrain_hold_previous = _terrain_hold; } diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index aa82261e9d30..c8fbdf42c95a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -87,4 +87,6 @@ class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction + + bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ }; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index 1dfdad7a1384..86f9b4f11f60 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -225,7 +225,7 @@ void FwAutotuneAttitudeControl::checkFilters() reset_filters = true; } - if (reset_filters) { + if (reset_filters || !_are_filters_initialized) { _are_filters_initialized = true; _filter_sample_rate = update_rate_hz; _sys_id.setLpfCutoffFrequency(_filter_sample_rate, _param_imu_gyro_cutoff.get()); diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 2052fe01062c..15c568419f56 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2023 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 @@ -141,6 +141,7 @@ px4_add_module( mavlink_c timesync tunes + variable_length_ringbuffer version UNITY_BUILD ) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 32389b719130..615cd6dbcd74 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -181,6 +181,7 @@ Mavlink::~Mavlink() perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); + perf_free(_forwarding_error_perf); } void @@ -1221,117 +1222,16 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } -int -Mavlink::message_buffer_init(int size) -{ - _message_buffer.size = size; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - _message_buffer.data = (char *)malloc(_message_buffer.size); - - int ret; - - if (_message_buffer.data == nullptr) { - ret = PX4_ERROR; - _message_buffer.size = 0; - - } else { - ret = OK; - } - - return ret; -} - -void -Mavlink::message_buffer_destroy() -{ - _message_buffer.size = 0; - _message_buffer.write_ptr = 0; - _message_buffer.read_ptr = 0; - free(_message_buffer.data); -} - -int -Mavlink::message_buffer_count() -{ - int n = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (n < 0) { - n += _message_buffer.size; - } - - return n; -} - -bool -Mavlink::message_buffer_write(const void *ptr, int size) -{ - // bytes available to write - int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; - - if (available < 0) { - available += _message_buffer.size; - } - - if (size > available) { - // buffer overflow - return false; - } - - char *c = (char *) ptr; - int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer - - if (n < size) { - // message goes over end of the buffer - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); - _message_buffer.write_ptr = 0; - - } else { - n = 0; - } - - // now: n = bytes already written - int p = size - n; // number of bytes to write - memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); - _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; - return true; -} - -int -Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) -{ - // bytes available to read - int available = _message_buffer.write_ptr - _message_buffer.read_ptr; - - if (available == 0) { - return 0; // buffer is empty - } - - int n = 0; - - if (available > 0) { - // read pointer is before write pointer, all available bytes can be read - n = available; - *is_part = false; - - } else { - // read pointer is after write pointer, read bytes from read_ptr to end of the buffer - n = _message_buffer.size - _message_buffer.read_ptr; - *is_part = _message_buffer.write_ptr > 0; - } - - *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); - return n; -} - void Mavlink::pass_message(const mavlink_message_t *msg) { - /* size is 8 bytes plus variable payload */ + /* size is 12 bytes plus variable payload */ int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; - pthread_mutex_lock(&_message_buffer_mutex); - message_buffer_write(msg, size); - pthread_mutex_unlock(&_message_buffer_mutex); + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.push_back(reinterpret_cast(msg), size)) { + perf_count(_forwarding_error_perf); + } } MavlinkShell * @@ -2236,7 +2136,7 @@ Mavlink::task_main(int argc, char *argv[]) return PX4_ERROR; } - /* initialize send mutex */ + pthread_mutex_init(&_message_buffer_mutex, nullptr); pthread_mutex_init(&_send_mutex, nullptr); pthread_mutex_init(&_radio_status_mutex, nullptr); @@ -2246,13 +2146,12 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + LockGuard lg{_message_buffer_mutex}; + + if (!_message_buffer.allocate(2 * sizeof(mavlink_message_t) + 1)) { PX4_ERR("msg buf alloc fail"); return PX4_ERROR; } - - /* initialize message buffer mutex */ - pthread_mutex_init(&_message_buffer_mutex, nullptr); } /* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/ @@ -2608,50 +2507,21 @@ Mavlink::task_main(int argc, char *argv[]) _events.update(t); - /* pass messages from other UARTs */ + /* pass messages from other instances */ if (get_forwarding_on()) { - bool is_part; - uint8_t *read_ptr; - uint8_t *write_ptr; - - pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - if (available > 0) { - // Reconstruct message from buffer - - mavlink_message_t msg; - write_ptr = (uint8_t *)&msg; - - // Pull a single message from the buffer - size_t read_count = available; - - if (read_count > sizeof(mavlink_message_t)) { - read_count = sizeof(mavlink_message_t); - } - - memcpy(write_ptr, read_ptr, read_count); - - // We hold the mutex until after we complete the second part of the buffer. If we don't - // we may end up breaking the empty slot overflow detection semantics when we mark the - // possibly partial read below. - pthread_mutex_lock(&_message_buffer_mutex); - - message_buffer_mark_read(read_count); - - /* write second part of buffer if there is some */ - if (is_part && read_count < sizeof(mavlink_message_t)) { - write_ptr += read_count; - available = message_buffer_get_ptr((void **)&read_ptr, &is_part); - read_count = sizeof(mavlink_message_t) - read_count; - memcpy(write_ptr, read_ptr, read_count); - message_buffer_mark_read(available); - } - - pthread_mutex_unlock(&_message_buffer_mutex); + mavlink_message_t msg; + size_t available_bytes; + { + // We only send one message at a time, not to put too much strain on a + // link from forwarded messages. + LockGuard lg{_message_buffer_mutex}; + available_bytes = _message_buffer.pop_front(reinterpret_cast(&msg), sizeof(msg)); + // We need to make sure to release the lock here before sending the + // bytes out via IP or UART which could potentially take longer. + } + if (available_bytes > 0) { resend_message(&msg); } } @@ -2701,11 +2571,6 @@ Mavlink::task_main(int argc, char *argv[]) _socket_fd = -1; } - if (get_forwarding_on()) { - message_buffer_destroy(); - pthread_mutex_destroy(&_message_buffer_mutex); - } - if (_mavlink_ulog) { _mavlink_ulog->stop(); _mavlink_ulog = nullptr; @@ -2713,6 +2578,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_mutex_destroy(&_send_mutex); pthread_mutex_destroy(&_radio_status_mutex); + pthread_mutex_destroy(&_message_buffer_mutex); PX4_INFO("exiting channel %i", (int)_channel); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f9ff0a45f827..09fa425acf2f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2023 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 @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -419,11 +420,6 @@ class Mavlink final : public ModuleParams bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); } - bool message_buffer_write(const void *ptr, int size); - - void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } - void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } - /** * Count transmitted bytes */ @@ -655,16 +651,9 @@ class Mavlink final : public ModuleParams ping_statistics_s _ping_stats {}; - struct mavlink_message_buffer { - int write_ptr; - int read_ptr; - int size; - char *data; - }; + pthread_mutex_t _message_buffer_mutex{}; + VariableLengthRingbuffer _message_buffer{}; - mavlink_message_buffer _message_buffer {}; - - pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; pthread_mutex_t _radio_status_mutex {}; @@ -686,6 +675,7 @@ class Mavlink final : public ModuleParams perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")}; /**< loop performance counter */ perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": tx run interval")}; /**< loop interval performance counter */ perf_counter_t _send_byte_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": send_bytes error")}; /**< send bytes error count */ + perf_counter_t _forwarding_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": forwarding error")}; /**< forwarding messages error count */ void mavlink_update_parameters(); @@ -715,18 +705,6 @@ class Mavlink final : public ModuleParams */ int configure_streams_to_default(const char *configure_single_stream = nullptr); - int message_buffer_init(int size); - - void message_buffer_destroy(); - - int message_buffer_count(); - - int message_buffer_is_empty() const { return (_message_buffer.read_ptr == _message_buffer.write_ptr); } - - int message_buffer_get_ptr(void **ptr, bool *is_part); - - void message_buffer_mark_read(int n) { _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; } - void pass_message(const mavlink_message_t *msg); void publish_telemetry_status(); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index b20480e79121..1ee18dc9b6e3 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1525,6 +1525,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->autocontinue = mission_item->autocontinue; + mavlink_mission_item->mission_type = _mission_type; /* default mappings for generic commands */ if (mission_item->frame == MAV_FRAME_MISSION) { diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp index 749186ee8d04..c99ccad653ab 100644 --- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp +++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp @@ -236,7 +236,7 @@ void McAutotuneAttitudeControl::checkFilters() reset_filters = true; } - if (reset_filters && !_are_filters_initialized) { + if (reset_filters || !_are_filters_initialized) { _filter_dt = _sample_interval_avg; const float filter_rate_hz = 1.f / _filter_dt; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 565b2f2150ce..3f923fce3f88 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -694,6 +694,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi break; case NAV_CMD_TAKEOFF: + case NAV_CMD_VTOL_TAKEOFF: // if already flying (armed and !landed) treat TAKEOFF like regular POSITION if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) @@ -707,10 +708,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi break; - case NAV_CMD_VTOL_TAKEOFF: - sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - break; - case NAV_CMD_LAND: case NAV_CMD_VTOL_LAND: sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index ae3d93c9e29b..13cea29cb5eb 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -58,6 +58,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"; @@ -91,6 +94,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[]) @@ -136,12 +144,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();