From 2625c5211bb2ea395b1598bc103e69414cb0acd5 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 7 Jan 2025 15:11:05 -0700 Subject: [PATCH 01/94] dronecan: don't init if can interface fails to init --- src/drivers/uavcan/uavcan_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 87f3110cac..5d3ac60b62 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -705,11 +705,12 @@ UavcanNode::Run() if (can_init_res < 0) { PX4_ERR("CAN driver init failed %i", can_init_res); + + } else { + _instance->init(node_id, can->driver.updateEvent()); + + _node_init = true; } - - _instance->init(node_id, can->driver.updateEvent()); - - _node_init = true; } pthread_mutex_lock(&_node_mutex); From 18b4b18a7519d44fc6ddffcdc505c27e42c82ab5 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Wed, 8 Jan 2025 16:50:18 +0100 Subject: [PATCH 02/94] FW land detector: do not check for groundspeed if invalid. In case the local position speed estimate is not valid, it is assumed to be 0m/s and thus check always passes. --- src/modules/land_detector/FixedwingLandDetector.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index ec8157034b..eadfff83a0 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -71,9 +71,13 @@ bool FixedwingLandDetector::_get_landed_state() } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { - // Horizontal velocity complimentary filter. - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + - _vehicle_local_position.vy * _vehicle_local_position.vy); + float val = 0.0f; + + if (_vehicle_local_position.v_xy_valid) { + // Horizontal velocity complimentary filter. + val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + + _vehicle_local_position.vy * _vehicle_local_position.vy); + } if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; From b4273bde2547cae236889a580697ce789d33dddf Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Wed, 8 Jan 2025 16:56:09 +0100 Subject: [PATCH 03/94] FW land detector: Introduce max rotational speed condition (new param: LNDFW_ROT_MAX). Checks that the filtered norm of the angular velocity is below LNDFW_ROT_MAX. --- .../land_detector/FixedwingLandDetector.cpp | 23 ++++++++++++++----- .../land_detector/FixedwingLandDetector.h | 2 ++ .../land_detector/land_detector_params_fw.c | 11 +++++++++ 3 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index eadfff83a0..4ed4a353ab 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -109,15 +109,26 @@ bool FixedwingLandDetector::_get_landed_state() const float acc_hor = matrix::Vector2f(_acceleration).norm(); _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; + // Check for angular velocity + const float rot_vel_hor = _angular_velocity.norm(); + val = _velocity_rot_filtered * 0.95f + rot_vel_hor * 0.05f; + + if (PX4_ISFINITE(val)) { + _velocity_rot_filtered = val; + } + // make groundspeed threshold tighter if airspeed is invalid - const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : - _param_lndfw_vel_xy_max.get(); + const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : + _param_lndfw_vel_xy_max.get(); + + const float max_rotation_threshold = math::radians(_param_lndfw_rot_max.get()) ; // Crude land detector for fixedwing. - landDetected = _airspeed_filtered < _param_lndfw_airspd.get() - && _velocity_xy_filtered < vel_xy_max_threshold - && _velocity_z_filtered < _param_lndfw_vel_z_max.get() - && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); + landDetected = _airspeed_filtered < _param_lndfw_airspd.get() + && _velocity_xy_filtered < vel_xy_max_threshold + && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _xy_accel_filtered < _param_lndfw_xyaccel_max.get() + && _velocity_rot_filtered < max_rotation_threshold; } else { // Control state topic has timed out and we need to assume we're landed. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 981fdc2fb5..3f3fa9ad35 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -75,6 +75,7 @@ private: float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; float _xy_accel_filtered{0.0f}; + float _velocity_rot_filtered{0.0f}; DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, @@ -82,6 +83,7 @@ private: (ParamFloat) _param_lndfw_airspd, (ParamFloat) _param_lndfw_vel_xy_max, (ParamFloat) _param_lndfw_vel_z_max, + (ParamFloat) _param_lndfw_rot_max, (ParamFloat) _param_lndfw_trig_time ); }; diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c index 02ab459032..6ab78fb867 100644 --- a/src/modules/land_detector/land_detector_params_fw.c +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -102,3 +102,14 @@ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f); * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f); + +/** + * Fixed-wing land detector: max rotational speed + * + * Maximum allowed norm of the angular velocity in the landed state. + * + * @unit deg/s + * @decimal 1 + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f); From c3e370b946a8fff35a6c7f0688321772aaa2eeca Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Wed, 8 Jan 2025 16:57:47 +0100 Subject: [PATCH 04/94] MC land detector: fix parameter description for LNDMC_ROT_MAX --- src/modules/land_detector/land_detector_params_mc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 121b365bb5..1d61a7e00b 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -76,9 +76,9 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); /** - * Multicopter max rotation + * Multicopter max rotational speed * - * Maximum allowed angular velocity around each axis allowed in the landed state. + * Maximum allowed norm of the angular velocity (roll, pitch) in the landed state. * * @unit deg/s * @decimal 1 From b042f2101f7b77fb46757fedfa60189a27a58be7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 8 Jan 2025 14:29:47 +0100 Subject: [PATCH 05/94] system_params: clarify SYS_HAS_MAG description after answering the questions: Ah the value can be 2? Should I set the number of magnetometers the board has or include the external ones? --- src/lib/systemlib/system_params.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index 1ca273be84..d396039f3f 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -189,11 +189,10 @@ PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10); PARAM_DEFINE_INT32(SYS_HAS_GPS, 1); /** - * Control if the vehicle has a magnetometer + * Control if and how many magnetometers are expected * - * Set this to 0 if the board has no magnetometer. - * If set to 0, the preflight checks will not check for the presence of a - * magnetometer, otherwise N sensors are required. + * 0: System has no magnetometer, preflight checks should pass without one. + * 1-N: Require the presence of N magnetometer sensors for check to pass. * * @reboot_required true * @group System From e9536cb30b228c89262c050194bccd53a2a0b916 Mon Sep 17 00:00:00 2001 From: Peter van der Perk <57130844+PetervdPerk-NXP@users.noreply.github.com> Date: Thu, 9 Jan 2025 03:35:08 +0100 Subject: [PATCH 06/94] Tropic-community use flash as storage using LittleFS (#24158) * Update NuttX --- boards/nxp/tropic-community/default.px4board | 2 +- .../nuttx-config/nsh/defconfig | 6 +- .../scripts/itcm_static_functions.ld | 16 + .../nuttx-config/scripts/script.ld | 2 +- .../nxp/tropic-community/src/CMakeLists.txt | 1 + .../nxp/tropic-community/src/board_config.h | 2 + .../src/imxrt_flexspi_storage.c | 614 ++++++++++++++++++ boards/nxp/tropic-community/src/init.c | 10 + platforms/nuttx/NuttX/nuttx | 2 +- 9 files changed, 651 insertions(+), 4 deletions(-) create mode 100644 boards/nxp/tropic-community/src/imxrt_flexspi_storage.c diff --git a/boards/nxp/tropic-community/default.px4board b/boards/nxp/tropic-community/default.px4board index 314ff5528b..c7fcb205e0 100644 --- a/boards/nxp/tropic-community/default.px4board +++ b/boards/nxp/tropic-community/default.px4board @@ -4,7 +4,7 @@ CONFIG_BOARD_ETHERNET=y CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" -CONFIG_BOARD_PARAM_FILE="/fs/microsd/mtd_params" +CONFIG_BOARD_PARAM_FILE="/fs/nor/mtd_params" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y diff --git a/boards/nxp/tropic-community/nuttx-config/nsh/defconfig b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig index 4b789eb358..15122b439d 100644 --- a/boards/nxp/tropic-community/nuttx-config/nsh/defconfig +++ b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig @@ -60,6 +60,8 @@ CONFIG_FS_BINFS=y CONFIG_FS_CROMFS=y CONFIG_FS_FAT=y CONFIG_FS_FATTIME=y +CONFIG_FS_LITTLEFS=y +CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=2 CONFIG_FS_PROCFS=y CONFIG_FS_PROCFS_MAX_TASKS=64 CONFIG_FS_PROCFS_REGISTER=y @@ -80,6 +82,8 @@ CONFIG_IMXRT_ENET_NTXBUFFERS=8 CONFIG_IMXRT_ENET_PHYINIT=y CONFIG_IMXRT_FLEXCAN3=y CONFIG_IMXRT_FLEXCAN_TXMB=1 +CONFIG_IMXRT_FLEXSPI1=y +CONFIG_IMXRT_FLEXSPI1_XIP=y CONFIG_IMXRT_GPIO1_0_15_IRQ=y CONFIG_IMXRT_GPIO1_16_31_IRQ=y CONFIG_IMXRT_GPIO2_0_15_IRQ=y @@ -116,7 +120,7 @@ CONFIG_INTELHEX_BINARY=y CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y -CONFIG_IPCFG_PATH="/fs/microsd" +CONFIG_IPCFG_PATH="/fs/nor" CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_LPI2C1_DMA=y diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld index 8b228a22f8..2f6259209a 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld @@ -117,3 +117,19 @@ *(.text.ipv4_input) *(.text.work_thread) *(.text.work_queue) + +/* Flash Storage */ +*(.text.imxrt_flexspi_transfer_blocking) +*(.text.imxrt_flexspi_transfer_blocking_private) +*(.text.imxrt_flexspi_write_blocking) +*(.text.imxrt_flexspi_read_blocking) +*(.text.imxrt_flexspi_check_and_clear_error) +*(.text.imxrt_flexspi_get_bus_idle_status) +*(.text.imxrt_flexspi_configure_prefetch) +*(.text.imxrt_flexspi_configure_prefetch_private) +*(.text.imxrt_flexspi_storage_write_enable) +*(.text.imxrt_flexspi_storage_wait_bus_busy) +*(.text.imxrt_flexspi_storage_read_status) +*(.text.imxrt_flexspi_storage_erase) +*(.text.imxrt_flexspi_storage_bwrite) +*(.text.imxrt_flexspi_storage_page_program) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/script.ld b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld index a715059912..01d2e8d951 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/script.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld @@ -22,7 +22,7 @@ MEMORY { - flash (rx) : ORIGIN = 0x60000000, LENGTH = 7936K + flash (rx) : ORIGIN = 0x60000000, LENGTH = 7936K - 128K sram (rwx) : ORIGIN = 0x20200000, LENGTH = 512K itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 384K dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K diff --git a/boards/nxp/tropic-community/src/CMakeLists.txt b/boards/nxp/tropic-community/src/CMakeLists.txt index 586c60841e..91d89b5d30 100644 --- a/boards/nxp/tropic-community/src/CMakeLists.txt +++ b/boards/nxp/tropic-community/src/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_library(drivers_board usb.c imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c + imxrt_flexspi_storage.c ) diff --git a/boards/nxp/tropic-community/src/board_config.h b/boards/nxp/tropic-community/src/board_config.h index cef895ab6c..83bbd101e2 100644 --- a/boards/nxp/tropic-community/src/board_config.h +++ b/boards/nxp/tropic-community/src/board_config.h @@ -326,6 +326,8 @@ extern void fmurt1062_timer_initialize(void); #include +int imxrt_flexspi_storage_initialize(void); + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c b/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c new file mode 100644 index 0000000000..f3ac06804f --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c @@ -0,0 +1,614 @@ +/**************************************************************************** + * boards/px4/fmu-v6xrt/src/imxrt_flexspi_storage.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include "px4_log.h" + +#include "imxrt_flexspi.h" +#include "board_config.h" +#include "hardware/imxrt_pinmux.h" + +#ifdef CONFIG_IMXRT_FLEXSPI + +/* Used sectors must be multiple of the flash block size + * i.e. W25Q32JV has a block size of 64KB +*/ + +#define NOR_USED_SECTORS (0x20U) /* 32 * 4KB = 128KB */ +#define NOR_TOTAL_SECTORS (0x0800U) +#define NOR_PAGE_SIZE (0x0100U) /* 256 bytes */ +#define NOR_SECTOR_SIZE (0x1000U) /* 4KB */ +#define NOR_START_SECTOR (NOR_TOTAL_SECTORS - NOR_USED_SECTORS) +#define NOR_START_PAGE ((NOR_START_SECTOR * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE) +#define NOR_STORAGE_ADDR (IMXRT_FLEXCIPHER_BASE + NOR_START_SECTOR * NOR_SECTOR_SIZE) + +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) + +enum { + /* SPI instructions */ + + READ_FAST = 0, + READ_STATUS_REG = 1, + WRITE_STATUS_REG = 3, + WRITE_ENABLE = 4, + SECTOR_ERASE_4K = 5, + READ_FAST_QUAD_OUTPUT = 6, + PAGE_PROGRAM_QUAD_INPUT = 7, + PAGE_PROGRAM = 9, + CHIP_ERASE = 11, +}; + +static const uint32_t g_flexspi_nor_lut[][4] = { + [READ_FAST] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0xeb, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_4PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x06, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_4PAD, 0x04), + }, + + [READ_STATUS_REG] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x05, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_1PAD, 0x04), + }, + + [WRITE_STATUS_REG] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x01, + FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04), + }, + + [WRITE_ENABLE] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x06, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + + [SECTOR_ERASE_4K] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x20, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + }, + + [CHIP_ERASE] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0xc7, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + + [PAGE_PROGRAM] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x02, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0x0), + }, + + [READ_FAST_QUAD_OUTPUT] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x6b, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x08, + FLEXSPI_COMMAND_READ_SDR, FLEXSPI_4PAD, 0x04), + }, + + [PAGE_PROGRAM_QUAD_INPUT] = + { + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x32, + FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18), + FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_4PAD, 0x04, + FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0), + }, + +}; + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* FlexSPI NOR device private data */ + +struct imxrt_flexspi_storage_dev_s { + struct mtd_dev_s mtd; + struct flexspi_dev_s *flexspi; /* Saved FlexSPI interface instance */ + uint8_t *ahb_base; + enum flexspi_port_e port; + struct flexspi_device_config_s *config; +}; + +/**************************************************************************** + * Private Functions Prototypes + ****************************************************************************/ + +/* MTD driver methods */ + +static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks); +static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev, + off_t offset, + size_t nbytes, + uint8_t *buffer); +static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + uint8_t *buffer); +static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + const uint8_t *buffer); +static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev, + int cmd, + unsigned long arg); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct flexspi_device_config_s g_flexspi_device_config = { + .flexspi_root_clk = 4000000, + .is_sck2_enabled = 0, + .flash_size = NOR_USED_SECTORS * NOR_SECTOR_SIZE / 4, + .cs_interval_unit = FLEXSPI_CS_INTERVAL_UNIT1_SCK_CYCLE, + .cs_interval = 0, + .cs_hold_time = 3, + .cs_setup_time = 3, + .data_valid_time = 0, + .columnspace = 0, + .enable_word_address = 0, + .awr_seq_index = 0, + .awr_seq_number = 0, + .ard_seq_index = READ_FAST, + .ard_seq_number = 1, + .ahb_write_wait_unit = FLEXSPI_AHB_WRITE_WAIT_UNIT2_AHB_CYCLE, + .ahb_write_wait_interval = 0, + .rx_sample_clock = FLEXSPI_READ_SAMPLE_CLK_LOOPBACK_FROM_DQS_PAD, +}; + +static struct imxrt_flexspi_storage_dev_s g_flexspi_nor = { + .mtd = + { + .erase = imxrt_flexspi_storage_erase, + .bread = imxrt_flexspi_storage_bread, + .bwrite = imxrt_flexspi_storage_bwrite, + .read = imxrt_flexspi_storage_read, + .ioctl = imxrt_flexspi_storage_ioctl, +#ifdef CONFIG_MTD_BYTE_WRITE + .write = NULL, +#endif + .name = "imxrt_flexspi_storage" + }, + .flexspi = (void *)0, + .ahb_base = (uint8_t *) NOR_STORAGE_ADDR, + .port = FLEXSPI_PORT_A1, + .config = &g_flexspi_device_config +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int imxrt_flexspi_storage_read_status( + const struct imxrt_flexspi_storage_dev_s *dev, + uint32_t *status) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_READ, + .seq_number = 1, + .seq_index = READ_STATUS_REG, + .data = status, + .data_size = 1, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_storage_write_enable( + const struct imxrt_flexspi_storage_dev_s *dev) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = 0, + .port = dev->port, + .cmd_type = FLEXSPI_COMMAND, + .seq_number = 1, + .seq_index = WRITE_ENABLE, + .data = NULL, + .data_size = 0, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_storage_erase_sector( + const struct imxrt_flexspi_storage_dev_s *dev, + off_t offset) +{ + int stat; + struct flexspi_transfer_s transfer = { + .device_address = offset, + .port = dev->port, + .cmd_type = FLEXSPI_COMMAND, + .seq_number = 1, + .seq_index = SECTOR_ERASE_4K, + .data = NULL, + .data_size = 0, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + + return 0; +} + +static int imxrt_flexspi_storage_erase_chip( + const struct imxrt_flexspi_storage_dev_s *dev) +{ + /* We can't erase the chip we're executing from */ + return -EINVAL; +} + +static int imxrt_flexspi_storage_page_program( + const struct imxrt_flexspi_storage_dev_s *dev, + off_t offset, + const void *buffer, + size_t len) +{ + int stat; + + struct flexspi_transfer_s transfer = { + .device_address = offset, + .port = dev->port, + .cmd_type = FLEXSPI_WRITE, + .seq_number = 1, + .seq_index = PAGE_PROGRAM_QUAD_INPUT, + .data = (uint32_t *) buffer, + .data_size = len, + }; + + stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer); + + if (stat != 0) { + return -EIO; + } + + return 0; +} + +static int imxrt_flexspi_storage_wait_bus_busy( + const struct imxrt_flexspi_storage_dev_s *dev) +{ + uint32_t status = 0; + int ret; + + do { + ret = imxrt_flexspi_storage_read_status(dev, &status); + + if (ret) { + return ret; + } + } while (status & 1); + + return 0; +} + +static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev, + off_t offset, + size_t nbytes, + uint8_t *buffer) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + uint8_t *src; + + finfo("offset: %08lx nbytes: %d\n", (long)offset, (int)nbytes); + + if (priv->port >= FLEXSPI_PORT_COUNT) { + return -EIO; + } + + src = priv->ahb_base + offset; + + memcpy(buffer, src, nbytes); + + finfo("return nbytes: %d\n", (int)nbytes); + return (ssize_t)nbytes; +} + +static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + uint8_t *buffer) +{ + ssize_t nbytes; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + /* On this device, we can handle the block read just like the byte-oriented + * read + */ + + nbytes = imxrt_flexspi_storage_read(dev, startblock * NOR_PAGE_SIZE, + nblocks * NOR_PAGE_SIZE, buffer); + + if (nbytes > 0) { + nbytes /= NOR_PAGE_SIZE; + } + + return nbytes; +} + +static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks, + const uint8_t *buffer) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + size_t len = nblocks * NOR_PAGE_SIZE; + off_t offset = (startblock + NOR_START_PAGE) * NOR_PAGE_SIZE; + uint8_t *src = (uint8_t *) buffer; +#ifdef CONFIG_ARMV7M_DCACHE + uint8_t *dst = priv->ahb_base + startblock * NOR_PAGE_SIZE; +#endif + int i; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false); + + irqstate_t flags = enter_critical_section(); + + while (len) { + i = MIN(NOR_PAGE_SIZE, len); + imxrt_flexspi_storage_write_enable(priv); + imxrt_flexspi_storage_page_program(priv, offset, src, i); + imxrt_flexspi_storage_wait_bus_busy(priv); + offset += i; + src += i; + len -= i; + } + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true); + + leave_critical_section(flags); + +#ifdef CONFIG_ARMV7M_DCACHE + up_invalidate_dcache((uintptr_t)dst, + (uintptr_t)dst + nblocks * NOR_PAGE_SIZE); +#endif + + return nblocks; +} + +static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev, + off_t startblock, + size_t nblocks) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + size_t blocksleft = nblocks; +#ifdef CONFIG_ARMV7M_DCACHE + uint8_t *dst = priv->ahb_base + startblock * NOR_SECTOR_SIZE; +#endif + + startblock += NOR_START_SECTOR; + + finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks); + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false); + + irqstate_t flags = enter_critical_section(); + + while (blocksleft-- > 0) { + /* Erase each sector */ + + imxrt_flexspi_storage_write_enable(priv); + imxrt_flexspi_storage_erase_sector(priv, startblock * NOR_SECTOR_SIZE); + imxrt_flexspi_storage_wait_bus_busy(priv); + startblock++; + } + + FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true); + + leave_critical_section(flags); + +#ifdef CONFIG_ARMV7M_DCACHE + up_invalidate_dcache((uintptr_t)dst, + (uintptr_t)dst + nblocks * NOR_SECTOR_SIZE); +#endif + + return (int)nblocks; +} + +static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev, + int cmd, + unsigned long arg) +{ + struct imxrt_flexspi_storage_dev_s *priv = + (struct imxrt_flexspi_storage_dev_s *)dev; + int ret = -EINVAL; /* Assume good command with bad parameters */ + + finfo("cmd: %d\n", cmd); + + switch (cmd) { + case MTDIOC_GEOMETRY: { + struct mtd_geometry_s *geo = + (struct mtd_geometry_s *)((uintptr_t)arg); + + if (geo) { + /* Populate the geometry structure with information need to + * know the capacity and how to access the device. + * + * NOTE: + * that the device is treated as though it where just an array + * of fixed size blocks. That is most likely not true, but the + * client will expect the device logic to do whatever is + * necessary to make it appear so. + */ + + geo->blocksize = (NOR_PAGE_SIZE); + geo->erasesize = (NOR_SECTOR_SIZE); + geo->neraseblocks = (NOR_USED_SECTORS); + + ret = OK; + + finfo("blocksize: %lu erasesize: %lu neraseblocks: %lu\n", + geo->blocksize, geo->erasesize, geo->neraseblocks); + } + } + break; + + case BIOC_PARTINFO: { + struct partition_info_s *info = + (struct partition_info_s *)arg; + + if (info != NULL) { + info->numsectors = (NOR_USED_SECTORS * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE; + info->sectorsize = NOR_PAGE_SIZE; + info->startsector = 0; + info->parent[0] = '\0'; + ret = OK; + } + } + break; + + case MTDIOC_BULKERASE: { + /* Erase the entire device */ + + imxrt_flexspi_storage_write_enable(priv); + imxrt_flexspi_storage_erase_chip(priv); + imxrt_flexspi_storage_wait_bus_busy(priv); + ret = OK; + } + break; + + default: + ret = -ENOTTY; /* Bad/unsupported command */ + break; + } + + finfo("return %d\n", ret); + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + + +/**************************************************************************** + * Name: imxrt_flexspi_storage_initialize + * + * Description: + * This function is called by board-bringup logic to configure the + * flash device. + * + * Returned Value: + * Zero is returned on success. Otherwise, a negated errno value is + * returned to indicate the nature of the failure. + * + ****************************************************************************/ + +int imxrt_flexspi_storage_initialize(void) +{ + struct mtd_dev_s *mtd_dev = &g_flexspi_nor.mtd; + int ret = -ENODEV; + + /* Select FlexSPI1 */ + + g_flexspi_nor.flexspi = imxrt_flexspi_initialize(0); + + if (g_flexspi_nor.flexspi) { + ret = OK; + + + FLEXSPI_UPDATE_LUT(g_flexspi_nor.flexspi, + 0, + (const uint32_t *)g_flexspi_nor_lut, + sizeof(g_flexspi_nor_lut) / 4); + } + + /* Register the MTD driver so that it can be accessed from the + * VFS. + */ + + ret = register_mtddriver("/dev/nor", mtd_dev, 0755, NULL); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to register MTD driver: %d\n", + ret); + } + +#ifdef CONFIG_FS_LITTLEFS + + /* Mount the LittleFS file system */ + + ret = nx_mount("/dev/nor", "/fs/nor", "littlefs", 0, + "autoformat"); + + if (ret < 0) { + syslog(LOG_ERR, + "ERROR: Failed to mount LittleFS at /mnt/lfs: %d\n", + ret); + } + +#endif + + return ret; +} +#endif /* CONFIG_IMXRT_FLEXSPI */ diff --git a/boards/nxp/tropic-community/src/init.c b/boards/nxp/tropic-community/src/init.c index f3b43fc4d5..244d6c5fcd 100644 --- a/boards/nxp/tropic-community/src/init.c +++ b/boards/nxp/tropic-community/src/init.c @@ -411,5 +411,15 @@ __EXPORT int board_app_initialize(uintptr_t arg) imxrt_caninitialize(3); #endif +#ifdef CONFIG_IMXRT_FLEXSPI + ret = imxrt_flexspi_storage_initialize(); + + if (ret < 0) { + syslog(LOG_ERR, + "ERROR: imxrt_flexspi_nor_initialize failed: %d\n", ret); + } + +#endif + return ret; } diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index e61fdd019d..4c875d0fdf 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit e61fdd019de6ee7685c071760a965961c5ef5227 +Subproject commit 4c875d0fdffb4f9ba0e0e341ae567d4d0a544e46 From ab70ae3252d96e8fb137800b89aaf58b5c90de78 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 20 Dec 2024 15:51:08 +0100 Subject: [PATCH 07/94] ekf2: fix GNSS drift false alarm In SIH, the GNSS signal is zero-mean, but apparently not symmetric. The issue is that saturating such a signal creates an artificial bias. This made the check fail as the bias was above the threshold. --- src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index e21ee82ff9..81b3fcc76b 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -102,11 +102,13 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift); - Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); - pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit); + Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); // Apply a low pass filter - _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + _gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + + // Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals + _gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit); // hdrift: calculate the horizontal drift speed and fail if too high _gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm(); From cbbbbc9dfac006e48385a72e96c23a4cf332a97e Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Fri, 3 Jan 2025 15:30:07 +0200 Subject: [PATCH 08/94] logger: fix file open issue if crypto algorithm is disabled move init_logfile_encryption() call after the buffer start_log() call to have log file already open while storing the header and key data to the beginning of the file. --- src/modules/logger/log_writer_file.cpp | 46 +++++++++++--------------- src/modules/logger/log_writer_file.h | 2 +- 2 files changed, 21 insertions(+), 27 deletions(-) diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index 14a2ad0136..530076f49b 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -88,7 +88,7 @@ LogWriterFile::~LogWriterFile() } #if defined(PX4_CRYPTO) -bool LogWriterFile::init_logfile_encryption(const char *filename) +bool LogWriterFile::init_logfile_encryption(const LogType type) { if (_algorithm == CRYPTO_NONE) { _min_blocksize = 1; @@ -151,16 +151,16 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) rsa_crypto.close(); - // Write the encrypted key to the disk - int key_fd = ::open((const char *)filename, O_CREAT | O_WRONLY | O_DIRECT | O_SYNC, PX4_O_MODE_666); + // Write the encrypted key to the beginning of the opened log file + int key_fd = _buffers[(int)type].fd(); if (key_fd < 0) { - PX4_ERR("Can't open key file, errno: %d", errno); + PX4_ERR("Log file not open for storing the key, errno: %d", errno); free(key); return false; } - // write the header to the combined key exchange & cipherdata file + // write header and key to the beginning of the log file struct ulog_key_header_s keyfile_header = { .magic = {'U', 'L', 'o', 'g', 'E', 'n', 'c'}, .hdr_ver = 1, @@ -171,20 +171,14 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) .initdata_size = (uint16_t)nonce_size }; - size_t hdr_sz = ::write(key_fd, (uint8_t *)&keyfile_header, sizeof(keyfile_header)); - size_t written = 0; - - if (hdr_sz == sizeof(keyfile_header)) { - // Header write succeeded, write the key - written = ::write(key_fd, key, key_size + nonce_size); - } + size_t written = ::write(key_fd, (uint8_t *)&keyfile_header, sizeof(keyfile_header)); + written += ::write(key_fd, key, key_size + nonce_size); // Free temporary memory allocations free(key); - ::close(key_fd); // Check that writing to the disk succeeded - if (written != key_size + nonce_size) { + if (written != sizeof(keyfile_header) + key_size + nonce_size) { PX4_ERR("Writing the encryption key to disk fail"); return false; } @@ -220,18 +214,22 @@ bool LogWriterFile::start_log(LogType type, const char *filename) } } -#if PX4_CRYPTO - bool enc_init = init_logfile_encryption(filename); + if (_buffers[(int)type].start_log(filename)) { - if (!enc_init) { - PX4_ERR("Failed to start encrypted logging"); - _crypto.close(); - return false; - } +#if PX4_CRYPTO + bool enc_init = init_logfile_encryption(type); + + if (!enc_init) { + PX4_ERR("Failed to start encrypted logging"); + _crypto.close(); + _buffers[(int)type]._should_run = false; + _buffers[(int)type].close_file(); + _buffers[(int)type].reset(); + return false; + } #endif - if (_buffers[(int)type].start_log(filename)) { PX4_INFO("Opened %s log file: %s", log_type_str(type), filename); notify(); return true; @@ -637,11 +635,7 @@ size_t LogWriterFile::LogFileBuffer::get_read_ptr(void **ptr, bool *is_part) bool LogWriterFile::LogFileBuffer::start_log(const char *filename) { -#if defined(PX4_CRYPTO) - _fd = ::open(filename, O_APPEND | O_WRONLY, PX4_O_MODE_666); -#else _fd = ::open(filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); -#endif _had_write_error.store(false); if (_fd < 0) { diff --git a/src/modules/logger/log_writer_file.h b/src/modules/logger/log_writer_file.h index 7654878938..4739b5b799 100644 --- a/src/modules/logger/log_writer_file.h +++ b/src/modules/logger/log_writer_file.h @@ -224,7 +224,7 @@ private: pthread_cond_t _cv; pthread_t _thread = 0; #if defined(PX4_CRYPTO) - bool init_logfile_encryption(const char *filename); + bool init_logfile_encryption(const LogType type); PX4Crypto _crypto; int _min_blocksize; px4_crypto_algorithm_t _algorithm; From 44b423f48dbe2688427f17c66fb29536f4834828 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 9 Jan 2025 08:54:58 +0100 Subject: [PATCH 09/94] drivers: ulanding: description: fix link to user guide section (#24189) Signed-off-by: Silvan --- .../distance_sensor/ulanding_radar/ulanding_radar_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp b/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp index 25219f6a7d..d8898fbb68 100644 --- a/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp @@ -102,7 +102,7 @@ static int usage() Serial bus driver for the Aerotenna uLanding radar. -Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/ulanding_radar.html +Setup/usage information: https://docs.px4.io/main/en/sensor/ulanding_radar.html ### Examples From a231fafafadf92ec76022e6afdfcb5aeb7e4ae90 Mon Sep 17 00:00:00 2001 From: Balduin Date: Thu, 9 Jan 2025 15:40:06 +0100 Subject: [PATCH 10/94] SIH: Add Standard VTOL Airframe (#24175) * add standard vtol airframe to SIH. mostly took changes from 4d930bde and applied to main. generate_fw_aerodynamics now takes four arguments rather than using the _u class member, because depending on vehicle type _u is used differently. --- .../init.d/airframes/1002_standard_vtol.hil | 9 ++ .../airframes/1103_standard_vtol_sih.hil | 101 ++++++++++++++++++ .../init.d/airframes/CMakeLists.txt | 1 + src/modules/simulation/simulator_sih/sih.cpp | 42 ++++++-- src/modules/simulation/simulator_sih/sih.hpp | 8 +- .../simulation/simulator_sih/sih_params.c | 1 + 6 files changed, 147 insertions(+), 15 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index b5a633dc1b..4129d2569c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -5,6 +5,15 @@ # @type Standard VTOL # @class VTOL # +# @output Motor1 MC motor front right +# @output Motor2 MC motor back left +# @output Motor3 MC motor front left +# @output Motor4 MC motor back right +# @output Motor5 Forward thrust motor +# @output Servo1 Aileron +# @output Servo2 Elevator +# @output Servo3 Rudder +# # @maintainer Roman Bapst # # @board px4_fmu-v2 exclude diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil new file mode 100644 index 0000000000..c67cf2cc85 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -0,0 +1,101 @@ +#!/bin/sh +# +# @name SIH Standard VTOL QuadPlane +# +# @type Simulation +# @class VTOL +# +# @output Motor1 MC motor front right +# @output Motor2 MC motor back left +# @output Motor3 MC motor front left +# @output Motor4 MC motor back right +# @output Motor5 Forward thrust motor +# @output Servo1 Aileron +# @output Servo2 Elevator +# @output Servo3 Rudder +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +param set UAVCAN_ENABLE 0 +param set-default VT_B_TRANS_DUR 5 +param set-default VT_ELEV_MC_LOCK 0 +param set-default VT_TYPE 2 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR0_PX 0.2 +param set-default CA_ROTOR0_PY 0.2 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_PX -0.2 +param set-default CA_ROTOR1_PY -0.2 +param set-default CA_ROTOR2_PX 0.2 +param set-default CA_ROTOR2_PY -0.2 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.2 +param set-default CA_ROTOR3_PY 0.2 +param set-default CA_ROTOR3_KM -0.05 + + +param set-default CA_ROTOR4_PX -0.3 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 + +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 0.5 +param set-default CA_SV_CS0_TYPE 2 +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS2_TRQ_Y 1 + +param set HIL_ACT_REV 32 + +param set-default FW_AIRSPD_MAX 12 +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 + +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 +param set-default HIL_ACT_FUNC5 201 +param set-default HIL_ACT_FUNC6 202 +param set-default HIL_ACT_FUNC7 203 +param set-default HIL_ACT_FUNC8 105 + +param set-default MAV_TYPE 22 + + + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +param set-default SYS_HITL 2 + +# disable some checks to allow to fly: +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default CBRK_IO_SAFETY 22027 + +param set-default SENS_DPRES_OFF 0.001 + +param set SIH_T_MAX 2.0 +param set SIH_Q_MAX 0.0165 +param set SIH_MASS 0.2 +# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg +param set SIH_IXX 0.00354 +param set SIH_IYY 0.000625 +param set SIH_IZZ 0.00300 +param set SIH_IXZ 0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.145 + +# sih as standard vtol +param set SIH_VEHICLE_TYPE 3 + +param set-default VT_ARSP_TRANS 6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 06d123d857..d9142ec1c5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM) 1100_rc_quad_x_sih.hil 1101_rc_plane_sih.hil 1102_tailsitter_duo_sih.hil + 1103_standard_vtol_sih.hil ) endif() diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index ea6425f21a..f90805dd66 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -76,7 +76,7 @@ void Sih::run() _airspeed_time = task_start; _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), - static_cast(2)); + static_cast(3)); _actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; @@ -216,7 +216,8 @@ void Sih::sensor_step() reconstruct_sensors_signals(now); - if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && now - _airspeed_time >= 50_ms) { + if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) + && now - _airspeed_time >= 50_ms) { _airspeed_time = now; send_airspeed(now); } @@ -302,7 +303,7 @@ void Sih::read_motors(const float dt) if (_actuator_out_sub.update(&actuators_out)) { _last_actuator_output_time = actuators_out.timestamp; - for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals + for (int i = 0; i < NUM_ACTUATORS_MAX; i++) { // saturate the motor signals if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) { _u[i] = actuators_out.output[i]; @@ -328,7 +329,7 @@ void Sih::generate_force_and_torques() _T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster // _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque _Mt_B = Vector3f(); - generate_fw_aerodynamics(); + generate_fw_aerodynamics(_u[0], _u[1], _u[2], _u[3]); } else if (_vehicle == VehicleType::TS) { _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1])); @@ -337,17 +338,31 @@ void Sih::generate_force_and_torques() // _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft // _Ma_B = -_KDW * _w_B; // first order angular damper + + } else if (_vehicle == VehicleType::SVTOL) { + + _T_B = Vector3f(_T_MAX * 2 * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), + _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), + _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); + + // thrust 0 because it is already contained in _T_B. in + // equations_of_motion they are all summed into sum_of_forces_E + generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); } } -void Sih::generate_fw_aerodynamics() +void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, + const float throttle_cmd) { const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); const float &alt = _lla.altitude(); - _wing_l.update_aero(v_B, _w_B, alt, -_u[0]*FLAP_MAX); - _wing_r.update_aero(v_B, _w_B, alt, _u[0]*FLAP_MAX); - _tailplane.update_aero(v_B, _w_B, alt, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); - _fin.update_aero(v_B, _w_B, alt, _u[2]*FLAP_MAX, _T_MAX * _u[3]); + + _wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX); + _wing_r.update_aero(v_B, _w_B, alt, -roll_cmd * FLAP_MAX); + + _tailplane.update_aero(v_B, _w_B, alt, pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd); + _fin.update_aero(v_B, _w_B, alt, yaw_cmd * FLAP_MAX, _T_MAX * throttle_cmd); _fuselage.update_aero(v_B, _w_B, alt); // sum of aerodynamic forces @@ -412,7 +427,7 @@ void Sih::equations_of_motion(const float dt) Vector3f ground_force_E; if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) { - if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { + if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) { ground_force_E = -sum_of_forces_E; if (!_grounded) { @@ -537,6 +552,8 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; + + // regardless of vehicle type, body frame, etc this holds as long as wind=0 airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = NAN; @@ -554,7 +571,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; distance_sensor_s distance_sensor{}; - //distance_sensor.timestamp_sample = time_now_us; + // distance_sensor.timestamp_sample = time_now_us; distance_sensor.device_id = device_id.devid; distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; @@ -706,6 +723,9 @@ int Sih::print_status() PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa()))); PX4_INFO("v segment (m/s)"); _ts[4].get_vS().print(); + + } else if (_vehicle == VehicleType::SVTOL) { + PX4_INFO("Running Standard VTOL"); } PX4_INFO("vehicle landed: %d", _grounded); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 588d231855..490e1dd823 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -134,7 +134,7 @@ private: uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; // hard constants - static constexpr uint16_t NB_MOTORS = 6; + static constexpr uint16_t NUM_ACTUATORS_MAX = 9; static constexpr float T1_C = 15.0f; // ground temperature in Celsius static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre @@ -161,7 +161,7 @@ private: void send_airspeed(const hrt_abstime &time_now_us); void send_dist_snsr(const hrt_abstime &time_now_us); void publish_ground_truth(const hrt_abstime &time_now_us); - void generate_fw_aerodynamics(); + void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); void generate_ts_aerodynamics(); void sensor_step(); static float computeGravity(double lat); @@ -218,9 +218,9 @@ private: matrix::Vector3f _v_E_dot{}; matrix::Dcmf _R_N2E; // local navigation to ECEF frame rotation matrix - float _u[NB_MOTORS] {}; // thruster signals + float _u[NUM_ACTUATORS_MAX] {}; // thruster signals - enum class VehicleType {MC, FW, TS}; + enum class VehicleType {MC, FW, TS, SVTOL}; VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 16c58430f6..f9e053a5eb 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -332,6 +332,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @value 0 Multicopter * @value 1 Fixed-Wing * @value 2 Tailsitter + * @value 3 Standard VTOL * @reboot_required true * @group Simulation In Hardware */ From 208d37e703f1bdb52f0b134fad685879a6e1d5cf Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Fri, 10 Jan 2025 16:58:15 +0100 Subject: [PATCH 11/94] Remove circular dependency if control allocation (#24195) ... and actuator effectiveness --- .../ActuatorEffectiveness/ActuatorEffectiveness.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp index 901c469746..b36da8230f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectiveness.hpp" -#include "../ControlAllocation/ControlAllocation.hpp" #include @@ -51,12 +50,12 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m return -1; } - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1); - effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2); matrix_selection_indexes[totalNumActuators()] = selected_matrix; ++num_actuators[(int)type]; return num_actuators_matrix[selected_matrix]++; From f5c05f6d0113304e480e3db1ea94dd6d0ed2ff34 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Fri, 10 Jan 2025 17:22:02 +0100 Subject: [PATCH 12/94] Take 2: Cleanup circular dependencies ActuatorEffectiveness --- .../ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp | 1 - .../ActuatorEffectivenessRoverAckermann.cpp | 1 - .../ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index c7c2bba7d7..0a4516ba56 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessFixedWing.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 94f5db16f3..e9eda4c538 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessRoverAckermann.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 79853c7518..f15624dd67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessStandardVTOL.hpp" -#include using namespace matrix; From 28fa04438616d16363306878e6bfee0473c77afc Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 13 Jan 2025 10:30:35 +0100 Subject: [PATCH 13/94] MC-stabilized: rescale thrust input to hover thrust at zero stick input Use hover thrust estimate in stabilized mode to rescale stick inputs. Prevents vehicle from losing/gaining altitude when switching from position to stabilized mode. --- src/modules/mc_att_control/mc_att_control.hpp | 13 ++++++---- .../mc_att_control/mc_att_control_main.cpp | 24 +++++++++++++++---- 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c15..995eea4256 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -98,9 +99,11 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -118,8 +121,10 @@ private: matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + float _hover_thrust{NAN}; + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index f6cc5db43d..22b78d519c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 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 @@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; + { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } + } + } + + if (!PX4_ISFINITE(_hover_thrust)) { + _hover_thrust = _param_mpc_thr_hover.get(); + } + + // throttle_stick_input is in range [-1, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; - default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle - thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, - {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale to hover throttle at 0 stick input + thrust = math::interpolateNXY(throttle_stick_input, + {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()}); break; } From 879e0ea9b10493029851bdbc651c3ce6d7a46a56 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 13 Jan 2025 10:33:14 +0100 Subject: [PATCH 14/94] MC-hte: use allocated thrust as input for hover thrust estimator. Improves estimates on vehicles where thrust is often saturating. --- .../MulticopterHoverThrustEstimator.cpp | 30 ++++++++++++------- .../MulticopterHoverThrustEstimator.hpp | 9 ++++-- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 07c23bbb5d..d8b97336b9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run() } } - // new local position setpoint needed every iteration - if (!_vehicle_local_position_setpoint_sub.updated()) { - return; - } - // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run() _hover_thrust_ekf.predict(dt); - vehicle_local_position_setpoint_s local_pos_sp; + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); - if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { - if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + control_allocator_status_s control_allocator_status; + + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint) + && _control_allocator_status_sub.update(&control_allocator_status) + && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms) + && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms) + ) { + matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust); + matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated; + + const matrix::Quatf q_att{vehicle_attitude.q}; + matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated); + + if (PX4_ISFINITE(thrust_allocated(2))) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects @@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run() 1.f); _hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z)); - _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); @@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run() _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state(); - publishStatus(local_pos.timestamp); + publishStatus(vehicle_thrust_setpoint.timestamp); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index b1d0324298..1738cfe785 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -54,10 +54,12 @@ #include #include #include +#include #include #include -#include #include +#include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +103,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; hrt_abstime _timestamp_last{0}; From 0fb8463b790def648c97e59359d487682528324b Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 13 Jan 2025 08:15:40 -0900 Subject: [PATCH 15/94] logger: always log can_interface_status (#24071) * logger: always log can_interface_status * logger: log topic can_interface_status using CONFIG_BOARD_UAVCAN_INTERFACES --- src/modules/logger/logged_topics.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d1e67a5cd7..3e91b24619 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); - add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); @@ -251,6 +250,10 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + +#ifdef CONFIG_BOARD_UAVCAN_INTERFACES + add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES); +#endif } void LoggedTopics::add_high_rate_topics() From e4e975806feb46f96eb7c85d5d1568747d9fe3b4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 13 Jan 2025 12:06:53 +0100 Subject: [PATCH 16/94] Enable CONFIG_EKF2_AUX_GLOBAL_POSITION by default Signed-off-by: Silvan Fuhrer --- boards/px4/fmu-v5/rover.px4board | 1 - boards/px4/fmu-v5/stackcheck.px4board | 1 - boards/px4/fmu-v5x/rover.px4board | 1 - boards/px4/fmu-v6c/rover.px4board | 1 - boards/px4/fmu-v6u/rover.px4board | 1 - boards/px4/fmu-v6x/multicopter.px4board | 1 - boards/px4/fmu-v6x/rover.px4board | 1 - boards/px4/fmu-v6xrt/rover.px4board | 1 - boards/px4/sitl/default.px4board | 1 - src/modules/ekf2/Kconfig | 2 +- 10 files changed, 1 insertion(+), 10 deletions(-) diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index b5f586406e..2c9e2d20d3 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -16,7 +16,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index f598bbb5d9..e8bcadebd7 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -45,4 +45,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index bc4c7d5d3f..42f8ec21ce 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6x/multicopter.px4board b/boards/px4/fmu-v6x/multicopter.px4board index 683220e0d7..9f540f567d 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -7,7 +7,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_COMMON_RC=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 9fdee39f1e..f9fea7df79 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef6..5636d855e5 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 93f96a656c..04bcfc104c 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -34,7 +34,7 @@ depends on MODULES_EKF2 menuconfig EKF2_AUX_GLOBAL_POSITION depends on MODULES_EKF2 bool "aux global position fusion support" - default n + default y ---help--- EKF2 auxiliary global position fusion support. From a16f7859ac00b0c1c207789030ca9e5dd41aed04 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 13 Jan 2025 13:39:43 +0100 Subject: [PATCH 17/94] boards: disable CONFIG_EKF2_AUX_GLOBAL_POSITION on some boards Signed-off-by: Silvan Fuhrer --- boards/holybro/kakutef7/default.px4board | 1 + boards/holybro/kakuteh7/default.px4board | 1 + boards/holybro/kakuteh7mini/default.px4board | 1 + boards/holybro/kakuteh7v2/default.px4board | 1 + boards/modalai/fc-v1/default.px4board | 1 + boards/px4/fmu-v2/default.px4board | 1 + boards/px4/fmu-v5/cryptotest.px4board | 1 + 7 files changed, 7 insertions(+) diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 49b8f9e0d5..75e689ed42 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 39ed637d33..12259d0f34 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index fe8405e4d5..b5b1ab5275 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index e4ca03c8b5..a023dc8d3d 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index e9104cfaee..fbd4d3fdf0 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -104,3 +104,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 479e394f6f..2e2350dd5a 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index cc31ed3c2e..a74698d42d 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_BOARD_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y CONFIG_DRIVERS_SW_CRYPTO=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" From e89a79b382e7688034450b7da767a78a178ed6b9 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 10 Jan 2025 17:09:01 -0700 Subject: [PATCH 18/94] boards: disable multi ekf on all ark flight controllers --- boards/ark/fmu-v6x/init/rc.board_defaults | 3 +++ boards/ark/pi6x/init/rc.board_defaults | 3 ++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 52d098101d..d0732e90ef 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -3,6 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default EKF2_MULTI_IMU 0 + # Mavlink ethernet (CFG 1000) param set-default MAV_2_CONFIG 1000 param set-default MAV_2_BROADCAST 1 @@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 param set-default SENS_EN_THERMAL 1 +param set-default SENS_IMU_MODE 1 param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_FF 0.0 #param set-default SENS_IMU_TEMP_I 0.025 diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults index 717632839b..1b92949d1d 100644 --- a/boards/ark/pi6x/init/rc.board_defaults +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -32,10 +32,11 @@ then param set-default SENS_TEMP_ID 2490378 fi -param set-default EKF2_MULTI_IMU 2 +param set-default EKF2_MULTI_IMU 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_OF_N_MIN 0.05 param set-default EKF2_RNG_A_HMAX 25 param set-default EKF2_RNG_QLTY_T 0.1 param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_MODE 1 From 50092a7f67606967419a839a3fe192c59aa05735 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Jan 2025 09:53:32 +0100 Subject: [PATCH 19/94] NuttX: update submodule to branch px4_firmware_nuttx-10.3.0+ with "FlexSPI allow RWW" merged --- platforms/nuttx/NuttX/nuttx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 4c875d0fdf..9a213327fb 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 4c875d0fdffb4f9ba0e0e341ae567d4d0a544e46 +Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c From 940fe45ba7e92af35c2a73355198f9be55b6cf51 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 19:53:44 +0100 Subject: [PATCH 20/94] ControlAllocator: introduce helicopter rotor rpm controller --- msg/CMakeLists.txt | 1 + msg/PwmInput.msg | 8 +- msg/RpmControlStatus.msg | 7 + .../ActuatorEffectivenessHelicopter.cpp | 7 +- .../ActuatorEffectivenessHelicopter.hpp | 4 + .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../ActuatorEffectiveness/RpmControl.hpp | 150 ++++++++++++++++++ .../control_allocator/ControlAllocator.cpp | 2 +- src/modules/control_allocator/module.yaml | 35 ++++ 9 files changed, 209 insertions(+), 7 deletions(-) create mode 100644 msg/RpmControlStatus.msg create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7abae77293..d8d2393c00 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,6 +181,7 @@ set(msg_files RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg + RpmControlStatus.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/PwmInput.msg b/msg/PwmInput.msg index fcc7dbe4ac..805d6fbd6b 100644 --- a/msg/PwmInput.msg +++ b/msg/PwmInput.msg @@ -1,4 +1,4 @@ -uint64 timestamp # Time since system start (microseconds) -uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) -uint32 pulse_width # Pulse width, timer counts -uint32 period # Period, timer counts +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts (microseconds) +uint32 period # Period, timer counts (microseconds) diff --git a/msg/RpmControlStatus.msg b/msg/RpmControlStatus.msg new file mode 100644 index 0000000000..c1ed3dd997 --- /dev/null +++ b/msg/RpmControlStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 rpm_raw # measured rpm +float32 rpm_estimate # filtered rpm +float32 rpm_setpoint # desired rpm + +float32 output diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b67..76481da71b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector #include +#include "RpmControl.hpp" + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -131,4 +133,6 @@ private: bool _main_motor_engaged{true}; const ActuatorType _tail_actuator_type; + + RpmControl _rpm_control{this}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2406b81bf8..a203ac9821 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -69,6 +70,7 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D target_link_libraries(ActuatorEffectiveness PRIVATE mathlib + PID ) px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp new file mode 100644 index 0000000000..a5f413178e --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RpmControl.hpp + * + * Control rpm of a helicopter rotor. + * Input: PWM input pulse period from an rpm sensor + * Output: Duty cycle command for the ESC + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +using namespace time_literals; + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + ~RpmControl() = default; + + void setSpoolupProgress(float spoolup_progress) + { + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < .8f) { + _pid.resetIntegral(); + } + } + + float getActuatorCorrection() + { + if (_pwm_input_sub.updated()) { + pwm_input_s pwm_input{}; + + if (_pwm_input_sub.copy(&pwm_input)) { + if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 1.f); + + } else { + _rpm_raw = 0; + } + + _timestamp_last_rpm_measurement = pwm_input.timestamp; + } + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); + _timestamp_last_update = now; + + _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.update(_rpm_raw); + + const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s); + const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f; + + if (no_rpm_pulse_timeout && no_excessive_rpm) { + const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + + } else { + _pid.setGains(0.f, 0.f, 0.f); + } + + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + + float output = _pid.update(_rpm_filter.getState(), dt, true); + + rpm_control_status_s rpm_control_status{}; + rpm_control_status.rpm_raw = _rpm_raw; + rpm_control_status.rpm_estimate = _rpm_filter.getState(); + rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); + rpm_control_status.output = output; + rpm_control_status.timestamp = hrt_absolute_time(); + _rpm_control_status_pub.publish(rpm_control_status); + + // Publish estimated rpm for MAVLink -> UI in ground station + rpm_s rpm{ + .timestamp = hrt_absolute_time(), + .indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm + }; + + _rpm_pub.publish(rpm); + + return output; + } + +private: + uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; + uORB::Publication _rpm_pub {ORB_ID::rpm}; + + float _rpm_raw{0.f}; + float _spoolup_progress{0.f}; + AlphaFilter _rpm_filter; + PID _pid; + hrt_abstime _timestamp_last_update{0}; + hrt_abstime _timestamp_last_rpm_measurement{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_heli_rpm_sp, + (ParamFloat) _param_ca_heli_rpm_p, + (ParamFloat) _param_ca_heli_rpm_i + ) +}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e..0bc3f9c86e 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -314,7 +314,7 @@ ControlAllocator::Run() #endif // Check if parameters have changed - if (_parameter_update_sub.updated() && !_armed) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df4..013b954033 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -528,6 +528,41 @@ parameters: which is mostly the case when the main rotor turns counter-clockwise. type: boolean default: 0 + CA_HELI_RPM_SP: + description: + short: Setpoint for main rotor rpm + long: | + Requires rpm feedback for the controller. + type: float + decimal: 0 + increment: 1 + min: 100 + max: 10000 + default: 1500 + CA_HELI_RPM_P: + description: + short: Proportional gain for rpm control + long: | + Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it. + + motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000 + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 + CA_HELI_RPM_I: + description: + short: Integral gain for rpm control + long: | + Same definition as the proportional gain but for integral. + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 # Others CA_FAILURE_MODE: From ee67e4bb282386c20ea85c7a24d5d7eeee519e35 Mon Sep 17 00:00:00 2001 From: oravla5 Date: Wed, 11 Dec 2024 18:55:57 +0100 Subject: [PATCH 21/94] RpmControl: class clean up --- .../ActuatorEffectiveness/RpmControl.hpp | 53 +++++++------------ 1 file changed, 18 insertions(+), 35 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index a5f413178e..fb509a5715 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -72,33 +72,24 @@ public: float getActuatorCorrection() { - if (_pwm_input_sub.updated()) { - pwm_input_s pwm_input{}; + if (_rpm_sub.updated()) { + rpm_s rpm_input{}; - if (_pwm_input_sub.copy(&pwm_input)) { - if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) { - // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 1.f); - - } else { - _rpm_raw = 0; - } - - _timestamp_last_rpm_measurement = pwm_input.timestamp; + if (_rpm_sub.copy(&rpm_input)) { + _rpm_estimate = rpm_input.rpm_estimate; + _rpm_raw = rpm_input.rpm_raw; + _timestamp_last_rpm_measurement = rpm_input.timestamp; } } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); - _timestamp_last_update = now; + hrt_abstime current_time = hrt_absolute_time(); + const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = current_time; - _rpm_filter.setParameters(dt, 0.5f); - _rpm_filter.update(_rpm_raw); + const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s; + const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; - const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s); - const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f; - - if (no_rpm_pulse_timeout && no_excessive_rpm) { + if (rpm_measurement_timeout && no_excessive_rpm) { const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); @@ -109,38 +100,30 @@ public: _pid.setOutputLimit(.5f); _pid.setIntegralLimit(.5f); - float output = _pid.update(_rpm_filter.getState(), dt, true); + float output = _pid.update(_rpm_estimate, dt, true); rpm_control_status_s rpm_control_status{}; rpm_control_status.rpm_raw = _rpm_raw; - rpm_control_status.rpm_estimate = _rpm_filter.getState(); + rpm_control_status.rpm_estimate = _rpm_estimate;; rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); rpm_control_status.output = output; rpm_control_status.timestamp = hrt_absolute_time(); _rpm_control_status_pub.publish(rpm_control_status); - // Publish estimated rpm for MAVLink -> UI in ground station - rpm_s rpm{ - .timestamp = hrt_absolute_time(), - .indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm - }; - - _rpm_pub.publish(rpm); - return output; } private: - uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + static constexpr float RPM_MAX_VALUE = 1800.f; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; - uORB::Publication _rpm_pub {ORB_ID::rpm}; + float _rpm_estimate{0.f}; float _rpm_raw{0.f}; float _spoolup_progress{0.f}; - AlphaFilter _rpm_filter; PID _pid; - hrt_abstime _timestamp_last_update{0}; hrt_abstime _timestamp_last_rpm_measurement{0}; + hrt_abstime _timestamp_last_update{0}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_heli_rpm_sp, From cd0e04f8b07df05b388f31c1ef24a67cd2cb675f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:18:51 +0100 Subject: [PATCH 22/94] RpmControl: name current timestamp now following the convention --- .../ActuatorEffectiveness/RpmControl.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index fb509a5715..c4485c57ec 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -82,11 +82,11 @@ public: } } - hrt_abstime current_time = hrt_absolute_time(); - const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = current_time; + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = now; - const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s; + const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; if (rpm_measurement_timeout && no_excessive_rpm) { From 4050cedfafc996b9b40a16449196c6089f996ca9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:19:53 +0100 Subject: [PATCH 23/94] RpmControl: call local message instance after message name following the convention --- .../ActuatorEffectiveness/RpmControl.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index c4485c57ec..9bdbf562b2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -73,12 +73,12 @@ public: float getActuatorCorrection() { if (_rpm_sub.updated()) { - rpm_s rpm_input{}; + rpm_s rpm{}; - if (_rpm_sub.copy(&rpm_input)) { - _rpm_estimate = rpm_input.rpm_estimate; - _rpm_raw = rpm_input.rpm_raw; - _timestamp_last_rpm_measurement = rpm_input.timestamp; + if (_rpm_sub.copy(&rpm)) { + _rpm_estimate = rpm.rpm_estimate; + _rpm_raw = rpm.rpm_raw; + _timestamp_last_rpm_measurement = rpm.timestamp; } } From ddd410e9d8aa114a2cba2c864863f757af08d3df Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:27:59 +0100 Subject: [PATCH 24/94] RpmControl: remove status message because it by now only contains redundant information --- msg/CMakeLists.txt | 1 - msg/RpmControlStatus.msg | 7 ------- .../ActuatorEffectiveness/RpmControl.hpp | 12 ------------ 3 files changed, 20 deletions(-) delete mode 100644 msg/RpmControlStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d8d2393c00..7abae77293 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,7 +181,6 @@ set(msg_files RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg - RpmControlStatus.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/RpmControlStatus.msg b/msg/RpmControlStatus.msg deleted file mode 100644 index c1ed3dd997..0000000000 --- a/msg/RpmControlStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 rpm_raw # measured rpm -float32 rpm_estimate # filtered rpm -float32 rpm_setpoint # desired rpm - -float32 output diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 9bdbf562b2..9af733995d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -50,7 +50,6 @@ #include #include #include -#include using namespace time_literals; @@ -77,7 +76,6 @@ public: if (_rpm_sub.copy(&rpm)) { _rpm_estimate = rpm.rpm_estimate; - _rpm_raw = rpm.rpm_raw; _timestamp_last_rpm_measurement = rpm.timestamp; } } @@ -102,24 +100,14 @@ public: float output = _pid.update(_rpm_estimate, dt, true); - rpm_control_status_s rpm_control_status{}; - rpm_control_status.rpm_raw = _rpm_raw; - rpm_control_status.rpm_estimate = _rpm_estimate;; - rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); - rpm_control_status.output = output; - rpm_control_status.timestamp = hrt_absolute_time(); - _rpm_control_status_pub.publish(rpm_control_status); - return output; } private: static constexpr float RPM_MAX_VALUE = 1800.f; uORB::Subscription _rpm_sub{ORB_ID(rpm)}; - uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; float _rpm_estimate{0.f}; - float _rpm_raw{0.f}; float _spoolup_progress{0.f}; PID _pid; hrt_abstime _timestamp_last_rpm_measurement{0}; From 2772ae7e0e38da0b55c5d2c8069a471a72b73c0a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:17:12 +0100 Subject: [PATCH 25/94] RpmControl: maximum rpm outliers are now caught by RpmCapture --- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 9af733995d..396183382f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -85,9 +85,8 @@ public: _timestamp_last_update = now; const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; - if (rpm_measurement_timeout && no_excessive_rpm) { + if (rpm_measurement_timeout) { const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); @@ -104,7 +103,6 @@ public: } private: - static constexpr float RPM_MAX_VALUE = 1800.f; uORB::Subscription _rpm_sub{ORB_ID(rpm)}; float _rpm_estimate{0.f}; From 1c4325db6d299eb11454bcbbfdfa043ff4d188b8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:30:43 +0100 Subject: [PATCH 26/94] RpmControl: split into cpp source file fixing includes --- .../ActuatorEffectiveness/CMakeLists.txt | 1 + .../ActuatorEffectiveness/RpmControl.cpp | 83 +++++++++++++++++++ .../ActuatorEffectiveness/RpmControl.hpp | 52 +----------- 3 files changed, 88 insertions(+), 48 deletions(-) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index a203ac9821..10ff984b29 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.cpp RpmControl.hpp ) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 0000000000..56bccfe779 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RpmControl.hpp" + +#include + +using namespace time_literals; + +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + +void RpmControl::setSpoolupProgress(float spoolup_progress) +{ + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < .8f) { + _pid.resetIntegral(); + } +} + +float RpmControl::getActuatorCorrection() +{ + if (_rpm_sub.updated()) { + rpm_s rpm{}; + + if (_rpm_sub.copy(&rpm)) { + _rpm_estimate = rpm.rpm_estimate; + _timestamp_last_rpm_measurement = rpm.timestamp; + } + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = now; + + const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; + + if (rpm_measurement_timeout) { + const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + + } else { + _pid.setGains(0.f, 0.f, 0.f); + } + + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + + float output = _pid.update(_rpm_estimate, dt, true); + + return output; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 396183382f..b5930902db 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -43,64 +43,20 @@ #pragma once -#include #include - +#include #include #include -#include #include -using namespace time_literals; - class RpmControl : public ModuleParams { public: - RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + RpmControl(ModuleParams *parent); ~RpmControl() = default; - void setSpoolupProgress(float spoolup_progress) - { - _spoolup_progress = spoolup_progress; - _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); - - if (_spoolup_progress < .8f) { - _pid.resetIntegral(); - } - } - - float getActuatorCorrection() - { - if (_rpm_sub.updated()) { - rpm_s rpm{}; - - if (_rpm_sub.copy(&rpm)) { - _rpm_estimate = rpm.rpm_estimate; - _timestamp_last_rpm_measurement = rpm.timestamp; - } - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = now; - - const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - - if (rpm_measurement_timeout) { - const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; - _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); - - } else { - _pid.setGains(0.f, 0.f, 0.f); - } - - _pid.setOutputLimit(.5f); - _pid.setIntegralLimit(.5f); - - float output = _pid.update(_rpm_estimate, dt, true); - - return output; - } + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); private: uORB::Subscription _rpm_sub{ORB_ID(rpm)}; From 2506bd3b5dea49042fafe9aca4f16d23437a02c0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 18:39:15 +0100 Subject: [PATCH 27/94] RpmControl: simplify the entire control logic --- .../ActuatorEffectiveness/RpmControl.cpp | 42 +++++++++---------- .../ActuatorEffectiveness/RpmControl.hpp | 11 ++--- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index 56bccfe779..f61a392ad4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -37,47 +37,45 @@ using namespace time_literals; -RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pid.setOutputLimit(PID_OUTPUT_LIMIT); + _pid.setIntegralLimit(PID_OUTPUT_LIMIT); +}; void RpmControl::setSpoolupProgress(float spoolup_progress) { _spoolup_progress = spoolup_progress; _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); - if (_spoolup_progress < .8f) { + if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) { _pid.resetIntegral(); } } float RpmControl::getActuatorCorrection() { + hrt_abstime now = hrt_absolute_time(); + + // RPM measurement update if (_rpm_sub.updated()) { rpm_s rpm{}; if (_rpm_sub.copy(&rpm)) { - _rpm_estimate = rpm.rpm_estimate; - _timestamp_last_rpm_measurement = rpm.timestamp; + const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f); + _timestamp_last_measurement = rpm.timestamp; + + const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); } } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = now; - - const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - - if (rpm_measurement_timeout) { - const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; - _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); - - } else { - _pid.setGains(0.f, 0.f, 0.f); + // Timeout + if (now > _timestamp_last_measurement + 1_s) { + _pid.resetIntegral(); + _actuator_correction = 0.f; } - _pid.setOutputLimit(.5f); - _pid.setIntegralLimit(.5f); - - float output = _pid.update(_rpm_estimate, dt, true); - - return output; + return _actuator_correction; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b5930902db..b412445048 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -59,13 +59,14 @@ public: float getActuatorCorrection(); private: - uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] - float _rpm_estimate{0.f}; - float _spoolup_progress{0.f}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; PID _pid; - hrt_abstime _timestamp_last_rpm_measurement{0}; - hrt_abstime _timestamp_last_update{0}; + float _spoolup_progress{0.f}; // [0,1] + hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout + float _actuator_correction{0.f}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_heli_rpm_sp, From 5e2848312d2a074bb6936be97b72712a237f58af Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 19:28:20 +0100 Subject: [PATCH 28/94] Commander: start timer for auto disarm after spoolup --- src/modules/commander/Commander.cpp | 3 ++- src/modules/commander/Commander.hpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5fc8ee4d8f..fef03b92c2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, + (_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index cf69743af6..04c1a0d05a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -330,7 +330,6 @@ private: param_t _param_rc_map_fltmode{PARAM_INVALID}; DEFINE_PARAMETERS( - (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, @@ -347,6 +346,7 @@ private: (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, + (ParamFloat) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max From b584f8381c5ce7b004ba442c454d92339565214d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 20:54:20 +0100 Subject: [PATCH 29/94] Helicopter defaults: don't auto disarm so quickly after spoolup --- ROMFS/px4fmu_common/init.d/rc.heli_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca268..93e6f0d33f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -12,6 +12,7 @@ param set-default MAV_TYPE 4 param set-default COM_PREARM_MODE 2 param set-default COM_SPOOLUP_TIME 10 +param set-default COM_DISARM_PRFLT 60 # No need for minimum collective pitch (or airmode) to keep torque authority param set-default MPC_MANTHR_MIN 0 From bc92008885f82966578ec2484af6e0e8eae0d756 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 21:02:51 +0100 Subject: [PATCH 30/94] RpmControl: Better consider the case where there's no rpm measurement (anymore) --- msg/Rpm.msg | 1 + .../control_allocator/ActuatorEffectiveness/RpmControl.cpp | 6 +++++- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index ca69e50fdb..b4de2cf422 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +# rpm values of 0.0 mean within a timeout there is no movement measured float32 rpm_estimate # filtered revolutions per minute float32 rpm_raw diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index f61a392ad4..53d9766e2d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -68,11 +68,15 @@ float RpmControl::getActuatorCorrection() const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; } } // Timeout - if (now > _timestamp_last_measurement + 1_s) { + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { _pid.resetIntegral(); _actuator_correction = 0.f; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b412445048..5fd0c96d91 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -63,6 +63,7 @@ private: static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; PID _pid; float _spoolup_progress{0.f}; // [0,1] hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout From 0723f75993d6dc37d4b4a7aa2c87f4da8679a72b Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Thu, 19 Dec 2024 08:06:58 -0800 Subject: [PATCH 31/94] ci: move to px4 git tag versions --- .github/workflows/dev_container.yml | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index bcfbdc2c6b..f64d29b684 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -17,7 +17,7 @@ on: jobs: build: name: Build and Push Container - runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] steps: - uses: actions/checkout@v4 with: @@ -52,12 +52,7 @@ jobs: ghcr.io/PX4/px4-dev ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }} tags: | - type=semver,pattern={{version}} - type=semver,pattern={{major}}.{{minor}} - type=semver,pattern={{major}} - type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700 - type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600 - type=ref,event=branch,suffix=,priority=500 + type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 From 12a9087e92cea4dd9e8741f8d7b237fa92a1fea5 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 14 Jan 2025 12:33:32 +0100 Subject: [PATCH 32/94] ekf2: constrain max variance by zero innovation update Clipping the variance of the covariance matrix has a destabilizing effect as it increases the correlation between the states. --- src/modules/ekf2/EKF/covariance.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 0c9a5df6cb..e7b2eea396 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -280,7 +280,17 @@ void Ekf::constrainStateVariances() void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), min, max); + if (P(i, i) < min) { + P(i, i) = min; + + } else if (P(i, i) > max) { + // Constrain the variance growth by fusing zero innovation as clipping the variance + // would artifically increase the correlation between states and destabilize the filter. + const float innov = 0.f; + const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) + const float innov_var = P(i, i) + R; + fuseDirectStateMeasurement(innov, innov_var, R, i); + } } } @@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float limited_max = math::constrain(state_var_max, min, max); float limited_min = math::constrain(limited_max / max_ratio, min, max); - for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), limited_min, limited_max); - } + constrainStateVar(state, limited_min, limited_max); } void Ekf::resetQuatCov(const float yaw_noise) From 9f8325e8e06c51192251686cf87a42645715c4be Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 15 Jan 2025 00:39:11 +0000 Subject: [PATCH 33/94] Update submodule mavlink to latest Wed Jan 15 00:39:11 UTC 2025 - mavlink in PX4/Firmware (fd5b52d4c53f35a520646a6c4ec75588f6b87e0f): https://github.com/mavlink/mavlink/commit/5e3a42b8f3f53038f2779f9f69bd64767b913bb8 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/619947d8bc29e80eecff18b0f4fecc42d9e171dd - Changes: https://github.com/mavlink/mavlink/compare/5e3a42b8f3f53038f2779f9f69bd64767b913bb8...619947d8bc29e80eecff18b0f4fecc42d9e171dd 619947d8 2024-12-19 Hamish Willee - common.xml - PING fix (#2197) 2f44ceff 2024-12-18 Julian Oes - common: use camera ID for CAMERA_IMAGE_CAPTURED (#2196) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 5e3a42b8f3..619947d8bc 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 5e3a42b8f3f53038f2779f9f69bd64767b913bb8 +Subproject commit 619947d8bc29e80eecff18b0f4fecc42d9e171dd From 3064a4ad4c8d14a5debe7ed564ddd36a22a72701 Mon Sep 17 00:00:00 2001 From: Minderring <1701213518@sz.edu.pku.cn> Date: Tue, 14 Jan 2025 21:44:01 +0800 Subject: [PATCH 34/94] boards configs: add airspeed driver for micoair743 aio and v2 --- boards/micoair/h743-aio/default.px4board | 6 +++++- boards/micoair/h743-v2/default.px4board | 6 +++++- boards/micoair/h743/default.px4board | 6 +++++- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index c912a7a6cb..2d92e89d96 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index 330e0fa594..fed1a0756b 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -11,11 +11,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index ef7458f7c3..149bcaff21 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y From 974446c0e8f8e06255f7c1a5a8a846f781a94357 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Wed, 15 Jan 2025 10:12:29 +0100 Subject: [PATCH 35/94] Make control allocation and actuator effectiveness a non-module-specific library (#24196) * Remove more circular dependencies with ActuatorEffectiveness * Separate vehicle specific actuator effectiveness Keep actuator effectivenss in control allocator * Remove test dependency for now * Group library directories Fix * Change directory names * Rebase fix --- src/lib/CMakeLists.txt | 1 + src/lib/control_allocation/CMakeLists.txt | 35 +++++++++++++++ .../ActuatorEffectiveness.cpp | 0 .../ActuatorEffectiveness.hpp | 0 .../actuator_effectiveness}/CMakeLists.txt | 33 -------------- .../control_allocation}/CMakeLists.txt | 2 +- .../control_allocation}/ControlAllocation.cpp | 0 .../control_allocation}/ControlAllocation.hpp | 2 +- .../ControlAllocationPseudoInverse.cpp | 0 .../ControlAllocationPseudoInverse.hpp | 0 .../ControlAllocationPseudoInverseTest.cpp | 0 ...ontrolAllocationSequentialDesaturation.cpp | 0 ...ontrolAllocationSequentialDesaturation.hpp | 0 ...olAllocationSequentialDesaturationTest.cpp | 2 +- src/modules/control_allocator/CMakeLists.txt | 4 +- .../ActuatorEffectivenessControlSurfaces.cpp | 0 .../ActuatorEffectivenessControlSurfaces.hpp | 2 +- .../ActuatorEffectivenessCustom.cpp | 0 .../ActuatorEffectivenessCustom.hpp | 2 +- .../ActuatorEffectivenessFixedWing.cpp | 0 .../ActuatorEffectivenessFixedWing.hpp | 2 +- .../ActuatorEffectivenessHelicopter.cpp | 0 .../ActuatorEffectivenessHelicopter.hpp | 2 +- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 0 ...ActuatorEffectivenessHelicopterCoaxial.hpp | 2 +- .../ActuatorEffectivenessHelicopterTest.cpp | 0 .../ActuatorEffectivenessMCTilt.cpp | 0 .../ActuatorEffectivenessMCTilt.hpp | 2 +- .../ActuatorEffectivenessMultirotor.cpp | 0 .../ActuatorEffectivenessMultirotor.hpp | 2 +- .../ActuatorEffectivenessRotors.cpp | 0 .../ActuatorEffectivenessRotors.hpp | 2 +- .../ActuatorEffectivenessRotorsTest.cpp | 0 .../ActuatorEffectivenessRoverAckermann.cpp | 0 .../ActuatorEffectivenessRoverAckermann.hpp | 2 +- .../ActuatorEffectivenessStandardVTOL.cpp | 0 .../ActuatorEffectivenessStandardVTOL.hpp | 2 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 0 .../ActuatorEffectivenessTailsitterVTOL.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 0 .../ActuatorEffectivenessTiltrotorVTOL.hpp | 2 +- .../ActuatorEffectivenessTilts.cpp | 0 .../ActuatorEffectivenessTilts.hpp | 2 +- .../ActuatorEffectivenessUUV.cpp | 0 .../ActuatorEffectivenessUUV.hpp | 2 +- .../CMakeLists.txt | 43 +++++++++++++++++++ .../RpmControl.cpp | 0 .../RpmControl.hpp | 0 48 files changed, 98 insertions(+), 52 deletions(-) create mode 100644 src/lib/control_allocation/CMakeLists.txt rename src/{modules/control_allocator/ActuatorEffectiveness => lib/control_allocation/actuator_effectiveness}/ActuatorEffectiveness.cpp (100%) rename src/{modules/control_allocator/ActuatorEffectiveness => lib/control_allocation/actuator_effectiveness}/ActuatorEffectiveness.hpp (100%) rename src/{modules/control_allocator/ActuatorEffectiveness => lib/control_allocation/actuator_effectiveness}/CMakeLists.txt (61%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/CMakeLists.txt (94%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocation.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocation.hpp (99%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationPseudoInverse.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationPseudoInverse.hpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationPseudoInverseTest.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationSequentialDesaturation.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationSequentialDesaturation.hpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationSequentialDesaturationTest.cpp (99%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessControlSurfaces.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessControlSurfaces.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessCustom.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessCustom.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessFixedWing.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessFixedWing.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopter.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopter.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopterCoaxial.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopterCoaxial.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopterTest.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMCTilt.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMCTilt.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMultirotor.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMultirotor.hpp (96%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRotors.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRotors.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRotorsTest.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRoverAckermann.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRoverAckermann.hpp (96%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessStandardVTOL.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessStandardVTOL.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTailsitterVTOL.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTailsitterVTOL.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTiltrotorVTOL.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTiltrotorVTOL.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTilts.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTilts.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessUUV.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessUUV.hpp (97%) create mode 100644 src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/RpmControl.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/RpmControl.hpp (100%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 69ab23aacc..8f58109b89 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL) add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL) add_subdirectory(collision_prevention EXCLUDE_FROM_ALL) add_subdirectory(component_information EXCLUDE_FROM_ALL) +add_subdirectory(control_allocation EXCLUDE_FROM_ALL) add_subdirectory(controllib EXCLUDE_FROM_ALL) add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) diff --git a/src/lib/control_allocation/CMakeLists.txt b/src/lib/control_allocation/CMakeLists.txt new file mode 100644 index 0000000000..f7c710b7f2 --- /dev/null +++ b/src/lib/control_allocation/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# Copyright (c) 2025 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. +# +############################################################################ + +add_subdirectory(control_allocation) +add_subdirectory(actuator_effectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt similarity index 61% rename from src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt rename to src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt index 10ff984b29..b8a3d0076d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt @@ -34,36 +34,6 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp - ActuatorEffectivenessUUV.cpp - ActuatorEffectivenessUUV.hpp - ActuatorEffectivenessControlSurfaces.cpp - ActuatorEffectivenessControlSurfaces.hpp - ActuatorEffectivenessCustom.cpp - ActuatorEffectivenessCustom.hpp - ActuatorEffectivenessFixedWing.cpp - ActuatorEffectivenessFixedWing.hpp - ActuatorEffectivenessHelicopter.cpp - ActuatorEffectivenessHelicopter.hpp - ActuatorEffectivenessHelicopterCoaxial.cpp - ActuatorEffectivenessHelicopterCoaxial.hpp - ActuatorEffectivenessMCTilt.cpp - ActuatorEffectivenessMCTilt.hpp - ActuatorEffectivenessMultirotor.cpp - ActuatorEffectivenessMultirotor.hpp - ActuatorEffectivenessTilts.cpp - ActuatorEffectivenessTilts.hpp - ActuatorEffectivenessRotors.cpp - ActuatorEffectivenessRotors.hpp - ActuatorEffectivenessStandardVTOL.cpp - ActuatorEffectivenessStandardVTOL.hpp - ActuatorEffectivenessTiltrotorVTOL.cpp - ActuatorEffectivenessTiltrotorVTOL.hpp - ActuatorEffectivenessTailsitterVTOL.cpp - ActuatorEffectivenessTailsitterVTOL.hpp - ActuatorEffectivenessRoverAckermann.hpp - ActuatorEffectivenessRoverAckermann.cpp - RpmControl.cpp - RpmControl.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -73,6 +43,3 @@ target_link_libraries(ActuatorEffectiveness mathlib PID ) - -px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) -px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt similarity index 94% rename from src/modules/control_allocator/ControlAllocation/CMakeLists.txt rename to src/lib/control_allocation/control_allocation/CMakeLists.txt index 4da638aac8..d65bd11fa0 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) -px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.hpp index c60784a03c..a6e2e2b254 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -71,7 +71,7 @@ #include -#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ControlAllocation { diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 60392330c9..2e0af6bff4 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,7 +40,7 @@ #include #include -#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp> +#include using namespace matrix; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 65872dc432..36d5c7e171 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -32,8 +32,7 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(ActuatorEffectiveness) -add_subdirectory(ControlAllocation) +add_subdirectory(VehicleActuatorEffectiveness) px4_add_module( MODULE modules__control_allocator @@ -50,6 +49,7 @@ px4_add_module( DEPENDS mathlib ActuatorEffectiveness + VehicleActuatorEffectiveness ControlAllocation px4_work_queue SlewRate diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index a0b8b06c16..7047363cc8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index 0cc1390bd2..b7906669f2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 5b4b7785b2..f0b095709e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 3c3193906b..565c8931c9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index d5316bf498..a507aee2dd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 848c8b8853..0b12482781 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index e0a355edd2..e5b7f3dcf8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 3844df4c84..c6f0425569 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 294e453ec6..f6108b4baf 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 0e5d1bfa44..9f945a6cd8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 604f05f9e0..708f104faa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -39,7 +39,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 310d937064..512bd41f41 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index d885a09155..1255d1b282 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 57e86f9436..1b89da8d9b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 0000000000..349893a3ea --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,43 @@ +px4_add_library(VehicleActuatorEffectiveness + ActuatorEffectivenessUUV.cpp + ActuatorEffectivenessUUV.hpp + ActuatorEffectivenessControlSurfaces.cpp + ActuatorEffectivenessControlSurfaces.hpp + ActuatorEffectivenessCustom.cpp + ActuatorEffectivenessCustom.hpp + ActuatorEffectivenessFixedWing.cpp + ActuatorEffectivenessFixedWing.hpp + ActuatorEffectivenessHelicopter.cpp + ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp + ActuatorEffectivenessMCTilt.cpp + ActuatorEffectivenessMCTilt.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessTilts.cpp + ActuatorEffectivenessTilts.hpp + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectivenessTiltrotorVTOL.hpp + ActuatorEffectivenessTailsitterVTOL.cpp + ActuatorEffectivenessTailsitterVTOL.hpp + ActuatorEffectivenessRoverAckermann.hpp + ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp + RpmControl.cpp +) + +target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(VehicleActuatorEffectiveness + PRIVATE + mathlib + ActuatorEffectiveness +) + +px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS VehicleActuatorEffectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp From 8ecb76aba22a54e803edfedb9145290b92b3544f Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Wed, 15 Jan 2025 14:02:28 +0100 Subject: [PATCH 36/94] [Multirotor] add yaw torque low pass filter (#24173) co-authored-by: danielmellinger <107884356+danielmellinger@users.noreply.github.com> co-authored-by: Eric Katzfey --- src/lib/mathlib/math/filter/AlphaFilter.hpp | 10 ++++++++++ .../mc_rate_control/MulticopterRateControl.cpp | 7 ++++++- .../mc_rate_control/MulticopterRateControl.hpp | 4 ++++ .../mc_rate_control/mc_rate_control_params.c | 14 ++++++++++++++ 4 files changed, 34 insertions(+), 1 deletion(-) diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index b7a6e86953..b80e2a73f5 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -89,6 +89,16 @@ public: return true; } + void setCutoffFreq(float cutoff_freq) + { + if (cutoff_freq > FLT_EPSILON) { + _time_constant = 1.f / (M_TWOPI_F * cutoff_freq); + + } else { + _time_constant = 0.f; + } + } + /** * Set filter parameter alpha directly without time abstraction * diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index fea041022e..3931125289 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -95,6 +95,8 @@ MulticopterRateControl::parameters_updated() // manual rate control acro mode rate limits _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get())); + + _output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get()); } void @@ -214,9 +216,12 @@ MulticopterRateControl::Run() } // run rate controller - const Vector3f torque_setpoint = + Vector3f torque_setpoint = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + // apply low-pass filtering on yaw axis to reduce high frequency torque caused by rotor acceleration + torque_setpoint(2) = _output_lpf_yaw.update(torque_setpoint(2), dt); + // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; _rate_control.getRateControlStatus(rate_ctrl_status); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 0f402c12c3..1a2427da18 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -129,6 +130,8 @@ private: float _energy_integration_time{0.0f}; float _control_energy[4] {}; + AlphaFilter _output_lpf_yaw; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, @@ -150,6 +153,7 @@ private: (ParamFloat) _param_mc_yawrate_d, (ParamFloat) _param_mc_yawrate_ff, (ParamFloat) _param_mc_yawrate_k, + (ParamFloat) _param_mc_yaw_tq_cutoff, (ParamFloat) _param_mc_acro_r_max, (ParamFloat) _param_mc_acro_p_max, diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 2571a70c1f..b37c52e746 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -292,3 +292,17 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); * @group Multicopter Rate Control */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * Low pass filter cutoff frequency for yaw torque setpoint + * + * Reduces vibrations by lowering high frequency torque caused by rotor acceleration. + * 0 disables the filter + * + * @min 0 + * @max 10 + * @unit Hz + * @decimal 3 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAW_TQ_CUTOFF, 2.f); From e01fef755abe864eace390ab979eb7666d85d197 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 15 Jan 2025 14:07:47 +0100 Subject: [PATCH 37/94] Control allocation: make heli rpm control an optional build flag disabled by default to save flash. The rpm capture dirver is also disabled on default releases --- src/modules/control_allocator/Kconfig | 7 +++++++ .../ActuatorEffectivenessHelicopter.cpp | 8 ++++++-- .../ActuatorEffectivenessHelicopter.hpp | 4 +++- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/src/modules/control_allocator/Kconfig b/src/modules/control_allocator/Kconfig index 4352269d50..63c1c61500 100644 --- a/src/modules/control_allocator/Kconfig +++ b/src/modules/control_allocator/Kconfig @@ -10,3 +10,10 @@ menuconfig USER_CONTROL_ALLOCATOR depends on BOARD_PROTECTED && MODULES_CONTROL_ALLOCATOR ---help--- Put control_allocator in userspace memory + +menuconfig CONTROL_ALLOCATOR_RPM_CONTROL + bool "Include RPM control for Helicopter rotor" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Add support for controlling the helicopter main rotor rpm diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 76481da71b..643b218c47 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -135,11 +135,15 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector Date: Wed, 15 Jan 2025 14:30:19 +0100 Subject: [PATCH 38/94] ekf2: do not auto-generate sideslip measurement jacobian This is to trade a bit of CPU load for more flash space. --- .../aid_sources/sideslip/sideslip_fusion.cpp | 9 +- .../EKF/python/ekf_derivation/derivation.py | 10 +- .../generated/compute_sideslip_h.h | 96 +++++++++ .../generated/compute_sideslip_h_and_k.h | 188 ------------------ 4 files changed, 102 insertions(+), 201 deletions(-) create mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h delete mode 100644 src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index a75c1bb54e..b2948629e4 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -43,7 +43,7 @@ #include "ekf.h" #include -#include +#include #include @@ -127,10 +127,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; const float epsilon = 1e-3f; - VectorState H; // Observation jacobian - VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K); + const VectorState H = sym::ComputeSideslipH(_state.vector(), epsilon); + VectorState K = P * H / sideslip.innovation_variance; if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); @@ -143,7 +142,5 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) sideslip.fused = true; sideslip.time_last_fuse = _time_delayed_us; - _fault_status.flags.bad_sideslip = false; - return true; } diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 0ad3fef4ed..adbc761ae9 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -354,10 +354,8 @@ def compute_sideslip_innov_and_innov_var( return (innov, innov_var) -def compute_sideslip_h_and_k( +def compute_sideslip_h( state: VState, - P: MTangent, - innov_var: sf.Scalar, epsilon: sf.Scalar ) -> (VTangent, VTangent): @@ -366,9 +364,7 @@ def compute_sideslip_h_and_k( H = jacobian_chain_rule(sideslip_pred, state) - K = P * H.T / sf.Max(innov_var, epsilon) - - return (H.T, K) + return (H.T) def predict_vel_body( state: VState @@ -739,7 +735,7 @@ if not args.disable_wind: generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"]) generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"]) - generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_sideslip_h, output_names=None) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h new file mode 100644 index 0000000000..436b0c07e7 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h @@ -0,0 +1,96 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_sideslip_h + * + * Args: + * state: Matrix25_1 + * epsilon: Scalar + * + * Outputs: + * res: Matrix24_1 + */ +template +matrix::Matrix ComputeSideslipH(const matrix::Matrix& state, + const Scalar epsilon) { + // Total ops: 131 + + // Input arrays + + // Intermediate terms (37) + const Scalar _tmp0 = -state(22, 0) + state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(3, 0); + const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp6 = 2 * state(0, 0); + const Scalar _tmp7 = _tmp6 * state(3, 0); + const Scalar _tmp8 = 2 * state(2, 0); + const Scalar _tmp9 = _tmp8 * state(1, 0); + const Scalar _tmp10 = _tmp7 + _tmp9; + const Scalar _tmp11 = -state(23, 0) + state(5, 0); + const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp8 * state(0, 0); + const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp11 + _tmp12 * state(6, 0); + const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); + const Scalar _tmp15 = Scalar(1.0) / (_tmp14); + const Scalar _tmp16 = _tmp2 * state(0, 0); + const Scalar _tmp17 = std::pow(_tmp14, Scalar(2)); + const Scalar _tmp18 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp19 = -_tmp7 + _tmp9; + const Scalar _tmp20 = _tmp6 * state(1, 0) + _tmp8 * state(3, 0); + const Scalar _tmp21 = _tmp0 * _tmp19 + _tmp11 * _tmp18 + _tmp20 * state(6, 0); + const Scalar _tmp22 = _tmp21 / _tmp17; + const Scalar _tmp23 = _tmp17 / (_tmp17 + std::pow(_tmp21, Scalar(2))); + const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp25 = _tmp24 * (_tmp15 * (_tmp0 * _tmp1 + _tmp3) - + _tmp22 * (-4 * _tmp0 * state(2, 0) + _tmp1 * _tmp11 - _tmp16)); + const Scalar _tmp26 = 2 * state(3, 0); + const Scalar _tmp27 = _tmp2 * state(1, 0); + const Scalar _tmp28 = _tmp2 * state(2, 0); + const Scalar _tmp29 = + _tmp24 * (_tmp15 * (-_tmp0 * _tmp26 + _tmp27) - _tmp22 * (_tmp11 * _tmp26 - _tmp28)); + const Scalar _tmp30 = _tmp24 * (_tmp15 * (_tmp0 * _tmp8 - 4 * _tmp11 * state(1, 0) + _tmp16) - + _tmp22 * (_tmp11 * _tmp8 + _tmp3)); + const Scalar _tmp31 = 4 * state(3, 0); + const Scalar _tmp32 = _tmp24 * (_tmp15 * (-_tmp0 * _tmp6 - _tmp11 * _tmp31 + _tmp28) - + _tmp22 * (-_tmp0 * _tmp31 + _tmp11 * _tmp6 + _tmp27)); + const Scalar _tmp33 = _tmp22 * _tmp5; + const Scalar _tmp34 = _tmp15 * _tmp19; + const Scalar _tmp35 = _tmp15 * _tmp18; + const Scalar _tmp36 = _tmp10 * _tmp22; + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(0, 0) = + -_tmp25 * state(3, 0) - _tmp29 * state(1, 0) + _tmp30 * state(0, 0) + _tmp32 * state(2, 0); + _res(1, 0) = + _tmp25 * state(0, 0) - _tmp29 * state(2, 0) + _tmp30 * state(3, 0) - _tmp32 * state(1, 0); + _res(2, 0) = + _tmp25 * state(1, 0) - _tmp29 * state(3, 0) - _tmp30 * state(2, 0) + _tmp32 * state(0, 0); + _res(3, 0) = _tmp23 * (-_tmp33 + _tmp34); + _res(4, 0) = _tmp23 * (_tmp35 - _tmp36); + _res(5, 0) = _tmp23 * (-_tmp12 * _tmp22 + _tmp15 * _tmp20); + _res(21, 0) = _tmp23 * (_tmp33 - _tmp34); + _res(22, 0) = _tmp23 * (-_tmp35 + _tmp36); + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h deleted file mode 100644 index 6181936f8f..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ /dev/null @@ -1,188 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_sideslip_h_and_k - * - * Args: - * state: Matrix25_1 - * P: Matrix24_24 - * innov_var: Scalar - * epsilon: Scalar - * - * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 - */ -template -void ComputeSideslipHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 518 - - // Input arrays - - // Intermediate terms (50) - const Scalar _tmp0 = -state(22, 0) + state(4, 0); - const Scalar _tmp1 = 2 * _tmp0; - const Scalar _tmp2 = 2 * state(3, 0); - const Scalar _tmp3 = _tmp2 * state(6, 0); - const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp6 = _tmp2 * state(0, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = 2 * state(2, 0); - const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0); - const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0); - const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); - const Scalar _tmp15 = Scalar(1.0) / (_tmp14); - const Scalar _tmp16 = 2 * _tmp10; - const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0); - const Scalar _tmp18 = std::pow(_tmp14, Scalar(2)); - const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp20 = -_tmp6 + _tmp8; - const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0); - const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0); - const Scalar _tmp23 = _tmp22 / _tmp18; - const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) - - _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17); - const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2))); - const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25; - const Scalar _tmp27 = _tmp26 * state(3, 0); - const Scalar _tmp28 = _tmp7 * state(6, 0); - const Scalar _tmp29 = _tmp11 * state(6, 0); - const Scalar _tmp30 = - _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29); - const Scalar _tmp31 = _tmp26 * state(1, 0); - const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) - - _tmp23 * (_tmp16 * state(2, 0) + _tmp3); - const Scalar _tmp33 = _tmp26 * state(0, 0); - const Scalar _tmp34 = 4 * state(3, 0); - const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) - - _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28); - const Scalar _tmp36 = _tmp26 * state(2, 0); - const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36; - const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35; - const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35; - const Scalar _tmp40 = _tmp23 * _tmp5; - const Scalar _tmp41 = _tmp15 * _tmp20; - const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41); - const Scalar _tmp43 = _tmp15 * _tmp19; - const Scalar _tmp44 = _tmp23 * _tmp9; - const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44); - const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21); - const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41); - const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44); - const Scalar _tmp49 = Scalar(1.0) / (math::max(epsilon, innov_var)); - - // Output terms (2) - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = _tmp37; - _h(1, 0) = _tmp38; - _h(2, 0) = _tmp39; - _h(3, 0) = _tmp42; - _h(4, 0) = _tmp45; - _h(5, 0) = _tmp46; - _h(21, 0) = _tmp47; - _h(22, 0) = _tmp48; - } - - if (K != nullptr) { - matrix::Matrix& _k = (*K); - - _k(0, 0) = - _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 + - P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46); - _k(1, 0) = - _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 + - P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46); - _k(2, 0) = - _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 + - P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46); - _k(3, 0) = - _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 + - P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46); - _k(4, 0) = - _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 + - P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46); - _k(5, 0) = - _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 + - P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46); - _k(6, 0) = - _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 + - P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46); - _k(7, 0) = - _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 + - P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46); - _k(8, 0) = - _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 + - P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46); - _k(9, 0) = - _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 + - P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46); - _k(10, 0) = - _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 + - P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46); - _k(11, 0) = - _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 + - P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46); - _k(12, 0) = - _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 + - P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46); - _k(13, 0) = - _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 + - P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46); - _k(14, 0) = - _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 + - P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46); - _k(15, 0) = - _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 + - P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46); - _k(16, 0) = - _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 + - P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46); - _k(17, 0) = - _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 + - P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46); - _k(18, 0) = - _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 + - P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46); - _k(19, 0) = - _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 + - P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46); - _k(20, 0) = - _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 + - P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46); - _k(21, 0) = - _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 + - P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46); - _k(22, 0) = - _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 + - P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46); - _k(23, 0) = - _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 + - P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym From 33841cf43896478b86766cb2b09526fe5c88e49c Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 7 Jan 2025 16:15:57 -0700 Subject: [PATCH 39/94] Update GPS submodule --- src/drivers/gps/devices | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index e048340d0f..bbdd5767a9 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit e048340d0f6a395a3189292c33d08174bb309143 +Subproject commit bbdd5767a961cc17bdcc577c79b1c708d2a7999a From f693fab7c832c4ee05ea7b620ad42a9d984404f6 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 16 Jan 2025 11:45:26 +0100 Subject: [PATCH 40/94] rc_update params: remove comment about default static mixing Was added back in 2014: 7441efde4745c0dddc08a36a0bbf83307f82948a --- src/modules/rc_update/params.c | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 7f8a21605c..8ada8e9561 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2025 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 @@ -1592,8 +1592,6 @@ PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0); /** * AUX1 Passthrough RC channel * - * Default function: Camera pitch - * * @min 0 * @max 18 * @group Radio Calibration @@ -1622,8 +1620,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /** * AUX2 Passthrough RC channel * - * Default function: Camera roll - * * @min 0 * @max 18 * @group Radio Calibration @@ -1652,8 +1648,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /** * AUX3 Passthrough RC channel * - * Default function: Camera azimuth / yaw - * * @min 0 * @max 18 * @group Radio Calibration From 1eb9434b8c39fef3691684df520686231dce3939 Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 17 Jan 2025 10:38:42 +0100 Subject: [PATCH 41/94] stream ATTITUDE_QUATERNION in low bandwidth mode --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1c63f8adee..6e81849600 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1744,6 +1744,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("CAMERA_TRIGGER", unlimited_rate); configure_stream_local("LOCAL_POSITION_NED", 30.0f); configure_stream_local("ATTITUDE", 20.0f); + configure_stream_local("ATTITUDE_QUATERNION", 20.0f); configure_stream_local("ALTITUDE", 10.0f); configure_stream_local("DISTANCE_SENSOR", 10.0f); configure_stream_local("MOUNT_ORIENTATION", 10.0f); From b1ca0495e280b91cb2e79efd9a3d5cd53431f43f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jan 2025 12:50:10 -0500 Subject: [PATCH 42/94] ekf2: yaw estimator additional validity checks --- src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp | 6 +++++- src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp | 4 ++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 889af97a08..6ce0cebc66 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const return false; } - return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); + const float yaw_var = _yawEstimator.getYawVar(); + + return (yaw_var > 0.f) + && (yaw_var < sq(_params.EKFGSF_yaw_err_max)) + && PX4_ISFINITE(yaw_var); } bool Ekf::isYawFailure() const diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 2b2293904c..89ce999146 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); } + + if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) { + reset(); + } } } From 29f981f14c32170de690b798819c55243aa9feea Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 20 Jan 2025 10:29:30 +0100 Subject: [PATCH 43/94] collision-prevention: extract rotation scaling function into new ObstacleMath library. New library created for static and/or repeated code across collision prevention and driver files. --- src/lib/collision_prevention/CMakeLists.txt | 7 ++- .../CollisionPrevention.cpp | 15 ++---- .../CollisionPrevention.hpp | 2 +- src/lib/collision_prevention/ObstacleMath.cpp | 54 +++++++++++++++++++ src/lib/collision_prevention/ObstacleMath.hpp | 47 ++++++++++++++++ 5 files changed, 111 insertions(+), 14 deletions(-) create mode 100644 src/lib/collision_prevention/ObstacleMath.cpp create mode 100644 src/lib/collision_prevention/ObstacleMath.hpp diff --git a/src/lib/collision_prevention/CMakeLists.txt b/src/lib/collision_prevention/CMakeLists.txt index 663c9e9fee..1b62a10f63 100644 --- a/src/lib/collision_prevention/CMakeLists.txt +++ b/src/lib/collision_prevention/CMakeLists.txt @@ -31,7 +31,12 @@ # ############################################################################ -px4_add_library(CollisionPrevention CollisionPrevention.cpp) +px4_add_library(CollisionPrevention + CollisionPrevention.cpp + ObstacleMath.cpp +) target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable +target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(CollisionPrevention PRIVATE mathlib) px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index b2c62d4906..3f1344ffdd 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 @@ -38,6 +38,7 @@ */ #include "CollisionPrevention.hpp" +#include "ObstacleMath.hpp" #include using namespace matrix; @@ -400,18 +401,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f))); - - const Vector3f forward_vector(1.0f, 0.0f, 0.0f); - - const Quatf q_sensor_rotation = vehicle_attitude * q_sensor; - - const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector); - - const float sensor_dist_scale = rotated_sensor_vector.xy().norm(); - if (distance_reading < distance_sensor.max_distance) { - distance_reading = distance_reading * sensor_dist_scale; + ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude); } uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 8fcc7c28f3..7d9d16cd7f 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-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 diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp new file mode 100644 index 0000000000..22d67c64ae --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "ObstacleMath.hpp" +#include + +using namespace matrix; + +namespace ObstacleMath +{ + +void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle) +{ + const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f))); + const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor; + const Vector3f forward(1.f, 0.f, 0.f); + const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward); + + float horizontal_projection_scale = sensor_direction_in_world.xy().norm(); + horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f); + distance *= horizontal_projection_scale; +} + +} // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp new file mode 100644 index 0000000000..3c19eb9110 --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + +namespace ObstacleMath +{ + +/** + * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane + * @param distance measurement which is scaled down + * @param yaw orientation of the measurement on the body horizontal plane + * @param q_world_vehicle vehicle attitude quaternion + */ +void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle); + +} // ObstacleMath From ab46502cbd2d210772ede0dc7953e7ba0780ec90 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 20 Jan 2025 10:30:22 +0100 Subject: [PATCH 44/94] obstacle-math: add unit tests for project_distance_on_horizontal_plane function. --- src/lib/collision_prevention/CMakeLists.txt | 1 + .../collision_prevention/ObstacleMathTest.cpp | 93 +++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 src/lib/collision_prevention/ObstacleMathTest.cpp diff --git a/src/lib/collision_prevention/CMakeLists.txt b/src/lib/collision_prevention/CMakeLists.txt index 1b62a10f63..ae42b65454 100644 --- a/src/lib/collision_prevention/CMakeLists.txt +++ b/src/lib/collision_prevention/CMakeLists.txt @@ -40,3 +40,4 @@ target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR target_link_libraries(CollisionPrevention PRIVATE mathlib) px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention) +px4_add_unit_gtest(SRC ObstacleMathTest.cpp LINKLIBS CollisionPrevention) diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp new file mode 100644 index 0000000000..e24b95134c --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 2025 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 "ObstacleMath.hpp" + +using namespace matrix; + +TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane) +{ + // standard vehicle orientation inputs + Quatf vehicle_pitch_up_45(Eulerf(0.0f, M_PI_4_F, 0.0f)); + Quatf vehicle_roll_right_45(Eulerf(M_PI_4_F, 0.0f, 0.0f)); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + float distance = 1.0f; + float sensor_orientation = 0; // radians (forward facing) + + // WHEN: we project the distance onto the horizontal plane + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45); + + // THEN: the distance should be scaled correctly + float expected_scale = sqrtf(2) / 2; + float expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + distance = 1.0f; + + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45); + + // THEN: the distance should be scaled correctly + expected_scale = 1.f; + expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + distance = 1.0f; + sensor_orientation = M_PI_2_F; // radians (right facing) + + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45); + + // THEN: the distance should be scaled correctly + expected_scale = sqrtf(2) / 2; + expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + + // GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation + distance = 1.0f; + + ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45); + + // THEN: the distance should be scaled correctly + expected_scale = 1.f; + expected_distance = 1.0f * expected_scale; + + EXPECT_NEAR(distance, expected_distance, 1e-5); + +} From 65a8cc0e0a8731111ad47968a15fda3567aba2cf Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 20 Jan 2025 10:33:30 +0100 Subject: [PATCH 45/94] sf45: scale measured distance with pitch and roll. Calls function from ObstacleMath library that accounts for the vehicle's attitude w.r.t the obstacle. Obstacles are assumed to be flat, vertical walls. --- .../lightware_sf45_serial/CMakeLists.txt | 1 + .../lightware_sf45_serial.cpp | 37 ++++++++++++------- .../lightware_sf45_serial.hpp | 6 ++- 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt index 561b7755c7..6d561f72db 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt +++ b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( DEPENDS drivers_rangefinder px4_work_queue + CollisionPrevention MODULE_CONFIG module.yaml ) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index a8ed20383d..a7a94c6791 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -33,19 +33,18 @@ #include "lightware_sf45_serial.hpp" -#include #include +#include +#include #include + #include #include - -#include +#include +#include using namespace time_literals; -/* Configuration Constants */ - - SF45LaserSerial::SF45LaserSerial(const char *port) : ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), @@ -136,8 +135,6 @@ int SF45LaserSerial::measure() int SF45LaserSerial::collect() { - float distance_m = -1.0f; - if (_sensor_state == STATE_UNINIT) { perf_begin(_sample_perf); @@ -196,8 +193,7 @@ int SF45LaserSerial::collect() sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM); if (_crc_valid) { - sf45_process_replies(&distance_m); - PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO")); + sf45_process_replies(); perf_end(_sample_perf); return PX4_OK; } @@ -592,7 +588,7 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8 } } -void SF45LaserSerial::sf45_process_replies(float *distance_m) +void SF45LaserSerial::sf45_process_replies() { switch (rx_field.msg_id) { case SF_DISTANCE_DATA_CM: { @@ -640,20 +636,34 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) } // Convert to meters for the debug message - *distance_m = raw_distance * SF45_SCALE_FACTOR; + float distance_m = raw_distance * SF45_SCALE_FACTOR; _current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist; uint8_t current_bin = sf45_convert_angle(scaled_yaw); if (current_bin != _previous_bin) { PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin, - (double)*distance_m); + (double)distance_m); + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = matrix::Quatf(vehicle_attitude.q); + } + } + + float current_bin_dist = static_cast(_current_bin_dist); + float scaled_yaw_rad = math::radians(static_cast(scaled_yaw)); + ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude); + _current_bin_dist = static_cast(current_bin_dist); if (_current_bin_dist > _obstacle_distance.max_distance) { _current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition } hrt_abstime now = hrt_absolute_time(); + _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); _publish_obstacle_msg(now); @@ -727,6 +737,7 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ } } } + uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { uint8_t mapped_sector = 0; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index c5cae5e049..bfe44f0da2 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -49,7 +49,9 @@ #include #include +#include #include +#include #include "sf45_commands.h" @@ -92,7 +94,7 @@ public: void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id); void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); - void sf45_process_replies(float *data); + void sf45_process_replies(); uint8_t sf45_convert_angle(const int16_t yaw); float sf45_wrap_360(float f); @@ -113,6 +115,7 @@ private: void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now); void _publish_obstacle_msg(hrt_abstime now); + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uint64_t _data_timestamps[BIN_COUNT]; @@ -141,6 +144,7 @@ private: int32_t _orient_cfg{0}; uint8_t _previous_bin{0}; uint16_t _current_bin_dist{UINT16_MAX}; + matrix::Quatf _vehicle_attitude{}; // end of SF45/B data members From b09340cc98e2e0345870c600eaecaee22569d4f5 Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Mon, 20 Jan 2025 13:30:51 +0100 Subject: [PATCH 46/94] Control Allocator: Add option for metric allocation (skip normalization) (#24199) * add: metric allocation * add: actual files * rft: moved metric allocation to pseudo-inverse via flag with public method * del: removed metric allocation test and added test in pseudo-inverse testing * rft: deleted extra newline at the end of pseudo inverse test file * feat: removed unnecessary log include --- .../ControlAllocationPseudoInverse.cpp | 16 +++++++++---- .../ControlAllocationPseudoInverse.hpp | 2 ++ .../ControlAllocationPseudoInverseTest.cpp | 24 +++++++++++++++++++ 3 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp index 5d86814d7e..0dc512b4ed 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp @@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix( update_normalization_scale); _mix_update_needed = true; _normalization_needs_update = update_normalization_scale; + + if (_metric_allocation && update_normalization_scale) { + // adding #include + PX4_WARN leads to failed linking on test + _normalization_needs_update = false; + } } void @@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse() if (_mix_update_needed) { matrix::geninv(_effectiveness, _mix); - if (_normalization_needs_update && !_had_actuator_failure) { - updateControlAllocationMatrixScale(); - _normalization_needs_update = false; + if (!_metric_allocation) { + if (_normalization_needs_update && !_had_actuator_failure) { + updateControlAllocationMatrixScale(); + _normalization_needs_update = false; + } + + normalizeControlAllocationMatrix(); } - normalizeControlAllocationMatrix(); _mix_update_needed = false; } } diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp index 27c367376b..d4527cc1f4 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp @@ -57,11 +57,13 @@ public: void setEffectivenessMatrix(const matrix::Matrix &effectiveness, const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, bool update_normalization_scale) override; + void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; } protected: matrix::Matrix _mix; bool _mix_update_needed{false}; + bool _metric_allocation{false}; /** * Recalculate pseudo inverse if required. diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp index 89faab8c92..0eafbc6c26 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp @@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase) EXPECT_EQ(actuator_sp, actuator_sp_expected); EXPECT_EQ(control_allocated, control_allocated_expected); } + +TEST(ControlAllocationMetricTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + matrix::Vector actuator_sp_expected; + + method.setMetricAllocation(true); + method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false); + method.setControlSetpoint(control_sp); + method.allocate(); + actuator_sp = method.getActuatorSetpoint(); + control_allocated_expected = method.getAllocatedControl(); + + EXPECT_EQ(actuator_sp, actuator_sp_expected); + EXPECT_EQ(control_allocated, control_allocated_expected); +} From 0d229055585a0a52e2818afceabfb1b09fddd56d Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Mon, 20 Jan 2025 15:27:37 +0100 Subject: [PATCH 47/94] VTOL params: fix param name --- src/modules/vtol_att_control/vtol_att_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 3bcb56e498..3b8114de3f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -354,7 +354,7 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); * Minimum pitch angle during hover. * * Any pitch setpoint below this value is translated to a forward force by the fixed-wing forward actuation if - * VT_FW_TRHUST_EN is set to 1. + * VT_FWD_TRHUST_EN is set. * * @unit deg * @min -10.0 From 4a5aa1e947bb4a00de5d1204976a9728c06da3b9 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Mon, 20 Jan 2025 15:50:21 +0100 Subject: [PATCH 48/94] Fix max-hagl restriction to position/altitude control (#23667) * fix max-hagl restriction to position/altitude control * max hagl vel restriction in ManAcc position mode * use interpolate func, change naming * simplyfied vertical vel limitation * move velocity-constraint adjustment to StickAccelXY --- msg/versioned/VehicleLocalPosition.msg | 10 +++--- src/drivers/ins/vectornav/VectorNav.cpp | 3 +- src/modules/ekf2/EKF/ekf.h | 5 +-- src/modules/ekf2/EKF/ekf_helper.cpp | 13 ++++--- src/modules/ekf2/EKF2.cpp | 10 ++++-- .../FlightTaskManualAcceleration.cpp | 21 +++++++++++ .../FlightTaskManualAcceleration.hpp | 5 +++ .../FlightTaskManualAltitude.cpp | 36 +++++++------------ .../FlightTaskManualAltitude.hpp | 1 + .../tasks/Utility/StickAccelerationXY.cpp | 14 +++++++- .../tasks/Utility/StickAccelerationXY.hpp | 6 ++-- .../BlockLocalPositionEstimator.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/navigator/mission_block.cpp | 3 +- .../simulator_mavlink/SimulatorMavlink.cpp | 3 +- 15 files changed, 90 insertions(+), 46 deletions(-) diff --git a/msg/versioned/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg index aec6311ab3..8c04b4153c 100644 --- a/msg/versioned/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -77,10 +77,12 @@ float32 evv # Standard deviation of vertical velocity error, (metres/sec) bool dead_reckoning # True if this position is estimated through dead-reckoning # estimator specified vehicle limits -float32 vxy_max # maximum horizontal speed - set to 0 when limiting not required (meters/sec) -float32 vz_max # maximum vertical speed - set to 0 when limiting not required (meters/sec) -float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters) -float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) +# set to INFINITY when limiting not required +float32 vxy_max # maximum horizontal speed (meters/sec) +float32 vz_max # maximum vertical speed (meters/sec) +float32 hagl_min # minimum height above ground level (meters) +float32 hagl_max_z # maximum height above ground level for z-control (meters) +float32 hagl_max_xy # maximum height above ground level for xy-control (meters) # TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position # TOPICS estimator_local_position diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 85dd4c1846..e4e9821d47 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -328,7 +328,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet) local_position.vxy_max = INFINITY; local_position.vz_max = INFINITY; local_position.hagl_min = INFINITY; - local_position.hagl_max = INFINITY; + local_position.hagl_max_z = INFINITY; + local_position.hagl_max_xy = INFINITY; local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8df41e6ba8..e4a5e62ef6 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -208,8 +208,9 @@ public: // vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. // vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. // hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. - // hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. - void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; + // hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed. + // hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed. + void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const; void resetGyroBias(); void resetGyroBiasCov(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 664613a2b6..0d8fac2fb5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -327,13 +327,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } -void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const +void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, + float *hagl_max_xy) const { // Do not require limiting by default *vxy_max = NAN; *vz_max = NAN; *hagl_min = NAN; - *hagl_max = NAN; + *hagl_max_z = NAN; + *hagl_max_xy = NAN; #if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits @@ -347,7 +349,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; - *hagl_max = rangefinder_hagl_max; + *hagl_max_z = rangefinder_hagl_max; } # if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -370,11 +372,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max); // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f); *vxy_max = flow_vxy_max; *hagl_min = flow_hagl_min; - *hagl_max = flow_hagl_max; + *hagl_max_xy = flow_hagl_max; } # endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c85527b912..4fcafacedd 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1628,7 +1628,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) || _ekf.control_status_flags().wind_dead_reckoning; // get control limit information - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy); // convert NaN to INFINITY if (!PX4_ISFINITE(lpos.vxy_max)) { @@ -1643,8 +1643,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.hagl_min = INFINITY; } - if (!PX4_ISFINITE(lpos.hagl_max)) { - lpos.hagl_max = INFINITY; + if (!PX4_ISFINITE(lpos.hagl_max_z)) { + lpos.hagl_max_z = INFINITY; + } + + if (!PX4_ISFINITE(lpos.hagl_max_xy)) { + lpos.hagl_max_xy = INFINITY; } // publish vehicle local position data diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index a1ea2b6b80..6b79aa1833 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -59,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se bool FlightTaskManualAcceleration::update() { + const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get(); + setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy); bool ret = FlightTaskManualAltitudeSmoothVel::update(); + float max_hagl_ratio = 0.0f; + + if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) { + max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy; + } + + // limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps + static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl + static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl + + if (max_hagl_ratio > factor_threshold) { + max_hagl_ratio = math::min(max_hagl_ratio, 1.f); + const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get()); + _stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel)); + + } else { + _stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max)); + } + _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 0ae3d35bb6..ac19500236 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -52,4 +52,9 @@ protected: StickAccelerationXY _stick_acceleration_xy{this}; WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_acc_hor + ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 280aea617f..7ea9289cb6 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() _min_distance_to_ground = -INFINITY; } - if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { - _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; - - } else { - _max_distance_to_ground = INFINITY; + if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) { + _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z; } } @@ -154,8 +151,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { // terrain following _terrainFollowing(apply_brake, stopped); - // respect maximum altitude - _respectMaxAltitude(); } else { // normal mode where height is dependent on local frame @@ -185,9 +180,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // user demands velocity change _position_setpoint(2) = NAN; // ensure that maximum altitude is respected - _respectMaxAltitude(); } } + + _respectMaxAltitude(); } void FlightTaskManualAltitude::_respectMinAltitude() @@ -229,29 +225,20 @@ void FlightTaskManualAltitude::_respectMaxAltitude() { if (PX4_ISFINITE(_dist_to_bottom)) { - // if there is a valid maximum distance to ground, linearly increase speed limit with distance - // below the maximum, preserving control loop vertical position error gain. - // TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller + float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom); + if (PX4_ISFINITE(_max_distance_to_ground)) { - _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), - -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); + _constraints.speed_up = math::constrain(vel_constrained, -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); } else { _constraints.speed_up = _param_mpc_z_vel_max_up.get(); } - // if distance to bottom exceeded maximum distance, slowly approach maximum distance - if (_dist_to_bottom > _max_distance_to_ground) { - // difference between current distance to ground and maximum distance to ground - const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground; - // set position setpoint to maximum distance to ground - _position_setpoint(2) = _position(2) + delta_distance_to_max; - // limit speed downwards to 0.7m/s - _constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f); - - } else { - _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + if (_dist_to_bottom > _max_distance_to_ground && !(_sticks.getThrottleZeroCenteredExpo() < FLT_EPSILON)) { + _velocity_setpoint(2) = math::constrain(-vel_constrained, 0.f, _param_mpc_z_vel_max_dn.get()); } + + _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); } } @@ -306,6 +293,7 @@ bool FlightTaskManualAltitude::update() _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); + _max_distance_to_ground = INFINITY; return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 600e81fe36..5370de28ff 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -53,6 +53,7 @@ public: bool activate(const trajectory_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; + void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; } protected: void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index f823f929e8..c36d024ead 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -73,8 +73,20 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) { + // gradually adjust velocity constraint because good tracking is required for the drag estimation + if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) { + if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) { + _current_velocity_constraint = _targeted_velocity_constraint; + + } else { + _current_velocity_constraint = math::constrain(_targeted_velocity_constraint, + _current_velocity_constraint - dt * _param_mpc_acc_hor.get(), + _current_velocity_constraint + dt * _param_mpc_acc_hor.get()); + } + } + // maximum commanded velocity can be constrained dynamically - const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint); + const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint); Vector2f velocity_scale(velocity_sc, velocity_sc); // maximum commanded acceleration is scaled down with velocity const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get()); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 9fff1da1c4..1b29580c4f 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -63,7 +63,8 @@ public: void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp); float getMaxAcceleration() { return _param_mpc_acc_hor.get(); }; float getMaxJerk() { return _param_mpc_jerk_max.get(); }; - void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; + void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); }; + float getVelocityConstraint() { return _current_velocity_constraint; }; private: CollisionPrevention _collision_prevention{this}; @@ -85,7 +86,8 @@ private: matrix::Vector2f _acceleration_setpoint; matrix::Vector2f _acceleration_setpoint_prev; - float _velocity_constraint{INFINITY}; + float _targeted_velocity_constraint{INFINITY}; + float _current_velocity_constraint{INFINITY}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_vel_manual, diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 8517ff03b0..f28bb78c08 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -643,7 +643,8 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vxy_max = INFINITY; _pub_lpos.get().vz_max = INFINITY; _pub_lpos.get().hagl_min = INFINITY; - _pub_lpos.get().hagl_max = INFINITY; + _pub_lpos.get().hagl_max_z = INFINITY; + _pub_lpos.get().hagl_max_xy = INFINITY; _pub_lpos.get().timestamp = hrt_absolute_time();; _pub_lpos.update(); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e698c85186..37328503b9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2738,7 +2738,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.vxy_max = INFINITY; hil_local_pos.vz_max = INFINITY; hil_local_pos.hagl_min = INFINITY; - hil_local_pos.hagl_max = INFINITY; + hil_local_pos.hagl_max_z = INFINITY; + hil_local_pos.hagl_max_xy = INFINITY; hil_local_pos.timestamp = hrt_absolute_time(); _local_pos_pub.publish(hil_local_pos); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index cbcefcd128..95912a22e3 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -1026,7 +1026,8 @@ void MissionBlock::updateMaxHaglFailsafe() const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; if (_navigator->get_global_position()->terrain_alt_valid - && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + && ((target_alt - _navigator->get_global_position()->terrain_alt) + > math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) { // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 58e6ece481..92c5fe73a5 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -607,7 +607,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message hil_lpos.vxy_max = std::numeric_limits::infinity(); hil_lpos.vz_max = std::numeric_limits::infinity(); hil_lpos.hagl_min = std::numeric_limits::infinity(); - hil_lpos.hagl_max = std::numeric_limits::infinity(); + hil_lpos.hagl_max_z = std::numeric_limits::infinity(); + hil_lpos.hagl_max_xy = std::numeric_limits::infinity(); // always publish ground truth attitude message _lpos_ground_truth_pub.publish(hil_lpos); From 7a9608e54b69568b530d237f3f6f3313cd404167 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Mon, 20 Jan 2025 20:43:30 +0100 Subject: [PATCH 49/94] increase EKF2_RNG_FOG for FW and VTOL --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 3 +++ src/modules/ekf2/params_range_finder.yaml | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 7b2b9b095d..db11cedbb3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -21,3 +21,6 @@ param set-default RTL_RETURN_ALT 30 param set-default RTL_DESCEND_ALT 10 param set-default GPS_UBX_DYNMODEL 6 + +# lower RNG_FOG since MC are expected to fly closer over obstacles +param set-default EKF2_RNG_FOG 1.0 diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 78c30bbcca..97f60bdff4 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -158,7 +158,7 @@ parameters: If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the measurement may gets rejected. 0 - disabled type: float - default: 1.0 + default: 3.0 min: 0.0 max: 20.0 unit: m From af062c85eb98070bb88044ac0531c803ae70bc55 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Jan 2025 10:33:24 +0100 Subject: [PATCH 50/94] Revert "FLightTaskAuto: limit nudging speed based on distance sensor" This reverts commit 97cb933cff6d837e59dd55123804cd78abdca22a. --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 834f79465f..8e19ffd5a3 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -282,12 +282,6 @@ void FlightTaskAuto::_prepareLandSetpoints() sticks_xy.setZero(); } - // If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed - if (PX4_ISFINITE(_dist_to_bottom)) { - // Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed - max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f)); - } - _stick_acceleration_xy.setVelocityConstraint(max_speed); _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); From 25e76883b75c6fba04869e4aa01f58364e831a13 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 9 Jan 2025 20:33:42 +0300 Subject: [PATCH 51/94] Integrator: use 32bit integer to store dt to avoid overflow Signed-off-by: RomanBapst --- src/modules/sensors/Integrator.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/Integrator.hpp b/src/modules/sensors/Integrator.hpp index b6c26f331e..7895bd0c5b 100644 --- a/src/modules/sensors/Integrator.hpp +++ b/src/modules/sensors/Integrator.hpp @@ -55,7 +55,7 @@ public: ~Integrator() = default; static constexpr float DT_MIN{1e-6f}; // 1 microsecond - static constexpr float DT_MAX{static_cast(UINT16_MAX) * 1e-6f}; + static constexpr float DT_MAX{static_cast(UINT32_MAX) * 1e-6f}; /** * Put an item into the integral. @@ -111,7 +111,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) { if (integral_ready()) { integral = _alpha; @@ -200,7 +200,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) { if (Integrator::reset(integral, integral_dt)) { // apply coning corrections From c7e494b8d92f10dea161fa63ac209d5f74b12e7b Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 9 Jan 2025 20:34:31 +0300 Subject: [PATCH 52/94] VehicleImu: use 32 bit integer for dt Signed-off-by: RomanBapst --- msg/VehicleImu.msg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/msg/VehicleImu.msg b/msg/VehicleImu.msg index a71bb7a01f..24912e06fb 100644 --- a/msg/VehicleImu.msg +++ b/msg/VehicleImu.msg @@ -9,8 +9,8 @@ uint32 gyro_device_id # Gyroscope unique device ID for the sensor that float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt) float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt) -uint16 delta_angle_dt # integration period in microseconds -uint16 delta_velocity_dt # integration period in microseconds +uint32 delta_angle_dt # integration period in microseconds +uint32 delta_velocity_dt # integration period in microseconds uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 From f36b45b2ff757329c483a22e97d2c4177a590795 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 13 Jan 2025 11:12:38 +0300 Subject: [PATCH 53/94] VehicleOpticalFlow: use 32bit integer for dt Signed-off-by: RomanBapst --- src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 334df661bb..f7d573d927 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -161,7 +161,7 @@ void VehicleOpticalFlow::Run() } Vector3f delta_angle{NAN, NAN, NAN}; - uint16_t delta_angle_dt; + uint32_t delta_angle_dt; if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) { _delta_angle += delta_angle; From 8d1bfb77c6780cae27a8744523b58659b6a6880d Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Tue, 21 Jan 2025 10:59:37 -0700 Subject: [PATCH 54/94] boards: ark fpv add gimbal module (#24229) --- boards/ark/fpv/default.px4board | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index ff3c09f754..095141f633 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -36,6 +36,7 @@ CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y From 918eca8de47c5ad435fd4e1de0390d7cc1ec5bf7 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 21 Jan 2025 12:14:38 -0900 Subject: [PATCH 55/94] gz: increase timeout for service request (#24164) * gz: increase timeout for service request * change error messages to warnings, specify retrying * fix typo --- src/modules/simulation/gz_bridge/GZBridge.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 2dc1ef13e4..e7b3ea484e 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -141,7 +141,7 @@ int GZBridge::init() // If Gazebo has not been called, wait 2 seconds and try again. else { - PX4_WARN("Service call timed out as Gazebo has not been detected."); + PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); system_usleep(2000000); } } @@ -159,7 +159,7 @@ int GZBridge::init() while (scene_created == false) { if (!callSceneInfoMsgService(scene_info_service)) { - PX4_WARN("Service call timed out as Gazebo has not been detected."); + PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); system_usleep(2000000); } else { @@ -919,7 +919,7 @@ bool GZBridge::callEntityFactoryService(const std::string &service, const gz::ms } } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } @@ -932,7 +932,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) gz::msgs::Empty req; gz::msgs::Scene rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!result) { PX4_ERR("Scene Info service call failed."); return false; @@ -942,7 +942,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) } } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } @@ -955,7 +955,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs:: gz::msgs::Boolean rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!rep.data() || !result) { PX4_ERR("String service call failed"); return false; @@ -964,7 +964,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs:: } else { - PX4_ERR("Service call timed out: %s", service.c_str()); + PX4_WARN("Service call timed out: %s", service.c_str()); return false; } @@ -977,7 +977,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V gz::msgs::Boolean rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!rep.data() || !result) { PX4_ERR("String service call failed"); return false; @@ -986,7 +986,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V } else { - PX4_ERR("Service call timed out: %s", service.c_str()); + PX4_WARN("Service call timed out: %s", service.c_str()); return false; } From a3215419d79e55c0acc5689270d675f7ba090499 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 21 Jan 2025 14:47:25 -0900 Subject: [PATCH 56/94] gz: remove model spawn offset (#24165) --- src/modules/simulation/gz_bridge/GZBridge.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index e7b3ea484e..23684e76c1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -95,12 +95,6 @@ int GZBridge::init() model_pose_v.push_back(0.0); } - // If model position z is less equal than 0, move above floor to prevent floor glitching - if (model_pose_v[2] <= 0.0) { - PX4_INFO("Model position z is less or equal 0.0, moving upwards"); - model_pose_v[2] = 0.5; - } - gz::msgs::Pose *p = req.mutable_pose(); gz::msgs::Vector3d *position = p->mutable_position(); position->set_x(model_pose_v[0]); From 1900d2c98f4a0d73184cd373f91cbc87540ecabc Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 21 Jan 2025 14:48:05 -0900 Subject: [PATCH 57/94] uavcan: fix hw_errors from mutex lock, hide ESC/Servo status if no function set (#23888) --- src/drivers/uavcan/uavcan_main.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 5d3ac60b62..ad39a943a7 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -1073,8 +1073,6 @@ void UavcanMixingInterfaceServo::Run() void UavcanNode::print_info() { - (void)pthread_mutex_lock(&_node_mutex); - // Memory status printf("Pool allocator status:\n"); printf("\tCapacity hard/soft: %" PRIu16 "/%" PRIu16 " blocks\n", @@ -1112,15 +1110,29 @@ UavcanNode::print_info() printf("\n"); #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) - printf("ESC outputs:\n"); - _mixing_interface_esc.mixingOutput().printStatus(); - printf("Servo outputs:\n"); - _mixing_interface_servo.mixingOutput().printStatus(); + // Print esc status if at least one channel is enabled + for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { + if (_mixing_interface_esc.mixingOutput().isFunctionSet(i)) { + printf("ESC outputs:\n"); + _mixing_interface_esc.mixingOutput().printStatus(); + printf("\n"); + break; + } + } + + // Print servo status if at least one channel is enabled + for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) { + if (_mixing_interface_servo.mixingOutput().isFunctionSet(i)) { + printf("Servo outputs:\n"); + _mixing_interface_servo.mixingOutput().printStatus(); + printf("\n"); + break; + } + } + #endif - printf("\n"); - // Sensor bridges for (const auto &br : _sensor_bridges) { printf("Sensor '%s':\n", br->get_name()); @@ -1140,8 +1152,6 @@ UavcanNode::print_info() perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - - (void)pthread_mutex_unlock(&_node_mutex); } void From caaae6ed5191e0dea9df562938815fff80350834 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 17 Jan 2025 10:27:31 +0100 Subject: [PATCH 58/94] ekf2: allow sideslip fusion to always start with airspeed fusion This allow sideslip fusion to start during VTOL front transition or even on a multirotor with a vertical stabilizer and an airspeed sensor for example. --- .../ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp | 2 +- src/modules/ekf2/EKF2.hpp | 9 +++------ src/modules/ekf2/params_sideslip.yaml | 4 ++-- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index b2948629e4..d529e5e994 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -50,7 +50,7 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) { _control_status.flags.fuse_beta = _params.beta_fusion_enabled - && _control_status.flags.fixed_wing + && (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd) && _control_status.flags.in_air && !_control_status.flags.fake_pos; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 0bec478e0d..deb08c5a60 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -573,12 +573,9 @@ private: #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - (ParamExtFloat) - _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) - - (ParamExtInt) - _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables + (ParamExtFloat) _param_ekf2_beta_gate, + (ParamExtFloat) _param_ekf2_beta_noise, + (ParamExtInt) _param_ekf2_fuse_beta, #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/params_sideslip.yaml b/src/modules/ekf2/params_sideslip.yaml index 49d5467fcc..0dcec672ad 100644 --- a/src/modules/ekf2/params_sideslip.yaml +++ b/src/modules/ekf2/params_sideslip.yaml @@ -6,8 +6,8 @@ parameters: description: short: Enable synthetic sideslip fusion long: 'For reliable wind estimation both sideslip and airspeed fusion (see - EKF2_ARSP_THR) should be enabled. Only applies to fixed-wing vehicles (or - VTOLs in fixed-wing mode). Note: side slip fusion is currently not supported + EKF2_ARSP_THR) should be enabled. Only applies to vehicles in fixed-wing mode + or with airspeed fusion active. Note: side slip fusion is currently not supported for tailsitters.' type: boolean default: 0 From ee150a15b41bc657b31ff70d59f711a522d433f3 Mon Sep 17 00:00:00 2001 From: PavloZMN Date: Wed, 22 Jan 2025 12:58:07 +0200 Subject: [PATCH 59/94] Optical Flow: add unit testes for only using downward distance sensor (#23266) * Test for Optical Flow checks correct camera position * Formatting fixed * Update src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt Co-authored-by: Mathieu Bresciani * Update src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp Co-authored-by: Mathieu Bresciani * Update src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp Co-authored-by: Silvan Fuhrer * For test GIVEN/WHEN/THEN added --------- Co-authored-by: Mathieu Bresciani Co-authored-by: Silvan Fuhrer --- .../vehicle_optical_flow/CMakeLists.txt | 4 + .../VehicleOpticalFlow.hpp | 6 +- .../vehicle_optical_flow/test/CMakeLists.txt | 35 ++++++ .../test/VehicleOpticalFlowTest.cpp | 118 ++++++++++++++++++ 4 files changed, 161 insertions(+), 2 deletions(-) create mode 100644 src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp diff --git a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt index ba8761aa00..bf543c0636 100644 --- a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt +++ b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt @@ -36,3 +36,7 @@ px4_add_library(vehicle_optical_flow VehicleOpticalFlow.hpp ) target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index 5a5ed1663b..dd3022c709 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -73,9 +73,12 @@ public: void PrintStatus(); +protected: + void UpdateDistanceSensor(); + int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations + private: void ClearAccumulatedData(); - void UpdateDistanceSensor(); void UpdateSensorGyro(); void Run() override; @@ -117,7 +120,6 @@ private: uint16_t _quality_sum{0}; uint8_t _accumulated_count{0}; - int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations hrt_abstime _last_range_sensor_update{0}; bool _delta_angle_available{false}; diff --git a/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt new file mode 100644 index 0000000000..e3d89c05a2 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_functional_gtest(SRC VehicleOpticalFlowTest.cpp LINKLIBS vehicle_optical_flow uORB sensor_calibration) diff --git a/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp b/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp new file mode 100644 index 0000000000..a97a6ba1ee --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test for VehicleOpticalFlow + */ + +#include +#include "../VehicleOpticalFlow.hpp" +#include +#include + + +distance_sensor_s createDistanceSensorMessage(uint16_t orientation) +{ + distance_sensor_s message; + message.timestamp = hrt_absolute_time(); + message.min_distance = 1.f; + message.max_distance = 100.f; + message.current_distance = 1.1f; + + message.variance = 0.1f; + message.signal_quality = 100; + message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + message.orientation = orientation; + message.h_fov = math::radians(50.f); + message.v_fov = math::radians(30.f); + return message; + +} + +class VehicleOpticalFlowTest : public ::testing::Test +{ +public: + + class VehicleOpticalFlowTestable : public sensors::VehicleOpticalFlow + { + public: + void UpdateDistanceSensorPublic() + { + VehicleOpticalFlow::UpdateDistanceSensor(); + } + bool IsDistanceSensorSelected() + { + return _distance_sensor_selected >= 0; + + } + }; + + void SetUp() override + { + uORB::Manager::initialize(); + + } + void TearDown() override + { + uORB::Manager::terminate(); + } +}; + + +TEST_F(VehicleOpticalFlowTest, CameraFacingDown) +{ + // GIVEN: message with sensor camera facing down + distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_DOWNWARD_FACING); + orb_advertise(ORB_ID(distance_sensor), &message); + + // WHEN: update distance sensor + VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable; + testable.UpdateDistanceSensorPublic(); + + // THEN: sensor selected + EXPECT_TRUE(testable.IsDistanceSensorSelected()); +} + +TEST_F(VehicleOpticalFlowTest, CameraFacingForward) +{ + // GIVEN: message with sensor camera facing forward + distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_FORWARD_FACING); + orb_advertise(ORB_ID(distance_sensor), &message); + + // WHEN: update distance sensor + VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable; + testable.UpdateDistanceSensorPublic(); + + // THEN: sensor is not selected + EXPECT_FALSE(testable.IsDistanceSensorSelected()); +} From 57fdda597be4e14e3f493f6b55ef550bbb9bdccd Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 21 Jan 2025 16:52:25 +0300 Subject: [PATCH 60/94] vtol_takeoff: store altitude on takeoff and don't use home position altitude as vehicle does not need to be close to home position Signed-off-by: RomanBapst --- src/modules/navigator/vtol_takeoff.cpp | 4 +++- src/modules/navigator/vtol_takeoff.h | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index f9a552ef58..503a0279f9 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -118,7 +118,7 @@ VtolTakeoff::on_active() _mission_item.time_inside = 1.f; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get(); + _mission_item.altitude = _takeoff_alt_msl + _param_loiter_alt.get(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.lat = _loiter_location(0); @@ -170,6 +170,8 @@ VtolTakeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, _transition_alt_amsl); + _takeoff_alt_msl = _navigator->get_global_position()->alt; + _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/vtol_takeoff.h b/src/modules/navigator/vtol_takeoff.h index a353d89ce8..3ba32ce7df 100644 --- a/src/modules/navigator/vtol_takeoff.h +++ b/src/modules/navigator/vtol_takeoff.h @@ -70,6 +70,7 @@ private: } _takeoff_state; float _transition_alt_amsl{0.f}; // absolute altitude at which vehicle will transition to forward flight + float _takeoff_alt_msl{0.f}; matrix::Vector2d _loiter_location; float _loiter_height{0}; From 045c8d9831c67ac284c5ed710da618e2bff6df69 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 22 Jan 2025 18:56:31 +0300 Subject: [PATCH 61/94] Mission feasibility checks: make adding new check less error prone (#24241) * make adding new feasibility checks less prone to errors Signed-off-by: RomanBapst * Update src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp Co-authored-by: Silvan Fuhrer --------- Signed-off-by: RomanBapst Co-authored-by: Silvan Fuhrer --- .../MissionFeasibility/FeasibilityChecker.cpp | 39 ++++++++----------- .../MissionFeasibility/FeasibilityChecker.hpp | 27 +++++++------ 2 files changed, 30 insertions(+), 36 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index dbaf173efa..c35d2c1bde 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,12 +46,7 @@ FeasibilityChecker::FeasibilityChecker() : void FeasibilityChecker::reset() { - _mission_validity_failed = false; - _takeoff_failed = false; - _land_pattern_validity_failed = false; - _distance_between_waypoints_failed = false; - _fixed_wing_land_approach_failed = false; - _takeoff_land_available_failed = false; + _checks_failed.value = 0; _found_item_with_position = false; _has_vtol_takeoff = false; @@ -155,11 +150,11 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int updateData(); } - if (!_mission_validity_failed) { - _mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); + if (!_checks_failed.flags.mission_validity_failed) { + _checks_failed.flags.mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); } - if (_mission_validity_failed) { + if (_checks_failed.flags.mission_validity_failed) { // if a mission item is not valid then abort the other checks return false; } @@ -177,7 +172,7 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int } if (current_index == total_count - 1) { - _takeoff_land_available_failed = !checkTakeoffLandAvailable(); + _checks_failed.flags.takeoff_land_available_failed = !checkTakeoffLandAvailable(); } _mission_item_previous = mission_item; @@ -188,39 +183,39 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) { - if (!_distance_between_waypoints_failed) { - _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); + if (!_checks_failed.flags.distance_between_waypoints_failed) { + _checks_failed.flags.distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); } if (!_first_waypoint_found) { checkHorizontalDistanceToFirstWaypoint(mission_item); } - if (!_takeoff_failed) { - _takeoff_failed = !checkTakeoff(mission_item); + if (!_checks_failed.flags.takeoff_failed) { + _checks_failed.flags.takeoff_failed = !checkTakeoff(mission_item); } - if (!_items_fit_to_vehicle_type_failed) { - _items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); + if (!_checks_failed.flags.items_fit_to_vehicle_type_failed) { + _checks_failed.flags.items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); } } void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index) { - if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + if (!_checks_failed.flags.land_pattern_validity_failed) { + _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } } void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index) { - if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + if (!_checks_failed.flags.land_pattern_validity_failed) { + _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } - if (!_fixed_wing_land_approach_failed) { - _fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); + if (!_checks_failed.flags.fixed_wing_land_approach_failed) { + _checks_failed.flags.fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); } } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index d0905d3635..1c002baf9a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -77,12 +77,7 @@ public: */ bool someCheckFailed() { - return _takeoff_failed || - _distance_between_waypoints_failed || - _land_pattern_validity_failed || - _fixed_wing_land_approach_failed || - _mission_validity_failed || - _takeoff_land_available_failed; + return _checks_failed.value != 0; } /** @@ -110,14 +105,18 @@ private: matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; - // internal flags to keep track of which checks failed - bool _mission_validity_failed{false}; - bool _takeoff_failed{false}; - bool _land_pattern_validity_failed{false}; - bool _distance_between_waypoints_failed{false}; - bool _fixed_wing_land_approach_failed{false}; - bool _takeoff_land_available_failed{false}; - bool _items_fit_to_vehicle_type_failed{false}; + union checks_failed_u { + struct { + bool mission_validity_failed : 1; + bool takeoff_failed : 1; + bool land_pattern_validity_failed : 1; + bool distance_between_waypoints_failed : 1; + bool fixed_wing_land_approach_failed : 1; + bool takeoff_land_available_failed : 1; + bool items_fit_to_vehicle_type_failed : 1; + } flags; + uint16_t value {0}; + } _checks_failed{}; // internal checkTakeoff related variables bool _found_item_with_position{false}; From e3b98e6ed23346a13005c69395c6fafb389e9487 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 22 Jan 2025 10:41:10 +1100 Subject: [PATCH 62/94] Tools: remove unused/wrong define and fix comment --- Tools/px_uploader.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 4aee08f388..34f3be05ca 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -189,7 +189,7 @@ class uploader: GET_CHIP = b'\x2c' # rev5+ , get chip version SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII - GET_VERSION = b'\x2f' # rev5+ , get chip description in ASCII + GET_VERSION = b'\x2f' # rev5+ , get bootloader version in ASCII CHIP_FULL_ERASE = b'\x40' # full erase of flash, rev6+ MAX_DES_LENGTH = 20 @@ -201,7 +201,6 @@ class uploader: INFO_BOARD_ID = b'\x02' # board type INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes - BL_VERSION = b'\x07' # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars) PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 READ_MULTI_MAX = 252 # protocol max is 255 From 1aa83d954bb8ccff2213cd3d69481b837b68076e Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 22 Jan 2025 11:30:49 +0000 Subject: [PATCH 63/94] update all px4board kconfig --- boards/ark/fpv/default.px4board | 2 +- boards/bitcraze/crazyflie/default.px4board | 2 +- boards/bitcraze/crazyflie21/default.px4board | 2 +- boards/diatone/mamba-f405-mk2/default.px4board | 2 +- boards/flywoo/gn-f405/default.px4board | 2 +- boards/holybro/kakutef7/default.px4board | 2 +- boards/micoair/h743-v2/default.px4board | 4 ++-- boards/modalai/fc-v1/default.px4board | 2 +- boards/omnibus/f4sd/default.px4board | 2 +- boards/px4/fmu-v2/default.px4board | 2 +- boards/raspberrypi/pico/default.px4board | 2 +- 11 files changed, 12 insertions(+), 12 deletions(-) diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index 095141f633..a7c34922cf 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -16,9 +16,9 @@ CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y CONFIG_DRIVERS_IMU_MURATA_SCH16T=y -CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y CONFIG_DRIVERS_UAVCAN=y diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 84139b6dec..35abd8e338 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -31,7 +31,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 13c44d61d9..0db161f13e 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 14e8219028..efb1faeb49 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -36,7 +36,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/flywoo/gn-f405/default.px4board b/boards/flywoo/gn-f405/default.px4board index bd7ea9015d..c1f2f643bc 100644 --- a/boards/flywoo/gn-f405/default.px4board +++ b/boards/flywoo/gn-f405/default.px4board @@ -27,7 +27,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 75e689ed42..82a3261e87 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -41,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index fed1a0756b..74f1e2de29 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -26,10 +26,10 @@ CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_TAP_ESC=y -CONFIG_DRIVERS_UAVCAN=y -CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index fbd4d3fdf0..b73fdd18f8 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -43,6 +43,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y @@ -104,4 +105,3 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y -# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 470b95db5d..5aeb871f81 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -31,7 +31,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 2e2350dd5a..c5255743df 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -41,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board index d481cf3478..27329c23bc 100644 --- a/boards/raspberrypi/pico/default.px4board +++ b/boards/raspberrypi/pico/default.px4board @@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y -CONFIG_NAVIGATOR_ADSB=n +# CONFIG_NAVIGATOR_ADSB is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y From 752b25235c4976b7f1087af9d7372f73fa4f1483 Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 22 Jan 2025 11:55:09 +0000 Subject: [PATCH 64/94] boards: update all NuttX defconfigs --- .../micoair/h743-v2/nuttx-config/bootloader/defconfig | 1 - boards/micoair/h743-v2/nuttx-config/nsh/defconfig | 11 ++--------- 2 files changed, 2 insertions(+), 10 deletions(-) diff --git a/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig b/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig index b2c2fc88ca..10f43ff17d 100644 --- a/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig +++ b/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig @@ -35,7 +35,6 @@ CONFIG_CDCACM_PRODUCTID=0x004b CONFIG_CDCACM_PRODUCTSTR="MicoAir743v2" CONFIG_CDCACM_RXBUFSIZE=6000 CONFIG_CDCACM_TXBUFSIZE=12000 -#TODO：ally for VENDOR ID in the future CONFIG_CDCACM_VENDORID=0x3162 CONFIG_CDCACM_VENDORSTR="MicoAir" CONFIG_DEBUG_FULLOPT=y diff --git a/boards/micoair/h743-v2/nuttx-config/nsh/defconfig b/boards/micoair/h743-v2/nuttx-config/nsh/defconfig index 5a1f555257..dab70e35fa 100644 --- a/boards/micoair/h743-v2/nuttx-config/nsh/defconfig +++ b/boards/micoair/h743-v2/nuttx-config/nsh/defconfig @@ -196,17 +196,17 @@ CONFIG_STM32H7_SDMMC1=y CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y CONFIG_STM32H7_SPI2=y -CONFIG_STM32H7_SPI3=y CONFIG_STM32H7_SPI2_DMA=y CONFIG_STM32H7_SPI2_DMA_BUFFER=2048 +CONFIG_STM32H7_SPI3=y CONFIG_STM32H7_SPI3_DMA=y CONFIG_STM32H7_SPI3_DMA_BUFFER=2048 CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM15=y CONFIG_STM32H7_TIM1=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y CONFIG_STM32H7_TIM8=y -CONFIG_STM32H7_TIM15=y CONFIG_STM32H7_UART4=y CONFIG_STM32H7_UART5=y CONFIG_STM32H7_UART7=y @@ -222,25 +222,18 @@ CONFIG_STM32H7_USART_SWAP=y CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=115200 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=800 -CONFIG_UART5_BAUD=115200 CONFIG_UART5_RXBUFSIZE=600 CONFIG_UART5_TXBUFSIZE=800 -CONFIG_UART7_BAUD=115200 CONFIG_UART7_RXBUFSIZE=600 CONFIG_UART7_TXBUFSIZE=800 -CONFIG_UART8_BAUD=115200 CONFIG_UART8_RXBUFSIZE=600 CONFIG_UART8_TXBUFSIZE=800 -CONFIG_USART1_BAUD=115200 CONFIG_USART1_RXBUFSIZE=1200 CONFIG_USART1_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=115200 CONFIG_USART2_RXBUFSIZE=600 CONFIG_USART2_TXBUFSIZE=800 -CONFIG_USART3_BAUD=115200 CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=800 CONFIG_USART6_BAUD=57600 From 6322ebc3db815b2e941b067b8bd1d3e409883928 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 22 Jan 2025 15:27:56 -0500 Subject: [PATCH 65/94] rcS: shift sensors + EKF startup earlier - we want the drivers, sensors hub, and estimator running as soon as possible to initialize and avoid commander false positives complaining about missing data --- ROMFS/px4fmu_common/init.d/rcS | 130 ++++++++++++++++++--------------- 1 file changed, 72 insertions(+), 58 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5cdd34e698..807a26ed03 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -302,6 +302,75 @@ else . $FCONFIG fi + + # + # Sensors System (start before Commander so Preflight checks are properly run). + # + if param greater SYS_HITL 0 + then + sensors start -h + + # disable GPS + param set GPS_1_CONFIG 0 + + # start the simulator in hardware if needed + if param compare SYS_HITL 2 + then + simulator_sih start + sensor_baro_sim start + sensor_mag_sim start + sensor_gps_sim start + sensor_agp_sim start + fi + + else + # + # board sensors: rc.sensors + # + set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors + if [ -f $BOARD_RC_SENSORS ] + then + echo "Board sensors: ${BOARD_RC_SENSORS}" + . $BOARD_RC_SENSORS + fi + unset BOARD_RC_SENSORS + + . ${R}etc/init.d/rc.sensors + + if param compare -s BAT1_SOURCE 2 + then + esc_battery start + fi + + if ! param compare BAT1_SOURCE 1 + then + battery_status start + fi + + sensors start + fi + + # + # state estimator selection + # + if param compare -s EKF2_EN 1 + then + ekf2 start & + fi + + if param compare -s LPE_EN 1 + then + local_position_estimator start + fi + + if param compare -s ATT_EN 1 + then + attitude_estimator_q start + fi + + # + # px4io + # if px4io supported then # Check if PX4IO present and update firmware if needed. @@ -369,79 +438,24 @@ else fi # - # Sensors System (start before Commander so Preflight checks are properly run). - # Commander needs to be this early for in-air-restarts. + # Commander # if param greater SYS_HITL 0 then + commander start -h + if ! pwm_out_sim start -m hil then tune_control play error fi - sensors start -h - commander start -h - # disable GPS - param set GPS_1_CONFIG 0 - - # start the simulator in hardware if needed - if param compare SYS_HITL 2 - then - simulator_sih start - sensor_baro_sim start - sensor_mag_sim start - sensor_gps_sim start - sensor_agp_sim start - fi - else - # - # board sensors: rc.sensors - # - set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors - if [ -f $BOARD_RC_SENSORS ] - then - echo "Board sensors: ${BOARD_RC_SENSORS}" - . $BOARD_RC_SENSORS - fi - unset BOARD_RC_SENSORS - - . ${R}etc/init.d/rc.sensors - - if param compare -s BAT1_SOURCE 2 - then - esc_battery start - fi - - if ! param compare BAT1_SOURCE 1 - then - battery_status start - fi - - sensors start commander start dshot start pwm_out start fi - # - # state estimator selection - if param compare -s EKF2_EN 1 - then - ekf2 start & - fi - - if param compare -s LPE_EN 1 - then - local_position_estimator start - fi - - if param compare -s ATT_EN 1 - then - attitude_estimator_q start - fi - # # Configure vehicle type specific parameters. # Note: rc.vehicle_setup is the entry point for all vehicle type specific setup. From 07e7c64e6035015dbad3559e715ceaeee1dac345 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Wed, 22 Jan 2025 13:32:26 -0700 Subject: [PATCH 66/94] drivers/power_monitor/ina238: retry if read fails * ina238: retry if read fails * ina238: increase retries and only publish not connected if register check fails * ina238: use I2C resets --- src/drivers/power_monitor/ina238/ina238.cpp | 29 +++++++++++++-------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 42f5e12858..b46b277a84 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -160,6 +160,8 @@ int INA238::Reset() int ret = PX4_ERROR; + _retries = 3; + if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) { return ret; } @@ -231,6 +233,16 @@ int INA238::collect() success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK); success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK); + if (success) { + _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); + _battery.updateCurrent(static_cast(current * _current_lsb)); + _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); + + _battery.setConnected(success); + + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); + } + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { // check configuration registers periodically or immediately following any failure if (RegisterCheck(_register_cfg[_checked_register])) { @@ -242,26 +254,21 @@ int INA238::collect() PX4_DEBUG("register check failed"); perf_count(_bad_register_perf); success = false; + + _battery.setConnected(success); + + _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); } } - if (!success) { - PX4_DEBUG("error reading from sensor"); - bus_voltage = current = 0; - } - - _battery.setConnected(success); - _battery.updateVoltage(static_cast(bus_voltage * INA238_VSCALE)); - _battery.updateCurrent(static_cast(current * _current_lsb)); - _battery.updateTemperature(static_cast(temperature * INA238_TSCALE)); - _battery.updateAndPublishBatteryStatus(hrt_absolute_time()); - perf_end(_sample_perf); if (success) { return PX4_OK; } else { + PX4_DEBUG("error reading from sensor"); + return PX4_ERROR; } } From d0042aa2750c6ec3f426a9bca16842efedf328b5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 23 Jan 2025 09:34:38 +0100 Subject: [PATCH 67/94] FW defaults: remove EKF2_GPS_CHECK custom FW setting We want to align the default over all vehicle types for this param. There are still some thresholds that are increased for FW. Signed-off-by: Silvan Fuhrer --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index db15c3d09d..e6271d8144 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -24,7 +24,6 @@ param set-default COM_DISARM_PRFLT -1 param set-default EKF2_ARSP_THR 8 param set-default EKF2_FUSE_BETA 1 -param set-default EKF2_GPS_CHECK 21 param set-default EKF2_MAG_ACCLIM 0 param set-default EKF2_REQ_EPH 10 param set-default EKF2_REQ_EPV 10 From 1aab194f9e5c218a10d93e30e5b298f618d95b47 Mon Sep 17 00:00:00 2001 From: cuav-liu1 Date: Thu, 23 Jan 2025 16:01:02 +0800 Subject: [PATCH 68/94] boards: fix 7-nano pwm voltage control pin not initialized --- boards/cuav/7-nano/src/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/cuav/7-nano/src/board_config.h b/boards/cuav/7-nano/src/board_config.h index 269a4890f3..a2c564ae89 100644 --- a/boards/cuav/7-nano/src/board_config.h +++ b/boards/cuav/7-nano/src/board_config.h @@ -352,6 +352,7 @@ GPIO_nSAFETY_SWITCH_LED_OUT, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_nARMED, \ + GPIO_PWM_LEVEL_CONTROL, \ } #define BOARD_ENABLE_CONSOLE_BUFFER From c3ba39f931d2cf483132ae5f60decfe87481d477 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Tue, 21 Jan 2025 23:00:37 -0700 Subject: [PATCH 69/94] dshot: remove dshot 1200 --- src/drivers/drv_dshot.h | 2 +- src/drivers/dshot/DShot.cpp | 5 +---- src/drivers/dshot/DShot.h | 2 -- src/drivers/pwm_out/module.yaml | 1 - 4 files changed, 2 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 9c36dfd52d..431a56787d 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -88,7 +88,7 @@ typedef enum { * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. * This allows some of the channels to remain configured * as GPIOs or as another function. Already used channels/timers will not be configured as DShot - * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 + * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600 * @return <0 on error, the initialized channels mask. */ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot); diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 940019d9bf..4bd9bdef19 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -125,9 +125,6 @@ void DShot::enable_dshot_outputs(const bool enabled) } else if (tim_config == -3) { dshot_frequency_request = DSHOT600; - } else if (tim_config == -2) { - dshot_frequency_request = DSHOT1200; - } else { _output_mask &= ~channels; // don't use for dshot } @@ -824,7 +821,7 @@ On startup, the module tries to occupy all available pins for DShot output. It skips all pins already in use (e.g. by a camera trigger module). It supports: -- DShot150, DShot300, DShot600, DShot1200 +- DShot150, DShot300, DShot600 - telemetry via separate UART and publishing as esc_status message - sending DShot commands via CLI diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index e60b33b862..4fc312c3e9 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -52,7 +52,6 @@ using namespace time_literals; static constexpr unsigned int DSHOT150 = 150000u; static constexpr unsigned int DSHOT300 = 300000u; static constexpr unsigned int DSHOT600 = 600000u; -static constexpr unsigned int DSHOT1200 = 1200000u; static constexpr int DSHOT_DISARM_VALUE = 0; static constexpr int DSHOT_MIN_THROTTLE = 1; @@ -107,7 +106,6 @@ private: DShot150 = 150, DShot300 = 300, DShot600 = 600, - DShot1200 = 1200, }; struct Command { diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index c63b4195d2..8c2049cceb 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -23,7 +23,6 @@ actuator_output: -5: DShot150 -4: DShot300 -3: DShot600 - -2: DShot1200 -1: OneShot 50: PWM 50 Hz 100: PWM 100 Hz From c76e74338b289ad9d8ef29ffe7cc229f8af79d0b Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 24 Jan 2025 14:52:30 +0100 Subject: [PATCH 70/94] ekf-replay: fix airspeed replay If available, the EKF uses airspeed_validated, not airspeed --- msg/Ekf2Timestamps.msg | 1 + src/modules/ekf2/EKF2.cpp | 4 ++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/replay/ReplayEkf2.cpp | 6 ++++++ src/modules/replay/ReplayEkf2.hpp | 1 + 5 files changed, 13 insertions(+) diff --git a/msg/Ekf2Timestamps.msg b/msg/Ekf2Timestamps.msg index ae3ac06766..2487cf0200 100644 --- a/msg/Ekf2Timestamps.msg +++ b/msg/Ekf2Timestamps.msg @@ -14,6 +14,7 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times # difference of +-3.2s to the sensor_combined topic. int16 airspeed_timestamp_rel +int16 airspeed_validated_timestamp_rel int16 distance_sensor_timestamp_rel int16 optical_flow_timestamp_rel int16 vehicle_air_data_timestamp_rel diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4fcafacedd..ae7aad0406 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -745,6 +745,7 @@ void EKF2::Run() ekf2_timestamps_s ekf2_timestamps { .timestamp = now, .airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .airspeed_validated_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, @@ -2080,6 +2081,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } _airspeed_validated_timestamp_last = airspeed_validated.timestamp; + + ekf2_timestamps.airspeed_validated_timestamp_rel = (int16_t)((int64_t)airspeed_validated.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); } } else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3e91b24619..4b1da7d3a1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -294,6 +294,7 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); + add_topic("airspeed_validated"); add_topic("vehicle_optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 89f4874ce5..e03072b59e 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,6 +39,7 @@ // for ekf2 replay #include +#include #include #include #include @@ -91,6 +92,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(airspeed)) { _airspeed_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(airspeed_validated)) { + _airspeed_validated_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; @@ -138,6 +142,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs }; handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); + handle_sensor_publication(ekf2_timestamps.airspeed_validated_timestamp_rel, _airspeed_validated_msg_id); handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); @@ -225,6 +230,7 @@ ReplayEkf2::onExitMainLoop() PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):"); print_sensor_statistics(_airspeed_msg_id, "airspeed"); + print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated"); print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 13cb3d8be5..519b4242d2 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -82,6 +82,7 @@ private: static constexpr uint16_t msg_id_invalid = 0xffff; uint16_t _airspeed_msg_id = msg_id_invalid; + uint16_t _airspeed_validated_msg_id = msg_id_invalid; uint16_t _distance_sensor_msg_id = msg_id_invalid; uint16_t _optical_flow_msg_id = msg_id_invalid; uint16_t _sensor_combined_msg_id = msg_id_invalid; From c1cab2d4e05db0792fbf2526008f421f3a42af7b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 23 Oct 2024 19:33:26 +0200 Subject: [PATCH 71/94] control_allocator: add unit for slew rate and reword description --- src/modules/control_allocator/module.yaml | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 013b954033..4f49ef8b86 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -71,15 +71,14 @@ parameters: description: short: Motor ${i} slew rate limit long: | - Minimum time allowed for the motor input signal to pass through - the full output range. A value x means that the motor signal - can only go from 0 to 1 in minimum x seconds (in case of - reversible motors, the range is -1 to 1). + Forces the motor output signal to take at least the configured time (in seconds) + to traverse its full rangr normally [0%, 100%] if reversible [-100%, 100%]. Zero means that slew rate limiting is disabled. type: float decimal: 2 increment: 0.01 + unit: s num_instances: *max_num_mc_motors min: 0 max: 10 @@ -90,14 +89,14 @@ parameters: description: short: Servo ${i} slew rate limit long: | - Minimum time allowed for the servo input signal to pass through - the full output range. A value x means that the servo signal - can only go from -1 to 1 in minimum x seconds. + Forces the servo output signal to take at least the configured time (in seconds) + to traverse its full range [-100%, 100%]. Zero means that slew rate limiting is disabled. type: float decimal: 2 increment: 0.05 + unit: s num_instances: *max_num_servos min: 0 max: 10 From 165f644580109ac4144a4167a0ec67614b5067cd Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 8 Nov 2024 10:00:27 +0100 Subject: [PATCH 72/94] control_allocator: fix typo and use [0,1] instead of [0%, 100%] in slew rate description Co-authored-by: Hamish Willee --- src/modules/control_allocator/module.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 4f49ef8b86..32f2862c9d 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -72,7 +72,7 @@ parameters: short: Motor ${i} slew rate limit long: | Forces the motor output signal to take at least the configured time (in seconds) - to traverse its full rangr normally [0%, 100%] if reversible [-100%, 100%]. + to traverse its full range (normally [0, 1], or if reversible [-1, 1]). Zero means that slew rate limiting is disabled. type: float From 5bca71791a27db30e89e8904c40cab51e11f8e8b Mon Sep 17 00:00:00 2001 From: Balduin Date: Mon, 27 Jan 2025 16:52:29 +0100 Subject: [PATCH 73/94] SIH: clean up control surface configuration (#24205) * fix sign error in appropriate place In PR https://github.com/PX4/PX4-Autopilot/pull/24175 I changed the control surface deflection signs in generate_fw_aerodynamics to make the 1103 airframe work correctly. However, this breaks the 1101 airframe, introducing sing errors there. So, here the change in generate_fw_aerodynamics is reverted to the state before PR #24175. Instead, the signs are set correctly by using the HIL_ACT_REV bitfield in the respective airframe config files. * match control surface parameters to SIH model --- .../airframes/10041_sihsim_airplane | 13 +++++++------ .../init.d/airframes/1101_rc_plane_sih.hil | 17 +++++++++-------- .../init.d/airframes/1103_standard_vtol_sih.hil | 11 +++++------ src/modules/simulation/simulator_sih/sih.cpp | 2 +- 4 files changed, 22 insertions(+), 21 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index 36a69b7714..c91e732a9e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -33,18 +33,19 @@ param set-default SIH_KDV 0.2 param set-default SIH_VEHICLE_TYPE 1 # sih as fixed wing param set-default RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched) - param set-default CA_AIRFRAME 1 - param set-default CA_ROTOR_COUNT 1 +param set-default CA_ROTOR0_PX 0.3 +# SIH for now hardcodes this configuration which we need to match in the airframe files. param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TRQ_R -0.5 -param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron param set-default CA_SV_CS1_TRQ_P 1 -param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS1_TYPE 3 # elevator param set-default CA_SV_CS2_TRQ_Y 1 -param set-default CA_SV_CS2_TYPE 4 +param set-default CA_SV_CS2_TYPE 4 # rudder + param set-default PWM_MAIN_FUNC1 201 param set-default PWM_MAIN_FUNC2 202 param set-default PWM_MAIN_FUNC3 203 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index 2bcfc14a2e..2b54650a07 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -17,15 +17,16 @@ param set UAVCAN_ENABLE 0 param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 param set-default CA_ROTOR0_PX 0.3 -param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TRQ_R -0.5 -param set-default CA_SV_CS0_TYPE 1 -param set-default CA_SV_CS1_TRQ_P 1 -param set-default CA_SV_CS1_TYPE 3 -param set-default CA_SV_CS2_TRQ_Y 1 -param set-default CA_SV_CS2_TYPE 4 -param set HIL_ACT_REV 1 +# SIH for now hardcodes this configuration which we need to match in the airframe files. +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 # elevator +param set-default CA_SV_CS2_TRQ_Y 1 +param set-default CA_SV_CS2_TYPE 4 # rudder + param set HIL_ACT_FUNC1 201 param set HIL_ACT_FUNC2 202 param set HIL_ACT_FUNC3 203 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil index c67cf2cc85..0309d3c0dc 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -41,20 +41,19 @@ param set-default CA_ROTOR3_PX -0.2 param set-default CA_ROTOR3_PY 0.2 param set-default CA_ROTOR3_KM -0.05 - param set-default CA_ROTOR4_PX -0.3 param set-default CA_ROTOR4_KM 0.05 param set-default CA_ROTOR4_AX 1 param set-default CA_ROTOR4_AZ 0 +# SIH for now hardcodes this configuration which we need to match in the airframe files. param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TRQ_R 0.5 -param set-default CA_SV_CS0_TYPE 2 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron param set-default CA_SV_CS1_TRQ_P 1 -param set-default CA_SV_CS1_TYPE 3 +param set-default CA_SV_CS1_TYPE 3 # elevator param set-default CA_SV_CS2_TRQ_Y 1 - -param set HIL_ACT_REV 32 +param set-default CA_SV_CS2_TYPE 4 # rudder param set-default FW_AIRSPD_MAX 12 param set-default FW_AIRSPD_MIN 7 diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index f90805dd66..07c24fe642 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -361,7 +361,7 @@ void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, _wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX); _wing_r.update_aero(v_B, _w_B, alt, -roll_cmd * FLAP_MAX); - _tailplane.update_aero(v_B, _w_B, alt, pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd); + _tailplane.update_aero(v_B, _w_B, alt, -pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd); _fin.update_aero(v_B, _w_B, alt, yaw_cmd * FLAP_MAX, _T_MAX * throttle_cmd); _fuselage.update_aero(v_B, _w_B, alt); From dc3f6a16088cd82bdd2051cfee6b8232417b5b68 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 27 Jan 2025 14:59:25 +0100 Subject: [PATCH 74/94] fmu-v6xrt: Fix runtime error on new GCC New GCC versions inline builtin function like memcpy. On the fmu-v6xrt we can't call the functions inside imxrt_ocram_initialize because the ram function still needs to be initialized. This commit add a compile hint to not use builtins inside the imxrt_ocram_initialize function --- boards/px4/fmu-v6xrt/src/CMakeLists.txt | 5 + .../fmu-v6xrt/src/imxrt_ocram_initialize.c | 118 ++++++++++++++++++ boards/px4/fmu-v6xrt/src/init.c | 63 ---------- 3 files changed, 123 insertions(+), 63 deletions(-) create mode 100644 boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c diff --git a/boards/px4/fmu-v6xrt/src/CMakeLists.txt b/boards/px4/fmu-v6xrt/src/CMakeLists.txt index e34b9a134c..1ab333085b 100644 --- a/boards/px4/fmu-v6xrt/src/CMakeLists.txt +++ b/boards/px4/fmu-v6xrt/src/CMakeLists.txt @@ -75,9 +75,14 @@ else() imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c imxrt_clockconfig.c + imxrt_ocram_initialize.c ${SRCS} ) + # Force compiler not to use builtin functions (like memcpy) + # to optimize for loops in init.c (imxrt_ocram_initialize) + set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin) + target_link_libraries(drivers_board PRIVATE arch_board_hw_info diff --git a/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c b/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c new file mode 100644 index 0000000000..5a063bd46a --- /dev/null +++ b/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019, 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. + * + ****************************************************************************/ + +/** + * @file imxrt_ocram_initialize.c + * + * PX4 fmu-v6xrt RAM startup early startup code. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "arm_internal.h" +#include "imxrt_iomuxc.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +__BEGIN_DECLS +extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ +extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ +extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ +extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ +extern uint64_t _edtcm; /* Copy destination end address in DTCM */ +__END_DECLS + +/**************************************************************************** + * Name: imxrt_ocram_initialize + * + * Description: + * Called off reset vector to reconfigure the flexRAM + * and finish the FLASH to RAM Copy. + * CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin + * + ****************************************************************************/ + +__EXPORT void imxrt_ocram_initialize(void) +{ + uint32_t regval; + register uint64_t *src; + register uint64_t *dest; + + /* Reallocate + * Final Configuration is + * No DTCM + * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) + * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) + * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) + * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) + * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) + * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) + * 256k System OCRAM M4 (2020:0000-2023:ffff) + */ + + putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17); + putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18); + regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); + putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16); + + /* Copy any necessary code sections from FLASH to ITCM. The process is the + * same as the code copying from FLASH to RAM above. */ + for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; + dest < (uint64_t *)&_eitcmfuncs;) { + *dest++ = *src++; + } + + /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be + * certain that there are no issues with the state of global variables. + */ + + for (dest = &_sdtcm; dest < &_edtcm;) { + *dest++ = 0; + } + +#if defined(CONFIG_BOOT_RUNFROMISRAM) + const uint32_t *src; + uint32_t *dest; + + for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size), + dest = (uint32_t *)(g_boot_data.start + g_boot_data.size); + dest < (uint32_t *) &_etext;) { + *dest++ = *src++; + } + +#endif +} diff --git a/boards/px4/fmu-v6xrt/src/init.c b/boards/px4/fmu-v6xrt/src/init.c index 4b2edbc658..57ac95ce20 100644 --- a/boards/px4/fmu-v6xrt/src/init.c +++ b/boards/px4/fmu-v6xrt/src/init.c @@ -109,11 +109,6 @@ extern void led_off(int led); extern uint32_t _srodata; /* Start of .rodata */ extern uint32_t _erodata; /* End of .rodata */ -extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ -extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ -extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ -extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ -extern uint64_t _edtcm; /* Copy destination end address in DTCM */ __END_DECLS /************************************************************************************ @@ -225,65 +220,7 @@ void imxrt_flash_setup_prefetch_partition(void) ARM_ISB(); ARM_DMB(); } -/**************************************************************************** - * Name: imxrt_ocram_initialize - * - * Description: - * Called off reset vector to reconfigure the flexRAM - * and finish the FLASH to RAM Copy. - * - ****************************************************************************/ -__EXPORT void imxrt_ocram_initialize(void) -{ - uint32_t regval; - register uint64_t *src; - register uint64_t *dest; - - /* Reallocate - * Final Configuration is - * No DTCM - * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) - * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) - * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) - * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) - * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) - * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) - * 256k System OCRAM M4 (2020:0000-2023:ffff) - */ - - putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17); - putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18); - regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); - putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16); - - /* Copy any necessary code sections from FLASH to ITCM. The process is the - * same as the code copying from FLASH to RAM above. */ - for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; - dest < (uint64_t *)&_eitcmfuncs;) { - *dest++ = *src++; - } - - /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be - * certain that there are no issues with the state of global variables. - */ - - for (dest = &_sdtcm; dest < &_edtcm;) { - *dest++ = 0; - } - -#if defined(CONFIG_BOOT_RUNFROMISRAM) - const uint32_t *src; - uint32_t *dest; - - for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size), - dest = (uint32_t *)(g_boot_data.start + g_boot_data.size); - dest < (uint32_t *) &_etext;) { - *dest++ = *src++; - } - -#endif -} /**************************************************************************** * Name: imxrt_boardinitialize From 92b1f51623c478d0b903e8cad7a5732ac4db2ae3 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 27 Jan 2025 15:03:35 +0100 Subject: [PATCH 75/94] tropic: Fix runtime error on new GCC --- .../nxp/tropic-community/src/CMakeLists.txt | 4 + .../src/imxrt_ocram_initialize.c | 102 ++++++++++++++++++ boards/nxp/tropic-community/src/init.c | 49 --------- 3 files changed, 106 insertions(+), 49 deletions(-) create mode 100644 boards/nxp/tropic-community/src/imxrt_ocram_initialize.c diff --git a/boards/nxp/tropic-community/src/CMakeLists.txt b/boards/nxp/tropic-community/src/CMakeLists.txt index 91d89b5d30..6c37994652 100644 --- a/boards/nxp/tropic-community/src/CMakeLists.txt +++ b/boards/nxp/tropic-community/src/CMakeLists.txt @@ -42,8 +42,12 @@ px4_add_library(drivers_board imxrt_flexspi_nor_boot.c imxrt_flexspi_nor_flash.c imxrt_flexspi_storage.c + imxrt_ocram_initialize.c ) +# Force compiler not to use builtin functions (like memcpy) +# to optimize for loops in init.c (imxrt_ocram_initialize) +set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin) target_link_libraries(drivers_board PRIVATE diff --git a/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c b/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c new file mode 100644 index 0000000000..67c1a4ca36 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2018-2019, 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. + * + ****************************************************************************/ + +/** + * @file imxrt_ocram_initialize.c + * + * PX4 fmu-v6xrt RAM startup early startup code. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "arm_internal.h" +#include "imxrt_iomuxc.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +__BEGIN_DECLS +extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ +extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ +extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ +extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ +extern uint64_t _edtcm; /* Copy destination end address in DTCM */ +__END_DECLS + +/**************************************************************************** + * Name: imxrt_ocram_initialize + * + * Description: + * Called off reset vector to reconfigure the flexRAM + * and finish the FLASH to RAM Copy. + * CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin + * + ****************************************************************************/ + +__EXPORT void imxrt_ocram_initialize(void) +{ + uint32_t regval; + register uint64_t *src; + register uint64_t *dest; + + /* FlexRAM Configuration + * F = 64K ITCM + * A = 64K DTCM + * 5 = 64K OCRAM + * So 0xFFFFFFAA is + * 384K FlexRAM ITCM + * 128K FlexRAM DTCM + * */ + + putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17); + regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); + putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16); + + /* Copy any necessary code sections from FLASH to ITCM. The process is the + * same as the code copying from FLASH to RAM above. */ + for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; + dest < (uint64_t *)&_eitcmfuncs;) { + *dest++ = *src++; + } + + /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be + * certain that there are no issues with the state of global variables. + */ + + for (dest = &_sdtcm; dest < &_edtcm;) { + *dest++ = 0; + } +} diff --git a/boards/nxp/tropic-community/src/init.c b/boards/nxp/tropic-community/src/init.c index 244d6c5fcd..8e5db86b37 100644 --- a/boards/nxp/tropic-community/src/init.c +++ b/boards/nxp/tropic-community/src/init.c @@ -104,11 +104,6 @@ extern void led_off(int led); extern uint32_t _srodata; /* Start of .rodata */ extern uint32_t _erodata; /* End of .rodata */ -extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ -extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ -extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ -extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ -extern uint64_t _edtcm; /* Copy destination end address in DTCM */ __END_DECLS /************************************************************************************ @@ -192,50 +187,6 @@ void imxrt_flash_setup_prefetch_partition(void) ARM_DMB(); } -/**************************************************************************** - * Name: imxrt_ocram_initialize - * - * Description: - * Called off reset vector to reconfigure the flexRAM - * and finish the FLASH to RAM Copy. - * - ****************************************************************************/ - -__EXPORT void imxrt_ocram_initialize(void) -{ - uint32_t regval; - register uint64_t *src; - register uint64_t *dest; - - /* FlexRAM Configuration - * F = 64K ITCM - * A = 64K DTCM - * 5 = 64K OCRAM - * So 0xFFFFFFAA is - * 384K FlexRAM ITCM - * 128K FlexRAM DTCM - * */ - - putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17); - regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); - putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16); - - /* Copy any necessary code sections from FLASH to ITCM. The process is the - * same as the code copying from FLASH to RAM above. */ - for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; - dest < (uint64_t *)&_eitcmfuncs;) { - *dest++ = *src++; - } - - /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be - * certain that there are no issues with the state of global variables. - */ - - for (dest = &_sdtcm; dest < &_edtcm;) { - *dest++ = 0; - } -} - /**************************************************************************** * Name: imxrt_boardinitialize * From 4df65c133ee36ffdfe22261bc618036a8644c43a Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Tue, 28 Jan 2025 10:37:16 +0100 Subject: [PATCH 76/94] add cs_baro_fault to switch to fallback baro if available (#24260) --- msg/EstimatorStatusFlags.msg | 1 + .../EKF/aid_sources/barometer/baro_height_control.cpp | 5 +++-- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/ekf.h | 1 - src/modules/ekf2/EKF2.cpp | 1 + src/modules/sensors/vehicle_air_data/VehicleAirData.cpp | 8 ++++++++ src/modules/sensors/vehicle_air_data/VehicleAirData.hpp | 3 +++ 7 files changed, 17 insertions(+), 3 deletions(-) diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 705a5f9a71..0e5525d545 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -47,6 +47,7 @@ bool cs_rng_terrain # 39 - true if we are fusing range finder data f bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused bool cs_constant_pos # 42 - true if the vehicle is at a constant position +bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 119c6fa1b3..2d20c64c81 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -65,6 +65,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if ((_baro_counter == 0) || baro_sample.reset) { _baro_lpf.reset(measurement); _baro_counter = 1; + _control_status.flags.baro_fault = false; } else { _baro_lpf.update(measurement); @@ -113,7 +114,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) const bool continuing_conditions_passing = (_params.baro_ctrl == 1) && measurement_valid && (_baro_counter > _obs_buffer_length) - && !_baro_hgt_faulty; + && !_control_status.flags.baro_fault; const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); @@ -148,7 +149,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { // Some other height source is still working - _baro_hgt_faulty = true; + _control_status.flags.baro_fault = true; } } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 85d075e0f5..95efc971bc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -620,6 +620,7 @@ uint64_t mag_heading_consistent : uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position + uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index e4a5e62ef6..47320e8c12 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -602,7 +602,6 @@ private: HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; - bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index ae7aad0406..9e1f8f5378 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1933,6 +1933,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; + status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 041da27c09..24996c84da 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -141,6 +141,9 @@ void VehicleAirData::Run() AirTemperatureUpdate(); + estimator_status_flags_s estimator_status_flags; + const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); + bool updated[MAX_SENSOR_COUNT] {}; for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { @@ -196,6 +199,11 @@ void VehicleAirData::Run() ParametersUpdate(true); } + if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index + && estimator_status_flags.cs_baro_fault) { + _priority[uorb_index] = 1; // 1 is min priority while still being enabled + } + // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index fd28de34f8..ce7a8a117c 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -54,6 +54,7 @@ #include #include #include +#include using namespace time_literals; @@ -89,6 +90,8 @@ private: uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_baro), 0}, {this, ORB_ID(sensor_baro), 1}, From 58d3e1ea8ecfed4a3ca0e527d859d0c3174e2e2b Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 28 Jan 2025 11:22:51 +0100 Subject: [PATCH 77/94] test: in VTOL integration test use VTOL_LAND (#24261) Signed-off-by: Silvan Fuhrer --- test/mavsdk_tests/vtol_mission_straight_south.plan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/mavsdk_tests/vtol_mission_straight_south.plan b/test/mavsdk_tests/vtol_mission_straight_south.plan index fa6d37472b..1507af13f5 100644 --- a/test/mavsdk_tests/vtol_mission_straight_south.plan +++ b/test/mavsdk_tests/vtol_mission_straight_south.plan @@ -41,7 +41,7 @@ "AltitudeMode": 1, "MISSION_ITEM_ID": "2", "autoContinue": true, - "command": 21, + "command": 85, "doJumpId": 2, "frame": 3, "params": [ From a0a2bdaea51b351cba9899095e74c6c4a1b42360 Mon Sep 17 00:00:00 2001 From: Bertug Dilman <92028947+bdilman@users.noreply.github.com> Date: Tue, 28 Jan 2025 14:33:15 +0100 Subject: [PATCH 78/94] commander: COM_MODE_ARM_CHK parameter to allow mode registration while armed (#24249) --- ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator | 3 +++ .../HealthAndArmingChecks/checks/externalChecks.hpp | 6 +++++- src/modules/commander/ModeManagement.cpp | 9 +++------ src/modules/commander/commander_params.c | 11 +++++++++++ 4 files changed, 22 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 90e86aa688..fd0954744e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -4,6 +4,9 @@ # Simulator IMU data provided at 250 Hz param set-default IMU_INTEG_RATE 250 +# For simulation, allow registering modes while armed for developer convenience +param set-default COM_MODE_ARM_CHK 1 + if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then echo "INFO [init] SIH simulator" diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index 7129e46203..bbba418000 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -38,6 +38,7 @@ #include #include #include +#include static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) health_component_t::avoidance, "enum definition missmatch"); @@ -66,7 +67,7 @@ public: void update(); bool isUnresponsive(int registration_id); - + bool allowUpdateWhileArmed() const { return _param_com_mode_arm_chk.get(); } private: static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms; static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; @@ -109,4 +110,7 @@ private: uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)}; uORB::Publication _arming_check_request_pub{ORB_ID(arming_check_request)}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_mode_arm_chk + ); }; diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 14ba7ba28f..4430a0f563 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -370,11 +370,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa _failsafe_action_active = failsafe_action_active; _external_checks.update(); - bool allow_update_while_armed = false; -#if defined(CONFIG_ARCH_BOARD_PX4_SITL) - // For simulation, allow registering modes while armed for developer convenience - allow_update_while_armed = true; -#endif + bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed(); if (armed && !allow_update_while_armed) { // Reject registration requests @@ -408,7 +404,8 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa } } - // As we're disarmed we can use the user intended mode, as no failsafe will be active + // As we're disarmed we can use the user intended mode, as no failsafe will be active. + // Note that this might not be true if COM_MODE_ARM_CHK is set checkNewRegistrations(update_request); checkUnregistrations(user_intended_nav_state, update_request); } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 48b0cce522..37dcbdc9a9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1018,3 +1018,14 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); * @increment 1 */ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3); + +/** + * Allow external mode registration while armed. + * + * By default disabled for safety reasons + * + * @group Commander + * @boolean + * + */ +PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0); From 41c4933e109b199d73acfeaac1231e5c21759cdf Mon Sep 17 00:00:00 2001 From: Balduin Date: Mon, 13 Jan 2025 15:41:47 +0100 Subject: [PATCH 79/94] add standard vtol airframe --- .../airframes/10043_sihsim_standard_vtol | 116 ++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + .../simulation/simulator_sih/CMakeLists.txt | 1 + src/modules/simulation/simulator_sih/sih.hpp | 5 + 4 files changed, 123 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol new file mode 100644 index 0000000000..6bfda5edf6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol @@ -0,0 +1,116 @@ +#!/bin/sh +# +# @name SIH Standard VTOL +# +# @type Simulation +# @class VTOL +# +# @output Motor1 MC motor front right +# @output Motor2 MC motor back left +# @output Motor3 MC motor front left +# @output Motor4 MC motor back right +# @output Motor5 Forward thrust motor +# @output Servo1 Aileron +# @output Servo2 Elevator +# @output Servo3 Rudder +# +# @board px4_fmu-v2 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +# SIH in SITL changes. could we source these from a common file? {{{ +# inspired by: diff ROMFS/px4fmu_common/init.d/airframes/4001_quad_x ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx + +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 + +param set-default EKF2_GPS_DELAY 0 + +# battery check disabled below already +# HIL_ACT_FUNC* set below -- do we need PWM_MAIN_FUNC* as well? +# SIH_VEHICLE_TYPE set below too. + +# }}} + +param set-default VT_B_TRANS_DUR 5 +param set-default VT_ELEV_MC_LOCK 0 +param set-default VT_TYPE 2 +param set-default MPC_MAN_Y_MAX 60 +param set-default MC_PITCH_P 5 + +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR0_PX 0.2 +param set-default CA_ROTOR0_PY 0.2 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_PX -0.2 +param set-default CA_ROTOR1_PY -0.2 +param set-default CA_ROTOR2_PX 0.2 +param set-default CA_ROTOR2_PY -0.2 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.2 +param set-default CA_ROTOR3_PY 0.2 +param set-default CA_ROTOR3_KM -0.05 + + +param set-default CA_ROTOR4_PX -0.3 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 1 +param set-default CA_ROTOR4_AZ 0 + +# SIH for now hardcodes this configuration which we need to match in the airframe files. +param set-default CA_SV_CS_COUNT 3 +param set-default CA_SV_CS0_TRQ_R 1 +param set-default CA_SV_CS0_TYPE 15 # single channel aileron +param set-default CA_SV_CS1_TRQ_P 1 +param set-default CA_SV_CS1_TYPE 3 # elevator +param set-default CA_SV_CS2_TRQ_Y 1 +param set-default CA_SV_CS2_TYPE 4 # rudder + +param set-default FW_AIRSPD_MAX 12 +param set-default FW_AIRSPD_MIN 7 +param set-default FW_AIRSPD_TRIM 10 + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 201 +param set-default PWM_MAIN_FUNC6 202 +param set-default PWM_MAIN_FUNC7 203 +param set-default PWM_MAIN_FUNC8 105 + +param set-default MAV_TYPE 22 + +# set SYS_HITL to 2 to start the SIH and avoid sensors startup +# param set-default SYS_HITL 2 + +# disable some checks to allow to fly: +# - without real battery +param set-default CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set-default CBRK_IO_SAFETY 22027 + +param set-default SENS_DPRES_OFF 0.001 + +param set SIH_T_MAX 2.0 +param set SIH_Q_MAX 0.0165 +param set SIH_MASS 0.2 +# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg +param set SIH_IXX 0.00354 +param set SIH_IYY 0.000625 +param set SIH_IZZ 0.00300 +param set SIH_IXZ 0 +param set SIH_KDV 0.2 +param set SIH_L_ROLL 0.145 + +# sih as standard vtol +param set SIH_VEHICLE_TYPE 3 + +param set-default VT_ARSP_TRANS 6 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index da741d2a64..51d9eab72e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -109,6 +109,7 @@ px4_add_romfs_files( 10040_sihsim_quadx 10041_sihsim_airplane 10042_sihsim_xvert + 10043_sihsim_standard_vtol 17001_flightgear_tf-g1 17002_flightgear_tf-g2 diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index b8d3bf1232..7ae8483c51 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -53,6 +53,7 @@ if(PX4_PLATFORM MATCHES "posix") airplane quadx xvert + standard_vtol ) # find corresponding airframes diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 490e1dd823..51f6e17dae 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -220,7 +220,12 @@ private: float _u[NUM_ACTUATORS_MAX] {}; // thruster signals + // MC = Multicopter + // FW = Fixed Wing + // TS = Tailsitter VTOL + // SVTOL = Standard VTOL enum class VehicleType {MC, FW, TS, SVTOL}; + VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing From 7e4760587153535f21a403de855d9e158c0764d3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 15:05:42 +0100 Subject: [PATCH 80/94] batteryCheck: separate event messages for low, critical and emergency battery states --- .../checks/batteryCheck.cpp | 67 +++++++++++++++---- 1 file changed, 55 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index d453c724d5..85a19cf550 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -206,19 +206,62 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold ? events::Log::Critical : events::Log::Warning; - /* EVENT - * @description - * The battery state of charge of the worst battery is below the threshold. - * - * - * This check can be configured via BAT_LOW_THR, BAT_CRIT_THR, BAT_EMERGEN_THR and COM_ARM_BAT_MIN parameters. - * - */ - reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level, - "Low battery"); - if (reporter.mavlink_log_pub()) { - mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); + switch (reporter.failsafeFlags().battery_warning) { + default: + case battery_status_s::BATTERY_WARNING_LOW: + /* EVENT + * @description + * The lowest battery state of charge is below the low threshold. + * + * + * Can be configured with BAT_LOW_THR. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), + log_level, "Low battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery\t"); + } + + break; + + case battery_status_s::BATTERY_WARNING_CRITICAL: + /* EVENT + * @description + * The lowest battery state of charge is below the critical threshold. + * + * + * Can be configured with BAT_CRIT_THR and from when to disalow arming with COM_ARM_BAT_MIN. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_critical"), + log_level, "Critical battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Critical battery\t"); + } + + break; + + case battery_status_s::BATTERY_WARNING_EMERGENCY: + /* EVENT + * @description + * The lowest battery state of charge is below the emergency threshold. + * + * + * Can be configured with BAT_EMERGEN_THR. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_emergency"), + log_level, "Emergency battery level"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Emergency battery level\t"); + } + + break; } } From 2f2e56c0974fa98b907432c710a766a2135599ad Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 24 Jan 2025 17:15:09 +0100 Subject: [PATCH 81/94] Navigator: replace custom NAV_EPSILON_POSITION with FLT_EPSILON Signed-off-by: Silvan --- src/modules/navigator/mission_block.cpp | 2 +- src/modules/navigator/navigation.h | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 95912a22e3..53a7ce0466 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -622,7 +622,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->lon = item.lon; sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; - sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : + sp->loiter_radius = (fabsf(item.loiter_radius) > FLT_EPSILON) ? fabsf(item.loiter_radius) : _navigator->get_loiter_radius(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index ecf616ffc4..520430c014 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -56,8 +56,6 @@ # define NUM_MISSIONS_SUPPORTED 500 #endif -#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ - /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_IDLE = 0, From ddf591c4f588d6829facd5369059298a72ea01c2 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 24 Jan 2025 17:16:57 +0100 Subject: [PATCH 82/94] Navigator: use FLT_EPSILON instead of 0.0001f for >0 float comparison Signed-off-by: Silvan --- src/modules/navigator/mission_block.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 53a7ce0466..1b05628a6c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -626,7 +626,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi _navigator->get_loiter_radius(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; - if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) { + if (item.acceptance_radius > FLT_EPSILON && PX4_ISFINITE(item.acceptance_radius)) { // if the mission item has a specified acceptance radius, overwrite the default one from parameters sp->acceptance_radius = item.acceptance_radius; From 96105cacc094487247fbdf94c15c75f6ff3e4b75 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 29 Jan 2025 09:22:55 +0100 Subject: [PATCH 83/94] SIH airframes: clean up configs - set SIH_L_ROLL that agrees with CA_* rotor geometry - remove unnecessary params & comments - clarify that ailerons are single channel --- .../init.d-posix/airframes/10040_sihsim_quadx | 4 --- .../airframes/10041_sihsim_airplane | 6 ---- .../init.d-posix/airframes/10042_sihsim_xvert | 4 --- .../airframes/10043_sihsim_standard_vtol | 28 +++---------------- .../airframes/1103_standard_vtol_sih.hil | 16 +++-------- 5 files changed, 8 insertions(+), 50 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx index 077d908ec5..623ee41f1f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx @@ -16,10 +16,6 @@ param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 -# disable some checks to allow to fly: -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 - # Square quadrotor X PX4 numbering param set-default CA_ROTOR_COUNT 4 param set-default CA_ROTOR0_PX 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index c91e732a9e..4f1ddf2635 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -16,12 +16,6 @@ param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 -# disable some checks to allow to fly: -# - with usb -param set-default CBRK_USB_CHK 197848 -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 - param set-default SIH_T_MAX 6 param set-default SIH_MASS 0.3 param set-default SIH_IXX 0.00402 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index d82b92c1ef..ee5ea9401c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -28,10 +28,6 @@ param set-default MC_PITCH_P 5 param set-default MAV_TYPE 19 -# disable some checks to allow to fly: -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 - param set-default SIH_T_MAX 2 param set-default SIH_Q_MAX 0.0165 param set-default SIH_MASS 0.2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol index 6bfda5edf6..4abc9de264 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol @@ -10,7 +10,7 @@ # @output Motor3 MC motor front left # @output Motor4 MC motor back right # @output Motor5 Forward thrust motor -# @output Servo1 Aileron +# @output Servo1 Ailerons (single channel) # @output Servo2 Elevator # @output Servo3 Rudder # @@ -19,26 +19,16 @@ . ${R}etc/init.d/rc.vtol_defaults -# SIH in SITL changes. could we source these from a common file? {{{ -# inspired by: diff ROMFS/px4fmu_common/init.d/airframes/4001_quad_x ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx - PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 param set-default EKF2_GPS_DELAY 0 -# battery check disabled below already -# HIL_ACT_FUNC* set below -- do we need PWM_MAIN_FUNC* as well? -# SIH_VEHICLE_TYPE set below too. - -# }}} - -param set-default VT_B_TRANS_DUR 5 -param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 2 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 @@ -57,8 +47,6 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.2 param set-default CA_ROTOR3_PY 0.2 param set-default CA_ROTOR3_KM -0.05 - - param set-default CA_ROTOR4_PX -0.3 param set-default CA_ROTOR4_KM 0.05 param set-default CA_ROTOR4_AX 1 @@ -73,9 +61,9 @@ param set-default CA_SV_CS1_TYPE 3 # elevator param set-default CA_SV_CS2_TRQ_Y 1 param set-default CA_SV_CS2_TYPE 4 # rudder -param set-default FW_AIRSPD_MAX 12 param set-default FW_AIRSPD_MIN 7 param set-default FW_AIRSPD_TRIM 10 +param set-default FW_AIRSPD_MAX 12 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 @@ -91,12 +79,6 @@ param set-default MAV_TYPE 22 # set SYS_HITL to 2 to start the SIH and avoid sensors startup # param set-default SYS_HITL 2 -# disable some checks to allow to fly: -# - without real battery -param set-default CBRK_SUPPLY_CHK 894281 -# - without safety switch -param set-default CBRK_IO_SAFETY 22027 - param set-default SENS_DPRES_OFF 0.001 param set SIH_T_MAX 2.0 @@ -108,9 +90,7 @@ param set SIH_IYY 0.000625 param set SIH_IZZ 0.00300 param set SIH_IXZ 0 param set SIH_KDV 0.2 -param set SIH_L_ROLL 0.145 +param set SIH_L_ROLL 0.2 # sih as standard vtol param set SIH_VEHICLE_TYPE 3 - -param set-default VT_ARSP_TRANS 6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil index 0309d3c0dc..5ac9e6b1ab 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -10,7 +10,7 @@ # @output Motor3 MC motor front left # @output Motor4 MC motor back right # @output Motor5 Forward thrust motor -# @output Servo1 Aileron +# @output Servo1 Ailerons (single channel) # @output Servo2 Elevator # @output Servo3 Rudder # @@ -20,8 +20,7 @@ . ${R}etc/init.d/rc.vtol_defaults param set UAVCAN_ENABLE 0 -param set-default VT_B_TRANS_DUR 5 -param set-default VT_ELEV_MC_LOCK 0 + param set-default VT_TYPE 2 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 @@ -40,7 +39,6 @@ param set-default CA_ROTOR2_KM -0.05 param set-default CA_ROTOR3_PX -0.2 param set-default CA_ROTOR3_PY 0.2 param set-default CA_ROTOR3_KM -0.05 - param set-default CA_ROTOR4_PX -0.3 param set-default CA_ROTOR4_KM 0.05 param set-default CA_ROTOR4_AX 1 @@ -55,9 +53,9 @@ param set-default CA_SV_CS1_TYPE 3 # elevator param set-default CA_SV_CS2_TRQ_Y 1 param set-default CA_SV_CS2_TYPE 4 # rudder -param set-default FW_AIRSPD_MAX 12 param set-default FW_AIRSPD_MIN 7 param set-default FW_AIRSPD_TRIM 10 +param set-default FW_AIRSPD_MAX 12 param set-default HIL_ACT_FUNC1 101 param set-default HIL_ACT_FUNC2 102 @@ -70,16 +68,12 @@ param set-default HIL_ACT_FUNC8 105 param set-default MAV_TYPE 22 - - # set SYS_HITL to 2 to start the SIH and avoid sensors startup param set-default SYS_HITL 2 # disable some checks to allow to fly: # - without real battery param set-default CBRK_SUPPLY_CHK 894281 -# - without safety switch -param set-default CBRK_IO_SAFETY 22027 param set-default SENS_DPRES_OFF 0.001 @@ -92,9 +86,7 @@ param set SIH_IYY 0.000625 param set SIH_IZZ 0.00300 param set SIH_IXZ 0 param set SIH_KDV 0.2 -param set SIH_L_ROLL 0.145 +param set SIH_L_ROLL 0.2 # sih as standard vtol param set SIH_VEHICLE_TYPE 3 - -param set-default VT_ARSP_TRANS 6 From 1eb9d05f69030394ed08db6c3bb1aaa659b1a124 Mon Sep 17 00:00:00 2001 From: Sam 3 Firestorm <46759700+gillamkid@users.noreply.github.com> Date: Wed, 29 Jan 2025 10:37:00 -0700 Subject: [PATCH 84/94] Gazebo classic: report correct limits for H480 gimbal yaw (#24269) --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index c0e0751341..fbd8e9e6bb 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit c0e0751341d46108377bbae2ae1bb6da8a5d4106 +Subproject commit fbd8e9e6bbd2188de81677494f15885dead99c48 From 3b828e157a6d9b7d811c80c73d95fd39fca5e181 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 20 Jan 2025 13:52:07 +0100 Subject: [PATCH 85/94] MC att: clarify prioritization algorithm Especially rename "mix" which is just the delta yaw angle --- .../AttitudeControl/AttitudeControl.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 6f1585f801..02f3894f0e 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -68,17 +68,21 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const qd_red = qd; } else { - // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + // Transform rotation from current to desired thrust vector into a world frame reduced desired attitude. + // This is a right multiplication as the tilt error quaternion is obtained from two Z vectors expressed in the world frame. qd_red *= q; } - // mix full and reduced desired attitude - Quatf q_mix = qd_red.inversed() * qd; - q_mix.canonicalize(); + // With a full desired attitude given by: qd = qd_red * qd_dyaw, extract the delta yaw component. + // By definition, the delta yaw quaternion has the form (cos(angle/2), 0, 0, sin(angle/2)) + Quatf qd_dyaw = qd_red.inversed() * qd; + qd_dyaw.canonicalize(); // catch numerical problems with the domain of acosf and asinf - q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); - q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); - qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f); + qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f); + + // scale the delta yaw angle and re-combine the desired attitude + qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3)))); // quaternion attitude control law, qe is rotation from q to qd const Quatf qe = q.inversed() * qd; From f1423635752ef1ab960577dfe1b26dd8f547abc1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 29 Jan 2025 16:31:17 +0100 Subject: [PATCH 86/94] HealthAndArmingChecks: allow to warn in certain modes and fail arming checks in other modes Previously it was only possible to warn in all modes and fail none or fail and warn in certain modes. --- .../commander/HealthAndArmingChecks/Common.cpp | 8 ++++++++ .../commander/HealthAndArmingChecks/Common.hpp | 14 ++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index fb5639087e..16e074ba18 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -73,6 +73,14 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index); } +void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message) +{ + armingCheckFailure(required_modes.fail_modes, component, log_levels.external); + addEvent(event_id, log_levels, message, + (uint32_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index); +} + Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, uint32_t modes, unsigned args_size) { diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 826aa7629c..e069788ddf 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -74,6 +74,12 @@ static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatc static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t), "type too small, use next larger type"); +// Type to pass two mode groups in one struct to have the same number of function arguments to facilitate events parsing +struct NavModesMessageFail { + NavModes message_modes; ///< modes in which there's user messageing but arming is allowed + NavModes fail_modes; ///< modes in which checks fail which must be a subset of message_modes +}; + static inline NavModes operator|(NavModes a, NavModes b) { return static_cast(static_cast(a) | static_cast(b)); @@ -251,6 +257,14 @@ public: void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id, const events::LogLevels &log_levels, const char *message); + /** + * Overloaded variant of armingCheckFailure() which allows to separately specify modes in which a message should be emitted and a subset in which arming is blocked + * @param required_modes .message_modes modes in which to put out the event and hence user message. + * .failing_modes modes in which to to fail arming. Has to be a subset of message_modes to never disallow arming without a reason. + */ + void armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message); + void clearArmingBits(NavModes modes); /** From 4c2e69b2e65c7ce9fa1546887241051e8823a431 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 29 Jan 2025 16:34:15 +0100 Subject: [PATCH 87/94] estimatorCheck: only warn about GPS in modes that require a position but fail all modes if GPS required by configuration --- .../checks/estimatorCheck.cpp | 60 ++++++++++--------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 1cd140fe5b..96b32c947c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -273,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } if (!context.isArmed() && ekf_gps_check_fail) { - NavModes required_groups_gps; + NavModesMessageFail required_modes; events::Log log_level; switch (static_cast(_param_com_arm_wo_gps.get())) { @@ -281,17 +281,21 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* FALLTHROUGH */ case GnssArmingCheck::DenyArming: - required_groups_gps = required_groups; + required_modes.message_modes = required_modes.fail_modes = NavModes::All; log_level = events::Log::Error; break; case GnssArmingCheck::WarningOnly: - required_groups_gps = NavModes::None; // optional + required_modes.message_modes = (NavModes)(reporter.failsafeFlags().mode_req_local_position + | reporter.failsafeFlags().mode_req_local_position_relaxed + | reporter.failsafeFlags().mode_req_global_position); + // Only warn and don't block arming because there could still be a valid position estimate from another source e.g. optical flow, VIO + required_modes.fail_modes = NavModes::None; log_level = events::Log::Warning; break; case GnssArmingCheck::Disabled: - required_groups_gps = NavModes::None; + required_modes.message_modes = required_modes.fail_modes = NavModes::None; log_level = events::Log::Disabled; break; } @@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_fix_too_low"), log_level, "GPS fix too low"); @@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_num_sats_too_low"), log_level, "Not enough GPS Satellites"); @@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_pdop_too_high"), log_level, "GPS PDOP too high"); @@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_err_too_high"), log_level, "GPS Horizontal Position Error too high"); @@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_err_too_high"), log_level, "GPS Vertical Position Error too high"); @@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_speed_acc_too_low"), log_level, "GPS Speed Accuracy too low"); @@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_drift_too_high"), log_level, "GPS Horizontal Position Drift too high"); @@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_drift_too_high"), log_level, "GPS Vertical Position Drift too high"); @@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_speed_drift_too_high"), log_level, "GPS Horizontal Speed Drift too high"); @@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_speed_drift_too_high"), log_level, "GPS Vertical Speed Drift too high"); @@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_spoofed"), log_level, "GPS signal spoofed"); @@ -437,7 +441,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Estimator not using GPS"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_not_fusing"), log_level, "Estimator not using GPS"); @@ -446,7 +450,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Poor GPS Quality"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_generic"), log_level, "Poor GPS Quality"); } From a9214b3aa3f3e0fdd6b2bd9eb91732a61aa89cdc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 31 Jan 2025 12:59:41 +1300 Subject: [PATCH 88/94] gimbal: don't spoof gimbal device (#24271) The current approach was wrong because the gimbal protocol now handles the case properly where the autopilot is in charge of a non-MAVLink gimbal. This means that we don't need to send message "as if we were a gimbal device" and instead set thet gimbal_device_id to 1 (up to 6) to indicate we are in charge or a non-MAVLink gimbal. --- src/modules/gimbal/output_mavlink.cpp | 3 ++ src/modules/mavlink/mavlink_receiver.cpp | 7 +-- .../streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp | 51 +++++++------------ src/modules/simulation/gz_bridge/GZGimbal.hpp | 2 +- 4 files changed, 22 insertions(+), 41 deletions(-) diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index ca562ec870..ae5222c674 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -109,6 +109,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _stream_device_attitude_status(); + // If the output is MAVLink v1, then we signal this by referring to compid 1. + gimbal_device_id = 1; + _last_update = now; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 37328503b9..aa8b50d01a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3075,12 +3075,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - if (gimbal_device_info_msg.gimbal_device_id == 0) { - gimbal_information.gimbal_device_id = msg->compid; - - } else { - gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id; - } + gimbal_information.gimbal_device_id = msg->compid; _gimbal_device_information_pub.publish(gimbal_information); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index c8338996ae..ae7f3cbad1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -74,48 +74,31 @@ private: return false; } - if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) { - // A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id - mavlink_gimbal_device_attitude_status_t msg{}; + mavlink_gimbal_device_attitude_status_t msg{}; - msg.target_system = gimbal_device_attitude_status.target_system; - msg.target_component = gimbal_device_attitude_status.target_component; + msg.target_system = gimbal_device_attitude_status.target_system; + msg.target_component = gimbal_device_attitude_status.target_component; - msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; - msg.flags = gimbal_device_attitude_status.device_flags; + msg.flags = gimbal_device_attitude_status.device_flags; - msg.q[0] = gimbal_device_attitude_status.q[0]; - msg.q[1] = gimbal_device_attitude_status.q[1]; - msg.q[2] = gimbal_device_attitude_status.q[2]; - msg.q[3] = gimbal_device_attitude_status.q[3]; + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; - msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; - msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; - msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; - msg.failure_flags = gimbal_device_attitude_status.failure_flags; - msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; - msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; - msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; + msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; + msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; - mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); - - } else { - // We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID - mavlink_message_t message; - mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, - _mavlink->get_channel(), &message, - gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, - gimbal_device_attitude_status.timestamp / 1000, - gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, - gimbal_device_attitude_status.angular_velocity_x, - gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, - gimbal_device_attitude_status.failure_flags, - 0, 0, 0); - _mavlink->forward_message(&message, _mavlink); - } + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); return true; } diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 00b46becc4..662268abd2 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -115,7 +115,7 @@ private: const float _yaw_min = NAN; // infinite yaw const float _yaw_max = NAN; // infinite yaw - const uint8_t _gimbal_device_id = 154; // TODO the implementation differs from the protocol + const uint8_t _gimbal_device_id = 1; // Gimbal is implemented by the same component: options are 1..6 uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS bool init(const std::string &world_name, const std::string &model_name); From f7dadd9b89f3052735a12bd617755e78cf5305e2 Mon Sep 17 00:00:00 2001 From: Perre Date: Mon, 3 Feb 2025 15:51:55 +0100 Subject: [PATCH 89/94] Remove inclusion of rotors in library to enable test (#24286) --- .../control_allocation/CMakeLists.txt | 2 +- ...olAllocationSequentialDesaturationTest.cpp | 60 +++++++++++++++++-- 2 files changed, 56 insertions(+), 6 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index d65bd11fa0..4da638aac8 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) -# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 2e0af6bff4..cd478a1b00 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,17 +40,27 @@ #include #include -#include using namespace matrix; namespace { +struct RotorGeometryTest { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + float moment_ratio; +}; + +struct GeometryTest { + RotorGeometryTest rotors[ActuatorEffectiveness::NUM_ACTUATORS]; + int num_rotors{0}; +}; // Makes and returns a Geometry object for a "standard" quad-x quadcopter. -ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() +GeometryTest make_quad_x_geometry() { - ActuatorEffectivenessRotors::Geometry geometry = {}; + GeometryTest geometry = {}; geometry.rotors[0].position(0) = 1.0f; geometry.rotors[0].position(1) = 1.0f; geometry.rotors[0].position(2) = 0.0f; @@ -88,7 +98,6 @@ ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() geometry.rotors[3].moment_ratio = -0.05f; geometry.num_rotors = 4; - return geometry; } @@ -98,7 +107,48 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() ActuatorEffectiveness::EffectivenessMatrix effectiveness; effectiveness.setZero(); const auto geometry = make_quad_x_geometry(); - ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + // Minimalistically copied from ActuatorEffectivenessRotors::computeEffectivenessMatrix + for (int i = 0; i < geometry.num_rotors; i++) { + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Fill corresponding items in effectiveness matrix + for (int j = 0; j < 3; j++) { + effectiveness(j, i) = moment(j); + effectiveness(j + 3, i) = thrust(j); + } + } + return effectiveness; } From cb332e047d393785ee63d5e8f60e6d820bfafb25 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:31:31 +0100 Subject: [PATCH 90/94] obstacle-math: add standard obstacle map functions. These functions help simplify repeated calculations accross driver and collision prevention files that are computing bins, angles and sensor offsets in obstacle maps. --- src/lib/collision_prevention/ObstacleMath.cpp | 58 +++++++ src/lib/collision_prevention/ObstacleMath.hpp | 44 +++++ .../collision_prevention/ObstacleMathTest.cpp | 150 +++++++++++++++++- 3 files changed, 251 insertions(+), 1 deletion(-) diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp index 22d67c64ae..0605b664ec 100644 --- a/src/lib/collision_prevention/ObstacleMath.cpp +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -51,4 +51,62 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons distance *= horizontal_projection_scale; } +int get_bin_at_angle(float bin_width, float angle) +{ + int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width); + return wrap_bin(bin_at_angle, 360 / bin_width); +} + +int get_offset_bin_index(int bin, float bin_width, float angle_offset) +{ + int offset = get_bin_at_angle(bin_width, angle_offset); + return wrap_bin(bin - offset, 360 / bin_width); +} + +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation) +{ + float offset = 0.0f; + + switch (orientation) { + case SensorOrientation::ROTATION_YAW_0: + offset = 0.0f; + break; + + case SensorOrientation::ROTATION_YAW_45: + offset = M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_YAW_90: + offset = M_PI_F / 2.0f; + break; + + case SensorOrientation::ROTATION_YAW_135: + offset = 3.0f * M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_YAW_180: + offset = M_PI_F; + break; + + case SensorOrientation::ROTATION_YAW_225: + offset = -3.0f * M_PI_F / 4.0f; + break; + + case SensorOrientation::ROTATION_YAW_270: + offset = -M_PI_F / 2.0f; + break; + + case SensorOrientation::ROTATION_YAW_315: + offset = -M_PI_F / 4.0f; + break; + } + + return offset; +} + +int wrap_bin(int bin, int bin_count) +{ + return (bin + bin_count) % bin_count; +} + } // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp index 3c19eb9110..f5317aeaa7 100644 --- a/src/lib/collision_prevention/ObstacleMath.hpp +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -36,6 +36,28 @@ namespace ObstacleMath { +enum SensorOrientation { + ROTATION_YAW_0 = 0, // MAV_SENSOR_ROTATION_NONE + ROTATION_YAW_45 = 1, // MAV_SENSOR_ROTATION_YAW_45 + ROTATION_YAW_90 = 2, // MAV_SENSOR_ROTATION_YAW_90 + ROTATION_YAW_135 = 3, // MAV_SENSOR_ROTATION_YAW_135 + ROTATION_YAW_180 = 4, // MAV_SENSOR_ROTATION_YAW_180 + ROTATION_YAW_225 = 5, // MAV_SENSOR_ROTATION_YAW_225 + ROTATION_YAW_270 = 6, // MAV_SENSOR_ROTATION_YAW_270 + ROTATION_YAW_315 = 7, // MAV_SENSOR_ROTATION_YAW_315 + + ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE + ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 + ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180 + ROTATION_LEFT_FACING = 6 // MAV_SENSOR_ROTATION_YAW_270 +}; + +/** + * Converts a sensor orientation to a yaw offset + * @param orientation sensor orientation + */ +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation); + /** * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane * @param distance measurement which is scaled down @@ -44,4 +66,26 @@ namespace ObstacleMath */ void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle); +/** + * Returns bin index at a given angle from the 0th bin + * @param bin_width width of a bin in degrees + * @param angle clockwise angle from start bin in degrees + */ +int get_bin_at_angle(float bin_width, float angle); + +/** + * Returns bin index for the current bin after an angle offset + * @param bin current bin index + * @param bin_width width of a bin in degrees + * @param angle_offset clockwise angle offset in degrees + */ +int get_offset_bin_index(int bin, float bin_width, float angle_offset); + +/** + * Wraps a bin index to the range [0, bin_count) + * @param bin bin index + * @param bin_count number of bins + */ +int wrap_bin(int bin, int bin_count); + } // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp index e24b95134c..cea54aa4a2 100644 --- a/src/lib/collision_prevention/ObstacleMathTest.cpp +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -33,6 +33,7 @@ #include #include +#include #include "ObstacleMath.hpp" using namespace matrix; @@ -89,5 +90,152 @@ TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane) expected_distance = 1.0f * expected_scale; EXPECT_NEAR(distance, expected_distance, 1e-5); - +} + +TEST(ObstacleMathTest, GetBinAtAngle) +{ + float bin_width = 5.0f; + + // GIVEN: a start bin, bin width, and angle + float angle = 0.0f; + + // WHEN: we calculate the bin index at the angle + uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 0); + + // GIVEN: a start bin, bin width, and angle + angle = 90.0f; + + // WHEN: we calculate the bin index at the angle + bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 18); + + // GIVEN: a start bin, bin width, and angle + angle = -90.0f; + + // WHEN: we calculate the bin index at the angle + bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 54); + + // GIVEN: a start bin, bin width, and angle + angle = 450.0f; + + // WHEN: we calculate the bin index at the angle + bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle); + + // THEN: the bin index should be correct + EXPECT_EQ(bin_index, 18); +} + + +TEST(ObstacleMathTest, OffsetBinIndex) +{ + // In this test, we want to offset the bin index by a negative and positive angle. + // We take the output of the first offset and offset it by the same angle in the + // opposite direction to return back to the original bin index. + + // GIVEN: a bin index, bin width, and a negative angle offset + uint16_t bin = 0; + float bin_width = 5.0f; + float angle_offset = -120.0f; + + // WHEN: we offset the bin index by the negative angle + uint16_t new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset); + + // THEN: the new bin index should be correctly offset by the wrapped angle + EXPECT_EQ(new_bin_index, 24); + + // GIVEN: the output bin index of the previous offset, bin width, and the same angle + // offset in positive direction + bin = 24; + bin_width = 5.0f; + angle_offset = 120.0f; + + // WHEN: we offset the bin index by the positive angle + new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset); + + // THEN: the new bin index should return back to the original bin index + EXPECT_EQ(new_bin_index, 0); +} + + +TEST(ObstacleMathTest, WrapBin) +{ + // GIVEN: a bin index within bounds and the number of bins + int bin = 0; + int bin_count = 72; + + // WHEN: we wrap a bin index within the bounds + int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count); + + // THEN: the wrapped bin index should stay 0 + EXPECT_EQ(wrapped_bin, 0); + + // GIVEN: a bin index that is out of bounds, and the number of bins + bin = 73; + bin_count = 72; + + // WHEN: we wrap a bin index that is larger than the number of bins + wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count); + + // THEN: the wrapped bin index should be wrapped back to the beginning + EXPECT_EQ(wrapped_bin, 1); + + // GIVEN: a negative bin index and the number of bins + bin = -1; + bin_count = 72; + + // WHEN: we wrap a bin index that is negative + wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count); + + // THEN: the wrapped bin index should be wrapped back to the end + EXPECT_EQ(wrapped_bin, 71); +} + +TEST(ObstacleMathTest, HandleMissedBins) +{ + // In this test, the current and previous bin are adjacent to the bins that are outside + // the sensor field of view. The missed bins (0,1,6 & 7) should be populated, and no + // data should be filled in the bins outside the FOV. + + // GIVEN: measurements, current bin, previous bin, bin width, and field of view offset + float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0}; + int current_bin = 2; + int previous_bin = 5; + int bin_width = 45.0f; + float fov = 270.0f; + float fov_offset = 360.0f - fov / 2; + + float measurement = measurements[current_bin]; + + // WHEN: we handle missed bins + int current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, bin_width, fov_offset); + int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset); + + int start = math::min(current_bin_offset, previous_bin_offset) + 1; + int end = math::max(current_bin_offset, previous_bin_offset); + + EXPECT_EQ(start, 1); + EXPECT_EQ(end, 5); + + for (uint16_t i = start; i < end; i++) { + uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset); + measurements[bin_index] = measurement; + } + + // THEN: the correct missed bins should be populated with the measurement + EXPECT_EQ(measurements[0], 1); + EXPECT_EQ(measurements[1], 1); + EXPECT_EQ(measurements[2], 1); + EXPECT_EQ(measurements[3], 0); + EXPECT_EQ(measurements[4], 0); + EXPECT_EQ(measurements[5], 2); + EXPECT_EQ(measurements[6], 1); + EXPECT_EQ(measurements[7], 1); } From 31bff3e5bbf8aefb68d28215ed03f0cf5dedaac1 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:37:00 +0100 Subject: [PATCH 91/94] sf45: change handle_missed_bins() function logic. To simplify logic for wrap-around cases and cases in which bins outside the FOV may be filled. Bin indices are offset such that the 0th bin is the first bin within the sensor FOV. This way we can always fill bins from smallest to largest index. --- .../lightware_sf45_serial.cpp | 46 ++++++------------- .../lightware_sf45_serial.hpp | 1 + 2 files changed, 16 insertions(+), 31 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index a7a94c6791..e6fbe3e4b9 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -664,6 +664,7 @@ void SF45LaserSerial::sf45_process_replies() hrt_abstime now = hrt_absolute_time(); + _obstacle_distance.distances[current_bin] = _current_bin_dist; _handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now); _publish_obstacle_msg(now); @@ -701,43 +702,26 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ { // if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement. // in this case we assume the measurement to be valid for all bins between the previous and the current bin. - uint8_t start; - uint8_t end; - if (abs(current_bin - previous_bin) > BIN_COUNT / 4) { - // wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins - // THis is simplyfied as we are not considering the scaning direction - start = math::max(previous_bin, current_bin); - end = math::min(previous_bin, current_bin); + // Shift bin indices such that we can never have the wrap-around case. + const float fov_offset_angle = 360.0f - SF45_FIELDOF_VIEW / 2.f; + const uint16_t current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, _obstacle_distance.increment, + fov_offset_angle); + const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment, + fov_offset_angle); - } else if (previous_bin < current_bin) { // Scanning clockwise - start = previous_bin + 1; - end = current_bin; + const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1; + const uint16_t end = math::max(current_bin_offset, previous_bin_offset); - } else { // scanning counter-clockwise - start = current_bin; - end = previous_bin - 1; - } - - if (start <= end) { - for (uint8_t i = start; i <= end; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } - - } else { // wrap-around case - for (uint8_t i = start; i < BIN_COUNT; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } - - for (uint8_t i = 0; i <= end; i++) { - _obstacle_distance.distances[i] = measurement; - _data_timestamps[i] = now; - } + // populate the missed bins with the measurement + for (uint16_t i = start; i < end; i++) { + uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle); + _obstacle_distance.distances[bin_index] = measurement; + _data_timestamps[bin_index] = now; } } + uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { uint8_t mapped_sector = 0; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index bfe44f0da2..d308a474d4 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -105,6 +105,7 @@ private: obstacle_distance_s::distances[0]); static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms}; static constexpr float SF45_SCALE_FACTOR = 0.01f; + static constexpr float SF45_FIELDOF_VIEW = 320.f; // degrees void start(); void stop(); From 48c0992a7d89e898bd766ea3ccceaabb439401cf Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 3 Feb 2025 16:41:37 +0100 Subject: [PATCH 92/94] sf45: refactor how sensor orientation (yaw_cfg) correction is applied to incoming sensor data. yaw_cfg is now read into the obstacle_distance message as the angle_offset. The offset is computed once at init and applied to each measurement. --- .../lightware_sf45_serial.cpp | 39 +++++-------------- 1 file changed, 10 insertions(+), 29 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index e6fbe3e4b9..7f16fa5d26 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -92,6 +92,11 @@ int SF45LaserSerial::init() param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg); param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg); + // set the sensor orientation + const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast + (_yaw_cfg)); + _obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle)); + start(); return PX4_OK; } @@ -594,7 +599,6 @@ void SF45LaserSerial::sf45_process_replies() case SF_DISTANCE_DATA_CM: { const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8); int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8)); - int16_t scaled_yaw = 0; // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float if (raw_yaw > 32000) { @@ -607,33 +611,10 @@ void SF45LaserSerial::sf45_process_replies() } // SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}" - scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; + float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; - switch (_yaw_cfg) { - case ROTATION_FORWARD_FACING: - break; - - case ROTATION_BACKWARD_FACING: - if (scaled_yaw > 180) { - scaled_yaw = scaled_yaw - 180; - - } else { - scaled_yaw = scaled_yaw + 180; // rotation facing aft - } - - break; - - case ROTATION_RIGHT_FACING: - scaled_yaw = scaled_yaw + 90; // rotation facing right - break; - - case ROTATION_LEFT_FACING: - scaled_yaw = scaled_yaw - 90; // rotation facing left - break; - - default: - break; - } + // Adjust for sensor orientation + scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset); // Convert to meters for the debug message float distance_m = raw_distance * SF45_SCALE_FACTOR; @@ -642,7 +623,7 @@ void SF45LaserSerial::sf45_process_replies() uint8_t current_bin = sf45_convert_angle(scaled_yaw); if (current_bin != _previous_bin) { - PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin, + PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin, (double)distance_m); if (_vehicle_attitude_sub.updated()) { @@ -726,7 +707,7 @@ uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) { uint8_t mapped_sector = 0; float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset); - mapped_sector = round(adjusted_yaw / _obstacle_distance.increment); + mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment); return mapped_sector; } From 90a806f5f8bf170a3724e03750a7ebd48226c8b7 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 3 Feb 2025 08:10:02 -0900 Subject: [PATCH 93/94] ark: v6x: update net config (#24281) * ark: v6x: disable net binary config, update default net config * added back CONFIG_IPCFG_BINARY=y --------- Co-authored-by: Alex Klimaj --- boards/ark/fmu-v6x/nuttx-config/nsh/defconfig | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig index f5e59d2c37..94fbcc11b5 100644 --- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig @@ -145,10 +145,11 @@ CONFIG_NETDB_DNSCLIENT=y CONFIG_NETDB_DNSCLIENT_ENTRIES=8 CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y -CONFIG_NETINIT_DHCPC=y +CONFIG_NETMAN_FALLBACK_IPADDR=0xC0A80004 CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0xA290AFE -CONFIG_NETINIT_DRIPADDR=0xA290AFE +CONFIG_NETINIT_IPADDR=0xC0A80004 +CONFIG_NETINIT_DNSIPADDR=0xC0A800FE +CONFIG_NETINIT_DRIPADDR=0xC0A80001 CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 From 64d8f9a3c66bc5a7be9cd2db84d8989992983d1c Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 27 Jan 2025 10:43:20 +0100 Subject: [PATCH 94/94] cmake: for vscode launch fallback to gdb-multiarch Newer toolchains don't ship with arm-none-eabi-gdb hence we should use gdb-multiarch instead --- platforms/nuttx/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index ba89894efe..19fcae552e 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -506,6 +506,11 @@ if(NOT NUTTX_DIR MATCHES "external") message(STATUS "Found SVD: ${DEBUG_SVD_FILE_PATH}") endif() + if(NOT CMAKE_GDB) + find_program(CMAKE_GDB gdb-multiarch) + message(STATUS "Found GDB: ${CMAKE_GDB}") + endif() + if(DEBUG_DEVICE) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/launch.json.in ${PX4_SOURCE_DIR}/.vscode/launch.json @ONLY) endif()