From 9e5cfa330aa912e38d31ee61192da683fee8d7f7 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 5 Feb 2025 08:51:17 +1100 Subject: [PATCH 01/63] Commander: Quick calibration supports mag too --- src/modules/commander/Commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fef03b92c2..a5e1583175 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3036,7 +3036,7 @@ The commander module contains the state machine for mode switching and failsafe #ifndef CONSTRAINED_FLASH PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration"); PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false); - PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false); + PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false); PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); PRINT_MODULE_USAGE_COMMAND("arm"); PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); From 0faec9b3e70b4df8e5665c91152da69ce6af6823 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Thu, 19 Dec 2024 15:47:28 -0700 Subject: [PATCH 02/63] boards: ARK Flow MR --- .vscode/cmake-variants.yaml | 5 + boards/ark/can-flow-mr/canbootloader.px4board | 5 + boards/ark/can-flow-mr/firmware.prototype | 13 ++ boards/ark/can-flow-mr/init/rc.board_sensors | 20 ++ .../nuttx-config/canbootloader/defconfig | 57 ++++++ .../can-flow-mr/nuttx-config/include/board.h | 136 +++++++++++++ .../nuttx-config/include/board_dma_map.h | 44 ++++ .../can-flow-mr/nuttx-config/nsh/defconfig | 149 ++++++++++++++ .../scripts/canbootloader_script.ld | 134 +++++++++++++ .../nuttx-config/scripts/script.ld | 146 ++++++++++++++ boards/ark/can-flow-mr/src/CMakeLists.txt | 65 ++++++ boards/ark/can-flow-mr/src/board_config.h | 95 +++++++++ boards/ark/can-flow-mr/src/boot.c | 188 ++++++++++++++++++ boards/ark/can-flow-mr/src/boot_config.h | 130 ++++++++++++ boards/ark/can-flow-mr/src/can.c | 130 ++++++++++++ boards/ark/can-flow-mr/src/init.c | 147 ++++++++++++++ boards/ark/can-flow-mr/src/led.c | 124 ++++++++++++ boards/ark/can-flow-mr/src/led.h | 37 ++++ boards/ark/can-flow-mr/src/spi.cpp | 48 +++++ boards/ark/can-flow-mr/uavcan_board_identity | 17 ++ 20 files changed, 1690 insertions(+) create mode 100644 boards/ark/can-flow-mr/canbootloader.px4board create mode 100644 boards/ark/can-flow-mr/firmware.prototype create mode 100644 boards/ark/can-flow-mr/init/rc.board_sensors create mode 100644 boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig create mode 100644 boards/ark/can-flow-mr/nuttx-config/include/board.h create mode 100644 boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h create mode 100644 boards/ark/can-flow-mr/nuttx-config/nsh/defconfig create mode 100644 boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld create mode 100644 boards/ark/can-flow-mr/nuttx-config/scripts/script.ld create mode 100644 boards/ark/can-flow-mr/src/CMakeLists.txt create mode 100644 boards/ark/can-flow-mr/src/board_config.h create mode 100644 boards/ark/can-flow-mr/src/boot.c create mode 100644 boards/ark/can-flow-mr/src/boot_config.h create mode 100644 boards/ark/can-flow-mr/src/can.c create mode 100644 boards/ark/can-flow-mr/src/init.c create mode 100644 boards/ark/can-flow-mr/src/led.c create mode 100644 boards/ark/can-flow-mr/src/led.h create mode 100644 boards/ark/can-flow-mr/src/spi.cpp create mode 100644 boards/ark/can-flow-mr/uavcan_board_identity diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 4c2753ba05..03968a6e06 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -131,6 +131,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_can-flow_canbootloader + ark_can-flow-mr_canbootloader: + short: ark_can-flow-mr_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_can-flow-mr_canbootloader ark_can-gps_default: short: ark_can-gps_default buildType: MinSizeRel diff --git a/boards/ark/can-flow-mr/canbootloader.px4board b/boards/ark/can-flow-mr/canbootloader.px4board new file mode 100644 index 0000000000..46917280f6 --- /dev/null +++ b/boards/ark/can-flow-mr/canbootloader.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y diff --git a/boards/ark/can-flow-mr/firmware.prototype b/boards/ark/can-flow-mr/firmware.prototype new file mode 100644 index 0000000000..2f5c9cf0d9 --- /dev/null +++ b/boards/ark/can-flow-mr/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 88, + "magic": "PX4FWv1", + "description": "Firmware for the ARK Flow MR board", + "image": "", + "build_time": 0, + "summary": "ARKFLOWMR", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/can-flow-mr/init/rc.board_sensors b/boards/ark/can-flow-mr/init/rc.board_sensors new file mode 100644 index 0000000000..2d476fcefb --- /dev/null +++ b/boards/ark/can-flow-mr/init/rc.board_sensors @@ -0,0 +1,20 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ + +param set-default IMU_GYRO_RATEMAX 1000 +param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_CLPNOTI 0 + +param set-default SENS_AFBR_S_RATE 25 +param set-default SENS_AFBR_L_RATE 10 +param set-default SENS_AFBR_THRESH 8 +param set-default SENS_AFBR_HYSTER 2 + +# Internal SPI +paa3905 -s start -Y 180 + +iim42653 -R 0 -s start + +afbrs50 start diff --git a/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig b/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig new file mode 100644 index 0000000000..a4540cbe54 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig @@ -0,0 +1,57 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INIT_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/can-flow-mr/nuttx-config/include/board.h b/boards/ark/can-flow-mr/nuttx-config/include/board.h new file mode 100644 index 0000000000..70818cb251 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/include/board.h @@ -0,0 +1,136 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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 "board_dma_map.h" + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/* HSI - 8 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 8 MHz Crystal + * LSE - not installed + */ +#define STM32_BOARD_USEHSE 1 +#define STM32_BOARD_XTAL 8000000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 + +/* Main PLL Configuration */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8) +#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) + +#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/ + +#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB +#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ + +#define STM32_SYSCLK_FREQUENCY 96000000ul + +/* AHB clock (HCLK) is SYSCLK (96MHz) */ +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK (96MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* Timers driven from APB2 will be PCLK2 since no prescale division */ +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */ +#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* UARTs */ +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_1 +#define GPIO_CAN1_TX GPIO_CAN1_TX_1 + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_1 /* PB10 */ + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h b/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..0eb81fc589 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX // DMA1, Stream 3, Channel 0 +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA1, Stream 4, Channel 0 + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_2 // DMA2, Stream 2, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 // DMA2, Stream 5, Channel 3 diff --git a/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig b/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..ed36c45b2e --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig @@ -0,0 +1,149 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_CROMFS=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2624 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_VARS=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=254 +CONFIG_SCHED_HPWORKSTACKSIZE=3000 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_WAITPID=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=2048 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI2_DMA=y +CONFIG_STM32_SPI2_DMA_BUFFER=2048 +CONFIG_STM32_TIM8=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 0000000000..48a59fe92d --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,134 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld b/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..2f4769b8f5 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/can-flow-mr/src/CMakeLists.txt b/boards/ark/can-flow-mr/src/CMakeLists.txt new file mode 100644 index 0000000000..4fae41fc0e --- /dev/null +++ b/boards/ark/can-flow-mr/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + init.c + led.c + spi.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/ark/can-flow-mr/src/board_config.h b/boards/ark/can-flow-mr/src/board_config.h new file mode 100644 index 0000000000..ad71f05b63 --- /dev/null +++ b/boards/ark/can-flow-mr/src/board_config.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PB11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) + +/* CAN termination software control */ +#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI) + +/* LEDs are driven with open drain to support Anode to 5V or 3.3V */ +#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) + +#define BROADCOM_AFBR_S50_S2PI_SPI_BUS 2 +#define BROADCOM_AFBR_S50_S2PI_CS /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12) +#define BROADCOM_AFBR_S50_S2PI_IRQ /* PB4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN4|GPIO_EXTI) +#define BROADCOM_AFBR_S50_S2PI_CLK /* PB10 */ GPIO_SPI2_SCK_1 +#define BROADCOM_AFBR_S50_S2PI_MOSI /* PB15 */ GPIO_SPI2_MOSI_1 +#define BROADCOM_AFBR_S50_S2PI_MISO /* PB14 */ GPIO_SPI2_MISO_1 + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define PX4_GPIO_INIT_LIST { \ + GPIO_BOOT_CONFIG, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN1_TERMINATION, \ + } + +__BEGIN_DECLS + +#define BOARD_HAS_N_S_RGB_LED 1 +#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/can-flow-mr/src/boot.c b/boards/ark/can-flow-mr/src/boot.c new file mode 100644 index 0000000000..a26034e254 --- /dev/null +++ b/boards/ark/can-flow-mr/src/boot.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "boot_config.h" +#include "board.h" + +#include +#include +#include + +#include +#include "led.h" + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)} + +typedef begin_packed_struct struct led_t { + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t hz; +} end_packed_struct led_t; + +static const led_t i2l[] = { + + led(0, off, 0, 0, 0, 0), + led(1, reset, 128, 128, 128, 30), + led(2, autobaud_start, 0, 128, 0, 1), + led(3, autobaud_end, 0, 128, 0, 2), + led(4, allocation_start, 0, 0, 64, 2), + led(5, allocation_end, 0, 128, 64, 3), + led(6, fw_update_start, 32, 128, 64, 3), + led(7, fw_update_erase_fail, 32, 128, 32, 3), + led(8, fw_update_invalid_response, 64, 0, 0, 1), + led(9, fw_update_timeout, 64, 0, 0, 2), + led(a, fw_update_invalid_crc, 64, 0, 0, 4), + led(b, jump_to_app, 0, 128, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red, + i2l[indication].green, + i2l[indication].blue, + i2l[indication].hz); +} diff --git a/boards/ark/can-flow-mr/src/boot_config.h b/boards/ark/can-flow-mr/src/boot_config.h new file mode 100644 index 0000000000..76782f9a93 --- /dev/null +++ b/boards/ark/can-flow-mr/src/boot_config.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#define OPT_TBOOT_MS 3000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 50 +#define OPT_BL_NUMBER_TIMERS 7 + +/* + * This Option set is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + * + */ +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/can-flow-mr/src/can.c b/boards/ark/can-flow-mr/src/can.c new file mode 100644 index 0000000000..7737965dc6 --- /dev/null +++ b/boards/ark/can-flow-mr/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/ark/can-flow-mr/src/init.c b/boards/ark/can-flow-mr/src/init.c new file mode 100644 index 0000000000..a6290bdc7a --- /dev/null +++ b/boards/ark/can-flow-mr/src/init.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" +#include "led.h" +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + watchdog_init(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {2, 16 * 1024, 0x08008000}, + {3, 16 * 1024, 0x0800C000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + //px4_platform_configure(); + + return OK; +} diff --git a/boards/ark/can-flow-mr/src/led.c b/boards/ark/can-flow-mr/src/led.c new file mode 100644 index 0000000000..9a80cae089 --- /dev/null +++ b/boards/ark/can-flow-mr/src/led.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +#include "led.h" + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +void rgb_led(int r, int g, int b, int freqs) +{ + + long fosc = TMR_FREQUENCY; + long prescale = 2048; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = 0; + + if (!once) { + once = 1; + + /* Enabel Clock to Block */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + + /* Reload */ + + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); + val |= ATIM_EGR_UG; + putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); + + /* Set Prescaler STM32_TIM_SETCLOCK */ + + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); + + /* Enable STM32_TIM_SETMODE*/ + + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); + + + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | + (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); + putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P | + ATIM_CCER_CC2E | ATIM_CCER_CC2P | + ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET)); + + + stm32_configgpio(GPIO_TIM1_CH1); + stm32_configgpio(GPIO_TIM1_CH2); + stm32_configgpio(GPIO_TIM1_CH3); + + /* master output enable = on */ + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); + } + + long p = freqs == 0 ? p1s : p1s / freqs; + putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + + putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); + putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); + + val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); + + if (freqs == 0) { + val &= ~ATIM_CR1_CEN; + + } else { + val |= ATIM_CR1_CEN; + } + + putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + +} diff --git a/boards/ark/can-flow-mr/src/led.h b/boards/ark/can-flow-mr/src/led.h new file mode 100644 index 0000000000..b68e4aa70d --- /dev/null +++ b/boards/ark/can-flow-mr/src/led.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void rgb_led(int r, int g, int b, int freqs); +__END_DECLS diff --git a/boards/ark/can-flow-mr/src/spi.cpp b/boards/ark/can-flow-mr/src/spi.cpp new file mode 100644 index 0000000000..697ddbbf1a --- /dev/null +++ b/boards/ark/can-flow-mr/src/spi.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}), + initSPIDevice(DRV_FLOW_DEVTYPE_PAA3905, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin12}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ark/can-flow-mr/uavcan_board_identity b/boards/ark/can-flow-mr/uavcan_board_identity new file mode 100644 index 0000000000..b0ecb3df4f --- /dev/null +++ b/boards/ark/can-flow-mr/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 88) +set(uavcanblid_name "\"org.ark.can-flow-mr\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) From bd2a00921702b1cd085d5c5eedfd4ebb88b095e1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Jun 2024 19:56:26 -0400 Subject: [PATCH 03/63] commander: remove MC nav test --- .../checks/estimatorCheck.cpp | 64 +------------------ .../checks/estimatorCheck.hpp | 6 -- 2 files changed, 2 insertions(+), 68 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 96b32c947c..994bf9cbc2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -590,66 +590,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & } } } - - const hrt_abstime now = hrt_absolute_time(); - - /* Check estimator status for signs of bad yaw induced post takeoff navigation failure - * for a short time interval after takeoff. - * Most of the time, the drone can recover from a bad initial yaw using GPS-inertial - * heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but - * if this does not fix the issue we need to stop using a position controlled - * mode to prevent flyaway crashes. - */ - - if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - if (!context.isArmed()) { - _nav_test_failed = false; - _nav_test_passed = false; - - } else { - if (!_nav_test_passed) { - // Both test ratios need to pass/fail together to change the nav test status - const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f) - && (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON); - - const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f); - - if (innovation_pass) { - _time_last_innov_pass = now; - - // if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed - const bool sufficient_time = (context.status().takeoff_time != 0) && (now > context.status().takeoff_time + 30_s); - const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f); - - // Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds - if ((now > _time_last_innov_fail + 10_s) && (sufficient_time || sufficient_speed)) { - _nav_test_passed = true; - _nav_test_failed = false; - } - - } else if (innovation_fail) { - _time_last_innov_fail = now; - - if (now > _time_last_innov_pass + 2_s) { - // if the innovation test has failed continuously, declare the nav as failed - _nav_test_failed = true; - /* EVENT - * @description - * Land and recalibrate the sensors. - */ - reporter.healthFailure(NavModes::All, health_component_t::local_position_estimate, - events::ID("check_estimator_nav_failure"), - events::Log::Emergency, "Navigation failure"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t"); - } - } - } - } - } - } } void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const @@ -739,8 +679,8 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // Check if quality checking of position accuracy and consistency is to be performed const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get(); - bool xy_valid = lpos.xy_valid && !_nav_test_failed; - bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; + bool xy_valid = lpos.xy_valid; + bool v_xy_valid = lpos.v_xy_valid; if (!context.isArmed()) { if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index b5cc6c2185..0e10484023 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -100,12 +100,6 @@ private: hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec) hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec) - // variables used to check for navigation failure after takeoff - hrt_abstime _time_last_innov_pass{0}; ///< last time velocity and position innovations passed - hrt_abstime _time_last_innov_fail{0}; ///< last time velocity and position innovations failed - bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed - bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - bool _gps_was_fused{false}; bool _gnss_spoofed{false}; From de650cab9ed6f17908a669f69e0d4fc4a354b9b0 Mon Sep 17 00:00:00 2001 From: Junwoo Hwang Date: Thu, 6 Feb 2025 03:09:02 +0900 Subject: [PATCH 04/63] boards/matek/h743-slim: change parameter path to MicroSD --- boards/matek/h743-slim/default.px4board | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index 71024fea5c..48e937fc43 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -5,6 +5,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_BOARD_PARAM_FILE="/fs/microsd/params" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y From 8d296a50f9b3cf2842dfb5e49037fbbd3ecbe532 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 5 Feb 2025 12:14:19 +0100 Subject: [PATCH 05/63] FlightTask: properly initialize EKF reset counters This fixes a race condition when switching from a flight mode that is not a flight task (e.g.: stabilized). In this case, the reset counters were initialized to 0 and deltas were applied to the first setpoints if the EKF had any of its reset counters different from 0. --- .../flight_mode_manager/FlightModeManager.cpp | 3 --- .../tasks/FlightTask/FlightTask.cpp | 11 +++++++++ .../tasks/FlightTask/FlightTask.hpp | 23 ++++++++----------- 3 files changed, 21 insertions(+), 16 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index c58a7fa157..f17e00aa96 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -374,11 +374,9 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) // Save current setpoints for the next FlightTask trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint; - ekf_reset_counters_s last_reset_counters{}; if (isAnyTaskActive()) { last_setpoint = _current_task.task->getTrajectorySetpoint(); - last_reset_counters = _current_task.task->getResetCounters(); } if (_initTask(new_task_index)) { @@ -399,7 +397,6 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) return FlightTaskError::ActivationFailed; } - _current_task.task->setResetCounters(last_reset_counters); _command_failed = false; return FlightTaskError::NoError; diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index cbecf517f6..fa2943f147 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -11,6 +11,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint) { _resetSetpoints(); _setDefaultConstraints(); + initEkfResetCounters(); _time_stamp_activate = hrt_absolute_time(); _gear = empty_landing_gear_default_keep; return true; @@ -24,6 +25,16 @@ void FlightTask::reActivate() activate(setpoint_preserve_vertical); } +void FlightTask::initEkfResetCounters() +{ + _reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter; + _reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter; + _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; + _reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter; + _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; + _reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter; +} + bool FlightTask::updateInitialize() { _time_stamp_current = hrt_absolute_time(); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 6ee861f3cd..06327d8ca8 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -55,15 +55,6 @@ #include #include -struct ekf_reset_counters_s { - uint8_t xy; - uint8_t vxy; - uint8_t z; - uint8_t vz; - uint8_t heading; - uint8_t hagl; -}; - class FlightTask : public ModuleParams { public: @@ -88,6 +79,8 @@ public: */ virtual void reActivate(); + virtual void initEkfResetCounters(); + /** * To be called to adopt parameters from an arrived vehicle command * @param command received command message containing the parameters @@ -115,9 +108,6 @@ public: */ const trajectory_setpoint_s getTrajectorySetpoint(); - const ekf_reset_counters_s getResetCounters() const { return _reset_counters; } - void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; } - /** * Get vehicle constraints. * The constraints can vary with task. @@ -236,7 +226,14 @@ protected: float _yaw_setpoint{}; float _yawspeed_setpoint{}; - ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets + struct ekf_reset_counters_s { + uint8_t xy; + uint8_t vxy; + uint8_t z; + uint8_t vz; + uint8_t heading; + uint8_t hagl; + } _reset_counters{}; ///< Counters for estimator local position resets /** * Vehicle constraints. From fd175d619c9e23e6bcd6f07e5fa21167dab35321 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Fri, 7 Feb 2025 05:43:34 +0100 Subject: [PATCH 06/63] boards/modalai/voxl2/target: remove trailing spaces, consistent tabs/spaces in files --- boards/modalai/voxl2/target/voxl-px4 | 28 +++--- .../modalai/voxl2/target/voxl-px4-hitl-start | 4 +- boards/modalai/voxl2/target/voxl-px4-start | 88 +++++++++---------- 3 files changed, 60 insertions(+), 60 deletions(-) diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index bf1b62e981..514fc026bb 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -74,49 +74,49 @@ print_config_settings(){ while getopts "bcdhfmorwz" flag do case "${flag}" in - b) + b) echo "[INFO] Holybro GPS selected" GPS=HOLYBRO ;; - c) + c) echo "[INFO] Wiping old config file" if [ -f "$CONFIG_FILE" ]; then rm -rf ${CONFIG_FILE} fi exit 0 ;; - d) + d) echo "[INFO] Disabling daemon mode" DAEMON_MODE=DISABLE ;; - h) + h) print_usage ;; - f) + f) echo "[INFO] Setting RC to FAKE_RC_INPUT" RC=FAKE_RC_INPUT ;; - m) + m) echo "[INFO] Matek GPS selected" GPS=MATEK ;; - o) + o) echo "[INFO] OSD module selected" OSD=ENABLE ;; - r) + r) echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected" RC=CRSF_MAV ;; - w) + w) echo "[INFO] TBS Crossfire RC receiver, raw selected" RC=CRSF_RAW ;; - z) + z) echo "[INFO] Fake sensor calibration values selected" SENSOR_CAL=FAKE ;; - *) + *) print_usage ;; esac @@ -130,9 +130,9 @@ else fi if [ $SENSOR_CAL == "FAKE" ]; then - /bin/echo "[INFO] Setting up fake sensor calibration values" - px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config - /bin/sync + /bin/echo "[INFO] Setting up fake sensor calibration values" + px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config + /bin/sync fi print_config_settings diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start index 9dcc4bb4f0..3f8bcdd503 100755 --- a/boards/modalai/voxl2/target/voxl-px4-hitl-start +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start @@ -35,9 +35,9 @@ fi /bin/sleep 1 if [ ! -f /data/px4/param/hitl_parameters ]; then - echo "[INFO] Setting default parameters for PX4 on voxl" + echo "[INFO] Setting default parameters for PX4 on voxl" . /etc/modalai/voxl-px4-hitl-set-default-parameters.config - /bin/sync + /bin/sync else param select /data/px4/param/hitl_parameters param load diff --git a/boards/modalai/voxl2/target/voxl-px4-start b/boards/modalai/voxl2/target/voxl-px4-start index 014d046c8b..66dfa194e5 100755 --- a/boards/modalai/voxl2/target/voxl-px4-start +++ b/boards/modalai/voxl2/target/voxl-px4-start @@ -13,7 +13,7 @@ echo "OSD: $OSD" echo "EXTRA STEPS:" for i in "${EXTRA_STEPS[@]}" do - echo -e "\t$i" + echo -e "\t$i" done echo -e "*************************\n" @@ -83,17 +83,17 @@ qshell ist8310 start -R 10 -X -b 1 # GPS and magnetometer if [ "$GPS" != "NONE" ]; then - # On M0052 the GPS driver runs on the apps processor - if [ $PLATFORM = "M0052" ]; then - gps start -d /dev/ttyHS2 - # On M0054 and M0104 the GPS driver runs on SLPI DSP - else - qshell gps start -d 6 - fi + # On M0052 the GPS driver runs on the apps processor + if [ $PLATFORM = "M0052" ]; then + gps start -d /dev/ttyHS2 + # On M0054 and M0104 the GPS driver runs on SLPI DSP + else + qshell gps start -d 6 + fi fi # Auto detect an ncp5623c i2c RGB LED. If one isn't connected this will -# fail but not cause any harm. +# fail but not cause any harm. /bin/echo "Looking for ncp5623c RGB LED" qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56 @@ -107,17 +107,17 @@ param touch SYS_AUTOSTART # ESC driver if [ "$ESC" == "VOXL_ESC" ]; then /bin/echo "Starting VOXL ESC driver" - qshell voxl_esc start + qshell voxl_esc start elif [ "$ESC" == "VOXL2_IO_PWM_ESC" ]; then - if [ "$RC" == "M0065_SBUS" ]; then - /bin/echo "Starting VOXL IO for PWM ESC with SBUS RC" - qshell voxl2_io start - else - /bin/echo "Starting VOXL IO for PWM ESC without SBUS RC" - qshell voxl2_io start -e - fi + if [ "$RC" == "M0065_SBUS" ]; then + /bin/echo "Starting VOXL IO for PWM ESC with SBUS RC" + qshell voxl2_io start + else + /bin/echo "Starting VOXL IO for PWM ESC without SBUS RC" + qshell voxl2_io start -e + fi else - /bin/echo "No ESC type specified, not starting an ESC driver" + /bin/echo "No ESC type specified, not starting an ESC driver" fi @@ -133,41 +133,41 @@ elif [ "$RC" == "CRSF_MAV" ]; then qshell mavlink_rc_in start -m -p 7 -b 115200 elif [ "$RC" == "SPEKTRUM" ]; then /bin/echo "Starting Spektrum RC" - # On M0052 the RC driver runs on the apps processor - if [ $PLATFORM = "M0052" ]; then + # On M0052 the RC driver runs on the apps processor + if [ $PLATFORM = "M0052" ]; then rc_input start -d /dev/ttyHS1 - # On M0054 and M0104 the RC driver runs on SLPI DSP - else + # On M0054 and M0104 the RC driver runs on SLPI DSP + else qshell spektrum_rc start - fi + fi elif [ "$RC" == "GHST" ]; then /bin/echo "Starting GHST RC driver" qshell ghst_rc start -d 7 elif [ "$RC" == "M0065_SBUS" ]; then - if [ $PLATFORM = "M0052" ]; then - apps_sbus start - elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then - /bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW" - qshell dsp_sbus start - retVal=$? - if [ $retVal -ne 0 ]; then - /bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed" - /bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW" - qshell voxl2_io start -d -p 7 - fi - else - /bin/echo "M0065 SBUS RC driver already started with PWM ESC start" - fi + if [ $PLATFORM = "M0052" ]; then + apps_sbus start + elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then + /bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW" + qshell dsp_sbus start + retVal=$? + if [ $retVal -ne 0 ]; then + /bin/echo "Starting M0065 SBUS RC driver for original M0065 FW failed" + /bin/echo "Attempting to start M0065 SBUS RC driver for new M0065 FW" + qshell voxl2_io start -d -p 7 + fi + else + /bin/echo "M0065 SBUS RC driver already started with PWM ESC start" + fi fi if [ "$DISTANCE_SENSOR" == "LIGHTWARE_SF000" ]; then - # Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor - qshell lightware_laser_serial start -d 7 + # Make sure to set the parameter SENS_EN_SF0X to 8 for sf000/b sensor + qshell lightware_laser_serial start -d 7 fi if [ "$POWER_MANAGER" == "VOXLPM" ]; then - # APM power monitor - qshell voxlpm start -X -b 2 + # APM power monitor + qshell voxlpm start -X -b 2 fi # Optional distance sensor on spare i2c @@ -191,7 +191,7 @@ qshell load_mon start # is publishing input_rc topics. Otherwise for external RC # over Mavlink this isn't needed. if [ "$RC" != "EXTERNAL" ]; then - qshell rc_update start + qshell rc_update start fi qshell commander start @@ -214,7 +214,7 @@ voxl_save_cal_params start # On M0052 there is only one IMU. So, PX4 needs to # publish IMU samples externally for VIO to use. if [ $PLATFORM = "M0052" ]; then - imu_server start + imu_server start fi # start the onboard fast link to connect to voxl-mavlink-server @@ -250,5 +250,5 @@ fi # Start optional EXTRA_STEPS for i in "${EXTRA_STEPS[@]}" do - $i + $i done From de1ade8eb883aa2d6cccebc687471252409d244c Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Fri, 7 Feb 2025 05:47:32 +0100 Subject: [PATCH 07/63] sensors/vehicle_air_data: only trigger the sensor fallback when the baro_fault flag switches from 0 -> 1 --- src/modules/sensors/vehicle_air_data/VehicleAirData.cpp | 6 +++++- src/modules/sensors/vehicle_air_data/VehicleAirData.hpp | 2 ++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 24996c84da..263a56090f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -200,7 +200,7 @@ void VehicleAirData::Run() } if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index - && estimator_status_flags.cs_baro_fault) { + && estimator_status_flags.cs_baro_fault && !_last_status_baro_fault) { _priority[uorb_index] = 1; // 1 is min priority while still being enabled } @@ -225,6 +225,10 @@ void VehicleAirData::Run() } } + if (estimator_status_flags_updated) { + _last_status_baro_fault = estimator_status_flags.cs_baro_fault; + } + // check for the current best sensor int best_index = 0; _voter.get_best(time_now_us, &best_index); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index ce7a8a117c..a0bdb4007d 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -126,6 +126,8 @@ private: float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature + bool _last_status_baro_fault{false}; + DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, (ParamFloat) _param_sens_baro_rate From e7e76e2e2124239319768e1f3e4e7b8942797bcf Mon Sep 17 00:00:00 2001 From: Pedro Roque Date: Fri, 7 Feb 2025 05:54:24 +0100 Subject: [PATCH 08/63] Spacecraft build and bare control allocator (#24221) --- .vscode/settings.json | 1 - .../71001_gazebo-classic_spacecraft_dart | 155 ++++++++++++ .../airframes/71002_gz_spacecraft_2d | 149 +++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 3 + ROMFS/px4fmu_common/init.d/CMakeLists.txt | 7 + .../init.d/airframes/70000_spacecraft_2d | 150 +++++++++++ ROMFS/px4fmu_common/init.d/rc.sc_apps | 36 +++ ROMFS/px4fmu_common/init.d/rc.sc_defaults | 27 ++ Tools/kconfig/allyesconfig.py | 2 + Tools/simulation/gz | 2 +- boards/px4/sitl/default.px4board | 1 + boards/px4/sitl/spacecraft.px4board | 17 ++ cmake/metadata.cmake | 1 + .../control_allocator/ControlAllocator.cpp | 8 + .../control_allocator/ControlAllocator.hpp | 2 + src/modules/control_allocator/module.yaml | 2 + src/modules/spacecraft/CMakeLists.txt | 51 ++++ src/modules/spacecraft/Kconfig | 12 + src/modules/spacecraft/SpacecraftHandler.cpp | 91 +++++++ src/modules/spacecraft/SpacecraftHandler.hpp | 87 +++++++ src/modules/spacecraft/module.yaml | 235 ++++++++++++++++++ 21 files changed, 1037 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d create mode 100644 ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d create mode 100644 ROMFS/px4fmu_common/init.d/rc.sc_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.sc_defaults create mode 100644 boards/px4/sitl/spacecraft.px4board create mode 100644 src/modules/spacecraft/CMakeLists.txt create mode 100644 src/modules/spacecraft/Kconfig create mode 100644 src/modules/spacecraft/SpacecraftHandler.cpp create mode 100644 src/modules/spacecraft/SpacecraftHandler.hpp create mode 100644 src/modules/spacecraft/module.yaml diff --git a/.vscode/settings.json b/.vscode/settings.json index 0192c81e86..1e799721ce 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -129,5 +129,4 @@ "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" }, - "ros.distro": "humble" } diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart new file mode 100644 index 0000000000..737ab67b85 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71001_gazebo-classic_spacecraft_dart @@ -0,0 +1,155 @@ +#!/bin/sh +# +# @name 6DoF Spacecraft Model +# +# @type Freeflyer with 8 thrusters +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 15 +param set-default MAV_TYPE 99 + +param set-default CA_THRUSTER_CNT 12 +param set-default CA_R_REV 0 + +# param set-default FW_ARSP_MODE 1 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# Set thrusters +param set-default CA_THRUSTER0_PX -0.50 +param set-default CA_THRUSTER0_PY 0.50 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 0.237 +param set-default CA_THRUSTER0_AX 0.0 +param set-default CA_THRUSTER0_AY -1.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.50 +param set-default CA_THRUSTER1_PY 0.50 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 0.237 +param set-default CA_THRUSTER1_AX 0.0 +param set-default CA_THRUSTER1_AY -1.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX 0.50 +param set-default CA_THRUSTER2_PY -0.50 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 0.237 +param set-default CA_THRUSTER2_AX 0.0 +param set-default CA_THRUSTER2_AY 1.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX -0.50 +param set-default CA_THRUSTER3_PY -0.50 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 0.237 +param set-default CA_THRUSTER3_AX 0.0 +param set-default CA_THRUSTER3_AY 1.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX -0.50 +param set-default CA_THRUSTER4_PY 0.0 +param set-default CA_THRUSTER4_PZ -0.50 +param set-default CA_THRUSTER4_CT 0.237 +param set-default CA_THRUSTER4_AX 1.0 +param set-default CA_THRUSTER4_AY 0.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.50 +param set-default CA_THRUSTER5_PY 0.0 +param set-default CA_THRUSTER5_PZ -0.50 +param set-default CA_THRUSTER5_CT 0.237 +param set-default CA_THRUSTER5_AX -1.0 +param set-default CA_THRUSTER5_AY 0.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX 0.50 +param set-default CA_THRUSTER6_PY 0.0 +param set-default CA_THRUSTER6_PZ 0.50 +param set-default CA_THRUSTER6_CT 0.237 +param set-default CA_THRUSTER6_AX -1.0 +param set-default CA_THRUSTER6_AY 0.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.50 +param set-default CA_THRUSTER7_PY 0.0 +param set-default CA_THRUSTER7_PZ 0.50 +param set-default CA_THRUSTER7_CT 0.237 +param set-default CA_THRUSTER7_AX 1.0 +param set-default CA_THRUSTER7_AY 0.0 +param set-default CA_THRUSTER7_AZ 0.0 + +param set-default CA_THRUSTER8_PX 0.0 +param set-default CA_THRUSTER8_PY -0.50 +param set-default CA_THRUSTER8_PZ -0.50 +param set-default CA_THRUSTER8_CT 0.237 +param set-default CA_THRUSTER8_AX 0.0 +param set-default CA_THRUSTER8_AY 0.0 +param set-default CA_THRUSTER8_AZ 1.0 + +param set-default CA_THRUSTER9_PX 0.0 +param set-default CA_THRUSTER9_PY 0.50 +param set-default CA_THRUSTER9_PZ -0.50 +param set-default CA_THRUSTER9_CT 0.237 +param set-default CA_THRUSTER9_AX 0.0 +param set-default CA_THRUSTER9_AY 0.0 +param set-default CA_THRUSTER9_AZ 1.0 + +param set-default CA_THRUSTER10_PX 0.0 +param set-default CA_THRUSTER10_PY 0.50 +param set-default CA_THRUSTER10_PZ 0.50 +param set-default CA_THRUSTER10_CT 0.237 +param set-default CA_THRUSTER10_AX 0.0 +param set-default CA_THRUSTER10_AY 0.0 +param set-default CA_THRUSTER10_AZ -1.0 + +param set-default CA_THRUSTER11_PX 0.0 +param set-default CA_THRUSTER11_PY -0.50 +param set-default CA_THRUSTER11_PZ 0.50 +param set-default CA_THRUSTER11_CT 0.237 +param set-default CA_THRUSTER11_AX 0.0 +param set-default CA_THRUSTER11_AY 0.0 +param set-default CA_THRUSTER11_AZ -1.0 + +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 105 +param set-default PWM_MAIN_FUNC6 106 +param set-default PWM_MAIN_FUNC7 107 +param set-default PWM_MAIN_FUNC8 108 +param set-default PWM_MAIN_FUNC9 109 +param set-default PWM_MAIN_FUNC10 110 +param set-default PWM_MAIN_FUNC11 111 +param set-default PWM_MAIN_FUNC12 112 + +# PWM Simulation +param set PWM_SIM_PWM_MAX 10000 +param set PWM_SIM_PWM_MIN 0 + +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d new file mode 100644 index 0000000000..480454d72b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -0,0 +1,149 @@ +#!/bin/sh +# +# @name 3DoF Spacecraft Model +# +# @type 2D Freeflyer with 8 thrusters - Planar motion +# +# @maintainer Pedro Roque +# + +. ${R}etc/init.d/rc.sc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} + +param set-default SIM_GZ_EN 1 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 +param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs +param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 99 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 0 + +# param set-default FW_ARSP_MODE 1 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 # 0 is PseudoInverse, 3 is Metric + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.4 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.4 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.4 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.4 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.4 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.4 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.4 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.4 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + +param set-default SIM_GZ_TH_FUNC1 101 +param set-default SIM_GZ_TH_FUNC2 102 +param set-default SIM_GZ_TH_FUNC3 103 +param set-default SIM_GZ_TH_FUNC4 104 +param set-default SIM_GZ_TH_FUNC5 105 +param set-default SIM_GZ_TH_FUNC6 106 +param set-default SIM_GZ_TH_FUNC7 107 +param set-default SIM_GZ_TH_FUNC8 108 + +param set-default SIM_GZ_TH_MIN1 0 +param set-default SIM_GZ_TH_MIN2 0 +param set-default SIM_GZ_TH_MIN3 0 +param set-default SIM_GZ_TH_MIN4 0 +param set-default SIM_GZ_TH_MIN5 0 +param set-default SIM_GZ_TH_MIN6 0 +param set-default SIM_GZ_TH_MIN7 0 +param set-default SIM_GZ_TH_MIN8 0 + +param set-default SIM_GZ_TH_MAX1 10000 +param set-default SIM_GZ_TH_MAX2 10000 +param set-default SIM_GZ_TH_MAX3 10000 +param set-default SIM_GZ_TH_MAX4 10000 +param set-default SIM_GZ_TH_MAX5 10000 +param set-default SIM_GZ_TH_MAX6 10000 +param set-default SIM_GZ_TH_MAX7 10000 +param set-default SIM_GZ_TH_MAX8 10000 + +# Controller Tunings +param set SC_YAWRATE_P 3.335 +param set SC_YAWRATE_I 0.87 +param set SC_YAWRATE_D 0.15 +param set SC_YR_INT_LIM 0.2 +param set SC_YAW_P 3.0 + +param set SPC_POS_P 0.20 +param set SPC_VEL_P 6.55 +param set SPC_VEL_I 0.0 +param set SPC_VEL_D 0.0 +param set SPC_VEL_MAX 12.0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 51d9eab72e..94f2f4d39a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -114,5 +114,8 @@ px4_add_romfs_files( 17001_flightgear_tf-g1 17002_flightgear_tf-g2 + 71001_gazebo-classic_spacecraft_dart + 71002_gz_spacecraft_2d + # [22000, 22999] Reserve for custom models ) diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 488b41575a..7c6f8f4369 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -92,6 +92,13 @@ if(CONFIG_MODULES_ROVER_ACKERMANN) ) endif() +if(CONFIG_MODULES_SPACECRAFT) + px4_add_romfs_files( + rc.sc_apps + rc.sc_defaults + ) +endif() + if(CONFIG_MODULES_ROVER_MECANUM) px4_add_romfs_files( rc.rover_mecanum_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d new file mode 100644 index 0000000000..c0f55737f5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/70000_spacecraft_2d @@ -0,0 +1,150 @@ +#!/bin/sh +# +# @name KTH Space Robot +# +# @type Space Robot +# @class 2D Space Robot +# +# @maintainer DISCOWER +# + +. ${R}etc/init.d/rc.sc_defaults + +param set-default CA_AIRFRAME 14 +param set-default MAV_TYPE 99 + +param set-default CA_THRUSTER_CNT 8 +param set-default CA_R_REV 0 + +# Auto to be provided by Custom Airframe +param set-default CA_METHOD 0 + +# Set proper failsafes +param set-default COM_ACT_FAIL_ACT 0 +param set-default COM_LOW_BAT_ACT 0 +param set-default NAV_DLL_ACT 0 +param set-default GF_ACTION 1 +param set-default NAV_RCL_ACT 1 +param set-default COM_POSCTL_NAVL 2 + +# Set Mocap Vision frame +param set EKF2_EV_CTRL 15 +param set EKF2_HGT_REF 3 + +# disable attitude failure detection +param set-default FD_FAIL_P 0 +param set-default FD_FAIL_R 0 + +param set-default CA_THRUSTER0_PX -0.12 +param set-default CA_THRUSTER0_PY -0.12 +param set-default CA_THRUSTER0_PZ 0.0 +param set-default CA_THRUSTER0_CT 1.4 +param set-default CA_THRUSTER0_AX 1.0 +param set-default CA_THRUSTER0_AY 0.0 +param set-default CA_THRUSTER0_AZ 0.0 + +param set-default CA_THRUSTER1_PX 0.12 +param set-default CA_THRUSTER1_PY -0.12 +param set-default CA_THRUSTER1_PZ 0.0 +param set-default CA_THRUSTER1_CT 1.4 +param set-default CA_THRUSTER1_AX -1.0 +param set-default CA_THRUSTER1_AY 0.0 +param set-default CA_THRUSTER1_AZ 0.0 + +param set-default CA_THRUSTER2_PX -0.12 +param set-default CA_THRUSTER2_PY 0.12 +param set-default CA_THRUSTER2_PZ 0.0 +param set-default CA_THRUSTER2_CT 1.4 +param set-default CA_THRUSTER2_AX 1.0 +param set-default CA_THRUSTER2_AY 0.0 +param set-default CA_THRUSTER2_AZ 0.0 + +param set-default CA_THRUSTER3_PX 0.12 +param set-default CA_THRUSTER3_PY 0.12 +param set-default CA_THRUSTER3_PZ 0.0 +param set-default CA_THRUSTER3_CT 1.4 +param set-default CA_THRUSTER3_AX -1.0 +param set-default CA_THRUSTER3_AY 0.0 +param set-default CA_THRUSTER3_AZ 0.0 + +param set-default CA_THRUSTER4_PX 0.12 +param set-default CA_THRUSTER4_PY -0.12 +param set-default CA_THRUSTER4_PZ 0.0 +param set-default CA_THRUSTER4_CT 1.4 +param set-default CA_THRUSTER4_AX 0.0 +param set-default CA_THRUSTER4_AY 1.0 +param set-default CA_THRUSTER4_AZ 0.0 + +param set-default CA_THRUSTER5_PX 0.12 +param set-default CA_THRUSTER5_PY 0.12 +param set-default CA_THRUSTER5_PZ 0.0 +param set-default CA_THRUSTER5_CT 1.4 +param set-default CA_THRUSTER5_AX 0.0 +param set-default CA_THRUSTER5_AY -1.0 +param set-default CA_THRUSTER5_AZ 0.0 + +param set-default CA_THRUSTER6_PX -0.12 +param set-default CA_THRUSTER6_PY -0.12 +param set-default CA_THRUSTER6_PZ 0.0 +param set-default CA_THRUSTER6_CT 1.4 +param set-default CA_THRUSTER6_AX 0.0 +param set-default CA_THRUSTER6_AY 1.0 +param set-default CA_THRUSTER6_AZ 0.0 + +param set-default CA_THRUSTER7_PX -0.12 +param set-default CA_THRUSTER7_PY 0.12 +param set-default CA_THRUSTER7_PZ 0.0 +param set-default CA_THRUSTER7_CT 1.4 +param set-default CA_THRUSTER7_AX 0.0 +param set-default CA_THRUSTER7_AY -1.0 +param set-default CA_THRUSTER7_AZ 0.0 + + +param set-default PWM_AUX_TIM0 10 +param set-default PWM_AUX_TIM1 10 +param set-default PWM_AUX_TIM2 10 + +param set-default PWM_AUX_FUNC1 101 +param set-default PWM_AUX_FUNC2 102 +param set-default PWM_AUX_FUNC3 103 +param set-default PWM_AUX_FUNC4 104 +param set-default PWM_AUX_FUNC5 105 +param set-default PWM_AUX_FUNC6 106 +param set-default PWM_AUX_FUNC7 107 +param set-default PWM_AUX_FUNC8 108 + +param set-default PWM_AUX_DIS1 0 +param set-default PWM_AUX_DIS2 0 +param set-default PWM_AUX_DIS3 0 +param set-default PWM_AUX_DIS4 0 +param set-default PWM_AUX_DIS5 0 +param set-default PWM_AUX_DIS6 0 +param set-default PWM_AUX_DIS7 0 +param set-default PWM_AUX_DIS8 0 + +param set-default PWM_AUX_MIN1 0 +param set-default PWM_AUX_MIN2 0 +param set-default PWM_AUX_MIN3 0 +param set-default PWM_AUX_MIN4 0 +param set-default PWM_AUX_MIN5 0 +param set-default PWM_AUX_MIN6 0 +param set-default PWM_AUX_MIN7 0 +param set-default PWM_AUX_MIN8 0 + +# BOARD_PWM_FREQ is downscaled by 10, thus PWM value is given in 10s of usec +param set-default PWM_AUX_MAX1 10000 +param set-default PWM_AUX_MAX2 10000 +param set-default PWM_AUX_MAX3 10000 +param set-default PWM_AUX_MAX4 10000 +param set-default PWM_AUX_MAX5 10000 +param set-default PWM_AUX_MAX6 10000 +param set-default PWM_AUX_MAX7 10000 +param set-default PWM_AUX_MAX8 10000 + +# Controller Tunings +param set-default SC_ROLLRATE_P 0.14 +param set-default SC_PITCHRATE_P 0.14 +param set-default SC_ROLLRATE_I 0.3 +param set-default SC_PITCHRATE_I 0.3 +param set-default SC_ROLLRATE_D 0.004 +param set-default SC_PITCHRATE_D 0.004 diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_apps b/ROMFS/px4fmu_common/init.d/rc.sc_apps new file mode 100644 index 0000000000..2df68601ac --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_apps @@ -0,0 +1,36 @@ +#!/bin/sh +# +# Standard apps for sr. Attitude/Position estimator, Attitude/Position control. +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +# Start Spacecraft App +spacecraft start + +# Estimator Group Selection +# ekf2 start & + +# Start MicroDDS Client +# uxrce_dds_client start -t udp -h 192.168.0.1 -n spacebot2 +# uxrce_dds_client start -t udp -p 8888 + +# +# Start Control Allocator +# +# sc_control_allocator start + +# +# Start Spacecraft Rate Controller. +# +# sc_rate_control start + +# +# Start Spacecraft Attitude Controller. +# +# sc_att_control start + +# +# Start Spacecraft Position Controller. +# +# sc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.sc_defaults b/ROMFS/px4fmu_common/init.d/rc.sc_defaults new file mode 100644 index 0000000000..8dc8867b22 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sc_defaults @@ -0,0 +1,27 @@ +#!/bin/sh +# +# NOTE: Script variables are declared/initialized/unset in the rcS script. +# + +set VEHICLE_TYPE sc + +# MAV_TYPE_QUADROTOR 2 +#param set-default MAV_TYPE 12 + +# Set micro-dds-client to use ethernet and IP-address 192.168.0.1 +param set-default UXRCE_DDS_AG_IP -1062731775 + +# Disable preflight disarm to not interfere with external launching +param set-default COM_DISARM_PRFLT -1 +param set-default CBRK_SUPPLY_CHK 894281 +param set-default COM_ARM_HFLT_CHK 0 + +#Missing params +param set-default CP_DIST -1.0 + +# Default to MoCap fusion +param set-default ATT_EXT_HDG_M 2 +param set-default EKF2_EV_CTRL 15 +param set-default EKF2_EV_DELAY 5 +param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 3 diff --git a/Tools/kconfig/allyesconfig.py b/Tools/kconfig/allyesconfig.py index 04d8947781..2c0f58d75a 100644 --- a/Tools/kconfig/allyesconfig.py +++ b/Tools/kconfig/allyesconfig.py @@ -53,6 +53,7 @@ exception_list = [ 'SYSTEMCMDS_I2C_LAUNCHER', # undefined reference to `system', 'MODULES_MUORB_APPS', # Weird QURT/Posix package doesn't work on x86 px4 sitl 'MODULES_SIMULATION_SIMULATOR_SIH', # Causes compile errors + 'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code) ] exception_list_sitl = [ @@ -73,6 +74,7 @@ exception_list_sitl = [ 'SYSTEMCMDS_I2CDETECT', # Not supported in SITL 'SYSTEMCMDS_DMESG', # Not supported in SITL 'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL + 'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code) ] def main(): diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 230450cc81..db4af69088 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 230450cc817dd7675612ed5ec72ee59b6989d367 +Subproject commit db4af69088cccef4549cf3a5c195d5cd97d6b36a diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 5636d855e5..b0b7a8a2b3 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -80,3 +80,4 @@ CONFIG_EXAMPLES_HELLO=y CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y CONFIG_EXAMPLES_PX4_SIMPLE_APP=y CONFIG_EXAMPLES_WORK_ITEM=y +CONFIG_MODULES_SPACECRAFT=n diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board new file mode 100644 index 0000000000..a5c735f328 --- /dev/null +++ b/boards/px4/sitl/spacecraft.px4board @@ -0,0 +1,17 @@ +CONFIG_MODULES_AIRSPEED_SELECTOR=n +CONFIG_MODULES_FLIGHT_MODE_MANAGER=n +CONFIG_MODULES_FW_ATT_CONTROL=n +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_FW_POS_CONTROL=n +CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_MC_ATT_CONTROL=n +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n +CONFIG_MODULES_MC_POS_CONTROL=n +CONFIG_MODULES_MC_RATE_CONTROL=n +CONFIG_MODULES_VTOL_ATT_CONTROL=n +CONFIG_MODULES_DIFFERENTIAL_DRIVE=n +CONFIG_EKF2_AUX_GLOBAL_POSITION=y +CONFIG_MODULES_CONTROL_ALLOCATOR=n +CONFIG_MODULES_SPACECRAFT=y diff --git a/cmake/metadata.cmake b/cmake/metadata.cmake index ef946c7031..82b2042701 100644 --- a/cmake/metadata.cmake +++ b/cmake/metadata.cmake @@ -53,6 +53,7 @@ file(GLOB_RECURSE yaml_config_files ${PX4_SOURCE_DIR}/src/modules/*.yaml # avoid param duplicates list(FILTER yaml_config_files EXCLUDE REGEX ".*/pwm_out_sim/") list(FILTER yaml_config_files EXCLUDE REGEX ".*/linux_pwm_out/") +list(FILTER yaml_config_files EXCLUDE REGEX ".*/spacecraft/") add_custom_target(metadata_parameters COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/docs diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 0bc3f9c86e..99e280dd71 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -270,6 +270,14 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessHelicopterCoaxial(this); break; + case EffectivenessSource::SPACECRAFT_2D: + // spacecraft_allocation does allocation and publishes directly to actuator_motors topic + break; + + case EffectivenessSource::SPACECRAFT_3D: + // spacecraft_allocation does allocation and publishes directly to actuator_motors topic + break; + default: PX4_ERR("Unknown airframe"); break; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 303316f1ef..1f91f7d17d 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -158,6 +158,8 @@ private: HELICOPTER_TAIL_ESC = 10, HELICOPTER_TAIL_SERVO = 11, HELICOPTER_COAXIAL = 12, + SPACECRAFT_2D = 13, + SPACECRAFT_3D = 14, }; enum class FailureMode { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 32f2862c9d..b7fe1eb019 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -31,6 +31,8 @@ parameters: 11: Helicopter (tail Servo) 12: Helicopter (Coaxial) 13: Rover (Mecanum) + 14: Spacecraft 2D + 15: Spacecraft 3D default: 0 CA_METHOD: diff --git a/src/modules/spacecraft/CMakeLists.txt b/src/modules/spacecraft/CMakeLists.txt new file mode 100644 index 0000000000..88a18e9c01 --- /dev/null +++ b/src/modules/spacecraft/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# Copyright (c) 2019 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_directories(${CMAKE_CURRENT_SOURCE_DIR}) + +px4_add_module( + MODULE modules__spacecraft + MAIN spacecraft + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + STACK_MAIN + 3000 + SRCS + SpacecraftHandler.cpp + SpacecraftHandler.hpp + MODULE_CONFIG + module.yaml + DEPENDS + mathlib + px4_work_queue +) diff --git a/src/modules/spacecraft/Kconfig b/src/modules/spacecraft/Kconfig new file mode 100644 index 0000000000..67b77c3b7d --- /dev/null +++ b/src/modules/spacecraft/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_SPACECRAFT + bool "SPACECRAFT" + default n + ---help--- + Enable support for spacecraft + +menuconfig USER_SPACECRAFT + bool "spacecraft running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_SPACECRAFT + ---help--- + Put SPACECRAFT in userspace memory diff --git a/src/modules/spacecraft/SpacecraftHandler.cpp b/src/modules/spacecraft/SpacecraftHandler.cpp new file mode 100644 index 0000000000..92cdf86655 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftHandler.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 SpacecraftHandler.cpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#include "SpacecraftHandler.hpp" + +int SpacecraftHandler::task_spawn(int argc, char *argv[]) +{ + return 0; +} + +int SpacecraftHandler::print_status() +{ + PX4_INFO("Running"); + + return 0; +} + +int SpacecraftHandler::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SpacecraftHandler::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + This implements control allocation for spacecraft vehicles. + It takes torque and thrust setpoints as inputs and outputs + actuator setpoint messages. + )DESCR_STR" + ); + + PRINT_MODULE_USAGE_NAME("spacecraft", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Control Allocator app start / stop handling function + */ +extern "C" __EXPORT int spacecraft_main(int argc, char *argv[]); + +int spacecraft_main(int argc, char *argv[]) +{ + return SpacecraftHandler::main(argc, argv); +} diff --git a/src/modules/spacecraft/SpacecraftHandler.hpp b/src/modules/spacecraft/SpacecraftHandler.hpp new file mode 100644 index 0000000000..8fc954dc71 --- /dev/null +++ b/src/modules/spacecraft/SpacecraftHandler.hpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 ControlAllocator.hpp + * + * Control allocator. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SpacecraftHandler : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + SpacecraftHandler(); + + virtual ~SpacecraftHandler(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: /**< loop duration performance counter */ + +}; diff --git a/src/modules/spacecraft/module.yaml b/src/modules/spacecraft/module.yaml new file mode 100644 index 0000000000..9dc0d1620b --- /dev/null +++ b/src/modules/spacecraft/module.yaml @@ -0,0 +1,235 @@ +__max_num_mc_motors: &max_num_mc_motors 12 +__max_num_thrusters: &max_num_thrusters 12 +__max_num_servos: &max_num_servos 8 +__max_num_tilts: &max_num_tilts 4 + +module_name: Control Allocation + +parameters: + - group: Geometry + definitions: + CA_AIRFRAME: + description: + short: Airframe selection + long: | + Defines which mixer implementation to use. + Some are generic, while others are specifically fit to a certain vehicle with a fixed set of actuators. + + 'Custom' should only be used if noting else can be used. + type: enum + values: + 0: Multirotor + 1: Fixed-wing + 2: Standard VTOL + 3: Tiltrotor VTOL + 4: Tailsitter VTOL + 5: Rover (Ackermann) + 6: Rover (Differential) + 7: Motors (6DOF) + 8: Multirotor with Tilt + 9: Custom + 10: Helicopter (tail ESC) + 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) + 13: Rover (Mecanum) + 14: Spacecraft 2D + 15: Spacecraft 3D + default: 14 + + CA_METHOD: + description: + short: Control allocation method + long: | + Selects the algorithm and desaturation method. + If set to Automtic, the selection is based on the airframe (CA_AIRFRAME). + type: enum + values: + 0: Pseudo-inverse with output clipping + 1: Pseudo-inverse with metric allocation + 2: Automatic + default: 1 + + CA_R_REV: + description: + short: Bidirectional/Reversible motors + long: | + Configure motors to be bidirectional/reversible. Note that the output driver needs to support this as well. + type: bitmask + bit: + 0: Motor 1 + 1: Motor 2 + 2: Motor 3 + 3: Motor 4 + 4: Motor 5 + 5: Motor 6 + 6: Motor 7 + 7: Motor 8 + 8: Motor 9 + 9: Motor 10 + 10: Motor 11 + 11: Motor 12 + default: 0 + + # (SC) Thrusters + CA_THRUSTER_CNT: + description: + short: Total number of thrusters + type: enum + values: + 0: '0' + 1: '1' + 2: '2' + 3: '3' + 4: '4' + 5: '5' + 6: '6' + 7: '7' + 8: '8' + 9: '9' + 10: '10' + 11: '11' + 12: '12' + default: 0 + CA_THRUSTER${i}_PX: + description: + short: Position of thruster ${i} along X body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PY: + description: + short: Position of thruster ${i} along Y body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_PZ: + description: + short: Position of thruster ${i} along Z body axis + type: float + decimal: 2 + increment: 0.1 + unit: m + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AX: + description: + short: Axis of thruster ${i} thrust vector, X body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AY: + description: + short: Axis of thruster ${i} thrust vector, Y body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: 0.0 + CA_THRUSTER${i}_AZ: + description: + short: Axis of thruster ${i} thrust vector, Z body axis component + long: Only the direction is considered (the vector is normalized). + type: float + decimal: 2 + increment: 0.1 + num_instances: *max_num_thrusters + min: -100 + max: 100 + default: -1.0 + + CA_THRUSTER${i}_CT: + description: + short: Thrust coefficient of rotor ${i} + long: | + The thrust coefficient if defined as Thrust = CT * u^2, + where u (with value between actuator minimum and maximum) + is the output signal sent to the motor controller. + type: float + decimal: 1 + increment: 1 + num_instances: *max_num_thrusters + min: 0 + max: 100 + default: 6.5 + +# Mixer +mixer: + actuator_types: + motor: + functions: 'Motor' + actuator_testing_values: + min: 0 + max: 1 + default_is_nan: true + DEFAULT: + actuator_testing_values: + min: -1 + max: 1 + default: -1 + + config: + param: CA_AIRFRAME + types: + 14: # Spacecraft 2D + title: 'Spacecraft 2D' + actuators: + - actuator_type: 'motor' + count: 'CA_THRUSTER_CNT' # count would be too long for 16 max size + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true + + 15: # Sapcecraft 3D + title: 'Spacecraft 3D' + actuators: + - actuator_type: 'motor' + count: 'CA_THRUSTER_CNT' + per_item_parameters: + standard: + position: [ 'CA_THRUSTER${i}_PX', 'CA_THRUSTER${i}_PY', 'CA_THRUSTER${i}_PZ' ] + extra: + - name: 'CA_THRUSTER${i}_AX' + label: 'Axis X' + function: 'axisx' + advanced: true + - name: 'CA_THRUSTER${i}_AY' + label: 'Axis Y' + function: 'axisy' + advanced: true + - name: 'CA_THRUSTER${i}_AZ' + label: 'Axis Z' + function: 'axisz' + advanced: true From 093b379b6b4db6b032ad887d676ed027c5607cf2 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 7 Feb 2025 10:59:25 +0100 Subject: [PATCH 09/63] sf45/collision-prevention: replace repeated code with ObstacleMath library functions. --- .../lightware_sf45_serial.cpp | 18 +-- .../lightware_sf45_serial.hpp | 6 - .../CollisionPrevention.cpp | 109 ++++-------------- .../CollisionPrevention.hpp | 9 -- src/lib/collision_prevention/ObstacleMath.cpp | 20 +++- src/lib/collision_prevention/ObstacleMath.hpp | 18 ++- .../collision_prevention/ObstacleMathTest.cpp | 16 +++ 7 files changed, 78 insertions(+), 118 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 7f16fa5d26..302f5f79cf 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 @@ -620,7 +620,8 @@ void SF45LaserSerial::sf45_process_replies() 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); + // Find bin index for the current sensor yaw angle (in sensor frame) + const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame); if (current_bin != _previous_bin) { PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin, @@ -702,21 +703,6 @@ 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; - float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset); - mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment); - - return mapped_sector; -} - -float SF45LaserSerial::sf45_wrap_360(float f) -{ - return matrix::wrap(f, 0.f, 360.f); -} - uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val) { uint32_t i; 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 d308a474d4..313b9e60f7 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 @@ -74,10 +74,6 @@ enum SF45_PARSED_STATE { }; enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum - 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 ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90 ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270 }; @@ -95,8 +91,6 @@ public: 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(); - uint8_t sf45_convert_angle(const int16_t yaw); - float sf45_wrap_360(float f); private: obstacle_distance_s _obstacle_distance{}; diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 3f1344ffdd..7e04629915 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -236,14 +236,14 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) { - float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - - (float)_obstacle_map_body_frame.increment / 2.f); - float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - + (float)_obstacle_map_body_frame.increment / 2.f); - float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg - - obstacle.increment / 2.f); - float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg + - obstacle.increment / 2.f); + float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, + obstacle.angle_offset - vehicle_orientation_deg); + float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, + obstacle.angle_offset - vehicle_orientation_deg); // if a bin stretches over the 0/360 degree line, adjust the angles if (bin_lower_angle > bin_upper_angle) { @@ -277,12 +277,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // corresponding data index (shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { for (int j = 0; j < 360 / obstacle.increment; j++) { - float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - - (float)_obstacle_map_body_frame.increment / 2.f); - float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - + (float)_obstacle_map_body_frame.increment / 2.f); - float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f); - float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f); + float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset); + float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset); // if a bin stretches over the 0/360 degree line, adjust the angles if (bin_lower_angle > bin_upper_angle) { @@ -373,7 +373,7 @@ void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw; - const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - + const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps @@ -394,7 +394,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // discard values below min range if (distance_reading > distance_sensor.min_distance) { - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); + float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast + (distance_sensor.orientation), distance_sensor.q); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices @@ -408,7 +409,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrapped_bin = _wrap_bin(bin); + int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); @@ -435,7 +436,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float mean_dist = 0; for (int j = i - filter_size; j <= i + filter_size; j++) { - int bin = _wrap_bin(j); + int bin = ObstacleMath::wrap_bin(j, BIN_COUNT); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { mean_dist += _param_cp_dist.get() * 100.f; @@ -445,7 +446,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi } } - const int bin = _wrap_bin(i); + const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT); mean_dist = mean_dist / (2.f * filter_size + 1.f); const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original); const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin]; @@ -465,52 +466,6 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi } } -float -CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const -{ - float offset = math::max(math::radians(angle_offset), 0.f); - - switch (distance_sensor.orientation) { - case distance_sensor_s::ROTATION_YAW_0: - offset = 0.0f; - break; - - case distance_sensor_s::ROTATION_YAW_45: - offset = M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_90: - offset = M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_YAW_135: - offset = 3.0f * M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_180: - offset = M_PI_F; - break; - - case distance_sensor_s::ROTATION_YAW_225: - offset = -3.0f * M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_270: - offset = -M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_YAW_315: - offset = -M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_CUSTOM: - offset = Eulerf(Quatf(distance_sensor.q)).psi(); - break; - } - - return offset; -} - float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { float obstacle_distance = 0.f; @@ -520,10 +475,10 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) Vector2f dir = direction / direction_norm; const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = - _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); - int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); - dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1); - obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; + ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); + + const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg); + obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; } return obstacle_distance; @@ -632,19 +587,3 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter() command.timestamp = getTime(); _vehicle_command_pub.publish(command); } - -float CollisionPrevention::_wrap_360(const float f) -{ - return wrap(f, 0.f, 360.f); -} - -int CollisionPrevention::_wrap_bin(int i) -{ - i = i % BIN_COUNT; - - while (i < 0) { - i += BIN_COUNT; - } - - return i; -} diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 7d9d16cd7f..76bfcb5ea7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -204,13 +204,6 @@ private: (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) - /** - * Transforms the sensor orientation into a yaw in the local frame - * @param distance_sensor, distance sensor message - * @param angle_offset, sensor body frame offset - */ - float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; - /** * Computes collision free setpoints * @param setpoint, setpoint before collision prevention intervention @@ -238,6 +231,4 @@ private: */ void _publishVehicleCmdDoLoiter(); - static float _wrap_360(const float f); - static int _wrap_bin(int i); }; diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp index 0605b664ec..e75f19d7b7 100644 --- a/src/lib/collision_prevention/ObstacleMath.cpp +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -57,13 +57,19 @@ int get_bin_at_angle(float bin_width, float angle) return wrap_bin(bin_at_angle, 360 / bin_width); } +float get_lower_bound_angle(int bin, float bin_width, float angle_offset) +{ + bin = wrap_bin(bin, 360 / bin_width); + return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f); +} + 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 sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4]) { float offset = 0.0f; @@ -99,6 +105,13 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation) case SensorOrientation::ROTATION_YAW_315: offset = -M_PI_F / 4.0f; break; + + case SensorOrientation::ROTATION_CUSTOM: + if (q != nullptr) { + offset = Eulerf(Quatf(q)).psi(); + } + + break; } return offset; @@ -109,4 +122,9 @@ int wrap_bin(int bin, int bin_count) return (bin + bin_count) % bin_count; } +float wrap_360(const float angle) +{ + return matrix::wrap(angle, 0.0f, 360.0f); +} + } // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp index f5317aeaa7..8a1db0217a 100644 --- a/src/lib/collision_prevention/ObstacleMath.hpp +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -45,6 +45,7 @@ enum SensorOrientation { 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_CUSTOM = 100, // MAV_SENSOR_ROTATION_CUSTOM ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 @@ -56,7 +57,7 @@ enum SensorOrientation { * Converts a sensor orientation to a yaw offset * @param orientation sensor orientation */ -float sensor_orientation_to_yaw_offset(const SensorOrientation orientation); +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr); /** * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane @@ -73,6 +74,14 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons */ int get_bin_at_angle(float bin_width, float angle); +/** + * Returns lower bound angle of a bin + * @param bin bin index + * @param bin_width width of a bin in degrees + * @param angle_offset clockwise angle offset in degrees + */ +float get_lower_bound_angle(int bin, float bin_width, float angle_offset); + /** * Returns bin index for the current bin after an angle offset * @param bin current bin index @@ -88,4 +97,11 @@ int get_offset_bin_index(int bin, float bin_width, float angle_offset); */ int wrap_bin(int bin, int bin_count); +/** + * Wraps an angle to the range [0, 360) + * @param angle angle in degrees + */ +float wrap_360(const float angle); + + } // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp index cea54aa4a2..9f6b00ae21 100644 --- a/src/lib/collision_prevention/ObstacleMathTest.cpp +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -133,6 +133,22 @@ TEST(ObstacleMathTest, GetBinAtAngle) EXPECT_EQ(bin_index, 18); } +TEST(ObstacleMathTest, GetLowerBound) +{ + // GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset + int bin = -1; + float bin_width = 7.5f; + float angle_offset = -4.3f; + + // WHEN: we calculate the lower bound angle of the bin + float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset); + + // THEN: the lower bound angle should be correct. The bin index is wrapped to the end and + // the angle offset is applied in the counter-clockwise direction. + EXPECT_FLOAT_EQ(lower_bound, 344.45); + +} + TEST(ObstacleMathTest, OffsetBinIndex) { From 69d95a66645572bc192a0c81aa8dc39d704b83d2 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Fri, 7 Feb 2025 11:00:15 +0100 Subject: [PATCH 10/63] sf45: separate sensor yaw variable into FRD and sensor frames for clarity. Obstacle map is created in sensor frame, but scaling for vehicle orientation is done in vehicle FRD frame. --- .../lightware_sf45_serial.cpp | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 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 302f5f79cf..587348ca7d 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 @@ -595,36 +595,37 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8 void SF45LaserSerial::sf45_process_replies() { + const int16_t YAW_THRESHOLD = 32000; + const int16_t YAW_ADJUSTMENT = 65535; + switch (rx_field.msg_id) { 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)); // The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float - if (raw_yaw > 32000) { - raw_yaw = raw_yaw - 65535; + if (raw_yaw > YAW_THRESHOLD) { + raw_yaw -= YAW_ADJUSTMENT; } - // The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle + // Adjust yaw for downward facing sensor if (_orient_cfg == ROTATION_DOWNWARD_FACING) { - raw_yaw = raw_yaw * -1; + raw_yaw = -raw_yaw; } // SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}" - float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; + float scaled_yaw_sensor_frame = raw_yaw * SF45_SCALE_FACTOR; + float scaled_yaw_frd = ObstacleMath::wrap_360(scaled_yaw_sensor_frame + _obstacle_distance.angle_offset); + float distance_m = raw_distance * SF45_SCALE_FACTOR; - // 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; + // Update the current bin distance _current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist; // Find bin index for the current sensor yaw angle (in sensor frame) const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame); if (current_bin != _previous_bin) { - PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin, + PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw_frd, current_bin, (double)distance_m); if (_vehicle_attitude_sub.updated()) { @@ -635,9 +636,10 @@ void SF45LaserSerial::sf45_process_replies() } } + // Scale distance with vehicle rotation 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); + float scaled_yaw_frd_rad = math::radians(static_cast(scaled_yaw_frd)); + ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_frd_rad, _vehicle_attitude); _current_bin_dist = static_cast(current_bin_dist); if (_current_bin_dist > _obstacle_distance.max_distance) { From ce64263ce719ceddd9e1d6fad4657b4ea5cdf7eb Mon Sep 17 00:00:00 2001 From: Bertug Dilman <92028947+bdilman@users.noreply.github.com> Date: Fri, 7 Feb 2025 14:44:48 +0100 Subject: [PATCH 11/63] publish validated airspeed topic (#24302) * publish validated airspeed topic * fix typo --- src/modules/uxrce_dds_client/dds_topics.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index acee6afe8d..211e75fd05 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -74,6 +74,9 @@ publications: - topic: /fmu/out/vehicle_trajectory_waypoint_desired type: px4_msgs::msg::VehicleTrajectoryWaypoint + - topic: /fmu/out/airspeed_validated + type: px4_msgs::msg::AirspeedValidated + # Create uORB::Publication subscriptions: - topic: /fmu/in/register_ext_component_request From 55f51d7e7e7a525ce0a0c990ccabbcd926f1a81e Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Fri, 20 Dec 2024 15:58:58 +0100 Subject: [PATCH 12/63] ackermann: refactor code architecture --- .../airframes/4012_gz_rover_ackermann | 36 +- .../51001_axial_scx10_2_trail_honcho | 37 +- boards/px4/sitl/spacecraft.px4board | 4 +- msg/CMakeLists.txt | 10 +- msg/RoverAckermannGuidanceStatus.msg | 6 - msg/RoverAckermannSetpoint.msg | 9 - msg/RoverAckermannStatus.msg | 11 - msg/RoverAttitudeSetpoint.msg | 5 + msg/RoverAttitudeStatus.msg | 6 + msg/RoverRateSetpoint.msg | 5 + msg/RoverRateStatus.msg | 7 + msg/RoverSteeringSetpoint.msg | 6 + msg/RoverThrottleSetpoint.msg | 7 + msg/RoverVelocityStatus.msg | 12 + src/lib/CMakeLists.txt | 1 + .../rover_control}/CMakeLists.txt | 9 +- src/lib/rover_control/RoverControl.cpp | 214 +++++++++ src/lib/rover_control/RoverControl.hpp | 118 +++++ src/lib/rover_control/RoverControlTest.cpp | 115 +++++ src/lib/rover_control/module.yaml | 191 ++++++++ src/modules/logger/logged_topics.cpp | 10 +- .../AckermannAttControl.cpp | 205 +++++++++ .../AckermannAttControl.hpp | 140 ++++++ .../CMakeLists.txt | 10 +- .../AckermannPosVelControl.cpp | 418 ++++++++++++++++++ .../AckermannPosVelControl.hpp | 257 +++++++++++ .../AckermannPosVelControl/CMakeLists.txt | 39 ++ .../AckermannRateControl.cpp | 223 ++++++++++ .../AckermannRateControl.hpp | 139 ++++++ .../AckermannRateControl/CMakeLists.txt | 39 ++ src/modules/rover_ackermann/CMakeLists.txt | 17 +- src/modules/rover_ackermann/Kconfig | 3 +- .../rover_ackermann/RoverAckermann.cpp | 250 ++++------- .../rover_ackermann/RoverAckermann.hpp | 110 ++--- .../RoverAckermannControl.cpp | 228 ---------- .../RoverAckermannControl.hpp | 137 ------ .../RoverAckermannGuidance.cpp | 309 ------------- .../RoverAckermannGuidance.hpp | 215 --------- src/modules/rover_ackermann/module.yaml | 163 +------ .../rover_pos_control_params.c | 34 +- 40 files changed, 2418 insertions(+), 1337 deletions(-) delete mode 100644 msg/RoverAckermannGuidanceStatus.msg delete mode 100644 msg/RoverAckermannSetpoint.msg delete mode 100644 msg/RoverAckermannStatus.msg create mode 100644 msg/RoverAttitudeSetpoint.msg create mode 100644 msg/RoverAttitudeStatus.msg create mode 100644 msg/RoverRateSetpoint.msg create mode 100644 msg/RoverRateStatus.msg create mode 100644 msg/RoverSteeringSetpoint.msg create mode 100644 msg/RoverThrottleSetpoint.msg create mode 100644 msg/RoverVelocityStatus.msg rename src/{modules/rover_ackermann/RoverAckermannGuidance => lib/rover_control}/CMakeLists.txt (88%) create mode 100644 src/lib/rover_control/RoverControl.cpp create mode 100644 src/lib/rover_control/RoverControl.hpp create mode 100644 src/lib/rover_control/RoverControlTest.cpp create mode 100644 src/lib/rover_control/module.yaml create mode 100644 src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp rename src/modules/rover_ackermann/{RoverAckermannControl => AckermannAttControl}/CMakeLists.txt (85%) create mode 100644 src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp create mode 100644 src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt create mode 100644 src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp create mode 100644 src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp create mode 100644 src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt delete mode 100644 src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp delete mode 100644 src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp delete mode 100644 src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp delete mode 100644 src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index e058b87dc4..cd870caabb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -11,23 +11,33 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_LAT_ACCEL_I 0.01 -param set-default RA_LAT_ACCEL_P 0.1 -param set-default RA_MAX_ACCEL 3 -param set-default RA_MAX_DECEL 6 -param set-default RA_MAX_JERK 15 -param set-default RA_MAX_LAT_ACCEL 4 -param set-default RA_MAX_SPEED 3 param set-default RA_MAX_STR_ANG 0.5236 -param set-default RA_MAX_STR_RATE 360 -param set-default RA_MAX_THR_SPEED 3.1 -param set-default RA_SPEED_I 0.01 -param set-default RA_SPEED_P 0.1 -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_STR_RATE_LIM 360 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 6 +param set-default RO_JERK_LIM 15 +param set-default RO_MAX_THR_SPEED 3.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.1 +param set-default RO_YAW_RATE_P 1 +param set-default RO_YAW_RATE_LIM 180 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 3 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 3 +param set-default RO_SPEED_I 0.1 +param set-default RO_SPEED_P 1 # Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho index 26363414ed..6bd7bc3e2b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -14,24 +14,33 @@ . ${R}etc/init.d/rc.rover_ackermann_defaults param set-default BAT1_N_CELLS 3 - -# Rover parameters param set-default NAV_ACC_RAD 0.5 + +# Ackermann Parameters +param set-default RA_WHEEL_BASE 0.321 param set-default RA_ACC_RAD_GAIN 2 param set-default RA_ACC_RAD_MAX 3 -param set-default RA_LAT_ACCEL_I 0.01 -param set-default RA_LAT_ACCEL_P 0.1 -param set-default RA_MAX_ACCEL 1.5 -param set-default RA_MAX_DECEL 10 -param set-default RA_MAX_JERK 20 -param set-default RA_MAX_LAT_ACCEL 3 -param set-default RA_MAX_SPEED 2.5 param set-default RA_MAX_STR_ANG 0.5236 -param set-default RA_MAX_STR_RATE 270 -param set-default RA_MAX_THR_SPEED 2.8 -param set-default RA_SPEED_I 0.01 -param set-default RA_SPEED_P 0.1 -param set-default RA_WHEEL_BASE 0.321 +param set-default RA_STR_RATE_LIM 270 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 1.5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 20 +param set-default RO_MAX_THR_SPEED 2.8 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0 +param set-default RO_YAW_RATE_P 0 +param set-default RO_YAW_RATE_LIM 0 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 0 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2.5 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 # Pure pursuit parameters param set-default PP_LOOKAHD_GAIN 1 diff --git a/boards/px4/sitl/spacecraft.px4board b/boards/px4/sitl/spacecraft.px4board index a5c735f328..56c966598b 100644 --- a/boards/px4/sitl/spacecraft.px4board +++ b/boards/px4/sitl/spacecraft.px4board @@ -11,7 +11,9 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n -CONFIG_MODULES_DIFFERENTIAL_DRIVE=n +CONFIG_MODULES_ROVER_ACKERMANN=n +CONFIG_MODULES_ROVER_DIFFERENTIAL=n +CONFIG_MODULES_ROVER_MECANUM=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_CONTROL_ALLOCATOR=n CONFIG_MODULES_SPACECRAFT=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7abae77293..51c9325413 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -171,9 +171,13 @@ set(msg_files RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg - RoverAckermannGuidanceStatus.msg - RoverAckermannSetpoint.msg - RoverAckermannStatus.msg + RoverAttitudeSetpoint.msg + RoverAttitudeStatus.msg + RoverVelocityStatus.msg + RoverRateSetpoint.msg + RoverRateStatus.msg + RoverSteeringSetpoint.msg + RoverThrottleSetpoint.msg RoverDifferentialGuidanceStatus.msg RoverDifferentialSetpoint.msg RoverDifferentialStatus.msg diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg deleted file mode 100644 index bc096ba4d6..0000000000 --- a/msg/RoverAckermannGuidanceStatus.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [deg] Heading error of the pure pursuit controller - -# TOPICS rover_ackermann_guidance_status diff --git a/msg/RoverAckermannSetpoint.msg b/msg/RoverAckermannSetpoint.msg deleted file mode 100644 index f504572774..0000000000 --- a/msg/RoverAckermannSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired speed in body x direction. Positiv: forwards, Negativ: backwards -float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized speed in body x direction. Positiv: forwards, Negativ: backwards -float32 steering_setpoint # [rad] Desired steering angle -float32 steering_setpoint_normalized # [-1, 1] Desired normalized steering angle -float32 lateral_acceleration_setpoint # [m/s^2] Desired acceleration in body y direction. Positiv: right, Negativ: left. - -# TOPICS rover_ackermann_setpoint diff --git a/msg/RoverAckermannStatus.msg b/msg/RoverAckermannStatus.msg deleted file mode 100644 index b97f40e438..0000000000 --- a/msg/RoverAckermannStatus.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 steering_setpoint_normalized # [-1, 1] Normalized steering setpoint -float32 adjusted_steering_setpoint_normalized # [-1, 1] Normalized steering setpoint after applying slew rate -float32 measured_lateral_acceleration # [m/s^2] Measured acceleration in body y direction. Positiv: right, Negativ: left. -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller -float32 pid_lat_accel_integral # Integral of the PID for the closed loop lateral acceleration controller - -# TOPICS rover_ackermann_status diff --git a/msg/RoverAttitudeSetpoint.msg b/msg/RoverAttitudeSetpoint.msg new file mode 100644 index 0000000000..0ea9a7ea17 --- /dev/null +++ b/msg/RoverAttitudeSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 yaw_setpoint # [rad] Expressed in NED frame + +# TOPICS rover_attitude_setpoint diff --git a/msg/RoverAttitudeStatus.msg b/msg/RoverAttitudeStatus.msg new file mode 100644 index 0000000000..197d31c2a7 --- /dev/null +++ b/msg/RoverAttitudeStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_yaw # [rad/s] Measured yaw rate +float32 adjusted_yaw_setpoint # [rad/s] Yaw setpoint that is being tracked (Applied slew rates) + +# TOPICS rover_attitude_status diff --git a/msg/RoverRateSetpoint.msg b/msg/RoverRateSetpoint.msg new file mode 100644 index 0000000000..88054f9dd6 --- /dev/null +++ b/msg/RoverRateSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 yaw_rate_setpoint # [rad/s] Expressed in NED frame + +# TOPICS rover_rate_setpoint diff --git a/msg/RoverRateStatus.msg b/msg/RoverRateStatus.msg new file mode 100644 index 0000000000..4e82eb30fa --- /dev/null +++ b/msg/RoverRateStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint that is being tracked (Applied slew rates) +float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller + +# TOPICS rover_rate_status diff --git a/msg/RoverSteeringSetpoint.msg b/msg/RoverSteeringSetpoint.msg new file mode 100644 index 0000000000..252215ee8a --- /dev/null +++ b/msg/RoverSteeringSetpoint.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Only for Ackermann-steered rovers) +float32 normalized_speed_diff # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Only for Differential/Mecanum rovers) + +# TOPICS rover_steering_setpoint diff --git a/msg/RoverThrottleSetpoint.msg b/msg/RoverThrottleSetpoint.msg new file mode 100644 index 0000000000..3b6d7047b3 --- /dev/null +++ b/msg/RoverThrottleSetpoint.msg @@ -0,0 +1,7 @@ + +uint64 timestamp # time since system start (microseconds) + +float32 throttle_body_x # throttle setpoint along body X axis [-1, 1] +float32 throttle_body_y # throttle setpoint along body Y axis [-1, 1] + +# TOPICS rover_throttle_setpoint diff --git a/msg/RoverVelocityStatus.msg b/msg/RoverVelocityStatus.msg new file mode 100644 index 0000000000..f16de0f6b7 --- /dev/null +++ b/msg/RoverVelocityStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 measured_speed_body_x # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards +float32 speed_body_x_setpoint # [m/s] Speed setpoint in body x direction. Positiv: forwards, Negativ: backwards +float32 adjusted_speed_body_x_setpoint # [m/s] Post slew rate speed setpoint in body x direction. Positiv: forwards, Negativ: backwards +float32 measured_speed_body_y # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left +float32 speed_body_y_setpoint # [m/s] Speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) +float32 adjusted_speed_body_y_setpoint # [m/s] Post slew rate speed setpoint in body y direction. Positiv: right, Negativ: left (Only for mecanum rovers) +float32 pid_throttle_body_x_integral # Integral of the PID for the closed loop controller of the speed in body x direction +float32 pid_throttle_body_y_integral # Integral of the PID for the closed loop controller of the speed in body y direction + +# TOPICS rover_velocity_status diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8f58109b89..1075ad4cd6 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -70,6 +70,7 @@ add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) +add_subdirectory(rover_control EXCLUDE_FROM_ALL) add_subdirectory(rtl EXCLUDE_FROM_ALL) add_subdirectory(sensor_calibration EXCLUDE_FROM_ALL) add_subdirectory(slew_rate EXCLUDE_FROM_ALL) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt b/src/lib/rover_control/CMakeLists.txt similarity index 88% rename from src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt rename to src/lib/rover_control/CMakeLists.txt index 373fc9b1be..f3a9277d29 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt +++ b/src/lib/rover_control/CMakeLists.txt @@ -31,8 +31,11 @@ # ############################################################################ -px4_add_library(RoverAckermannGuidance - RoverAckermannGuidance.cpp +px4_add_library(rover_control + RoverControl.cpp + RoverControl.hpp ) -target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(rover_control PUBLIC PID) +target_link_libraries(rover_control PUBLIC geo) +px4_add_unit_gtest(SRC RoverControlTest.cpp LINKLIBS rover_control) diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp new file mode 100644 index 0000000000..7b0864a428 --- /dev/null +++ b/src/lib/rover_control/RoverControl.cpp @@ -0,0 +1,214 @@ +/**************************************************************************** + * + * 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 "RoverControl.hpp" +using namespace matrix; +namespace RoverControl +{ +float throttleControl(SlewRate &motor_setpoint, const float throttle_setpoint, + const float current_motor_setpoint, const float max_accel, const float max_decel, const float max_thr_spd, + const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(throttle_setpoint) || !PX4_ISFINITE(current_motor_setpoint) || !PX4_ISFINITE(dt)) { + return NAN; + } + + const bool accelerating = fabsf(throttle_setpoint) > fabsf(current_motor_setpoint); + + if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate + motor_setpoint.setSlewRate(max_accel / max_thr_spd); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - + current_motor_setpoint)) { + motor_setpoint.setForcedValue(current_motor_setpoint); + } + + motor_setpoint.update(throttle_setpoint, dt); + + } else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate + motor_setpoint.setSlewRate(max_decel / max_thr_spd); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - + current_motor_setpoint)) { + motor_setpoint.setForcedValue(current_motor_setpoint); + } + + motor_setpoint.update(throttle_setpoint, dt); + + } else { // Fallthrough if slew rate parameters are not configured + motor_setpoint.setForcedValue(throttle_setpoint); + } + + return motor_setpoint.getState(); +} + +float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, + const float yaw_slew_rate, float vehicle_yaw, float yaw_setpoint, const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(yaw_setpoint) || !PX4_ISFINITE(vehicle_yaw) || !PX4_ISFINITE(dt)) { + return NAN; + } + + yaw_setpoint = wrap_pi(yaw_setpoint); + vehicle_yaw = wrap_pi(vehicle_yaw); + + if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured + adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) { + adjusted_yaw_setpoint.setForcedValue(vehicle_yaw); + } + + adjusted_yaw_setpoint.update(yaw_setpoint, dt); + + } else { + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); + } + + // Feedback control + pid_yaw.setSetpoint( + wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)); // Error as setpoint to take care of wrapping + return pid_yaw.update(0.f, dt); + +} + +float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const float speed_setpoint, + const float vehicle_speed, const float max_accel, const float max_decel, const float max_thr_speed, const float dt) +{ + // Sanitize inputs + if (!PX4_ISFINITE(speed_setpoint) || !PX4_ISFINITE(vehicle_speed) || !PX4_ISFINITE(dt)) { + return NAN; + } + + // Apply acceleration and deceleration limit + if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { + speed_with_rate_limit.setSlewRate(max_accel); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( + speed_setpoint - vehicle_speed)) { + speed_with_rate_limit.setForcedValue(vehicle_speed); + } + + speed_with_rate_limit.update(speed_setpoint, dt); + + } else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) { + speed_with_rate_limit.setSlewRate(max_decel); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( + speed_setpoint - vehicle_speed)) { + speed_with_rate_limit.setForcedValue(vehicle_speed); + } + + speed_with_rate_limit.update(speed_setpoint, dt); + + } else { // Fallthrough if slew rate is not configured + speed_with_rate_limit.setForcedValue(speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + // Feedforward + if (max_thr_speed > FLT_EPSILON) { + forward_speed_normalized = math::interpolate(speed_with_rate_limit.getState(), + -max_thr_speed, max_thr_speed, + -1.f, 1.f); + } + + // Feedback control + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + return math::constrain(forward_speed_normalized, -1.f, 1.f); +} + +void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + MapProjection &global_ned_proj_ref) +{ + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + curr_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + curr_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, NAN); // Fallback if current waypoint is invalid + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + prev_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.previous.lat, + position_setpoint_triplet.previous.lon); + + } else { + prev_wp_ned = curr_pos_ned.isAllFinite() ? curr_pos_ned : Vector2f(NAN, + NAN); // Fallback if previous waypoint is invalid + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN, + NAN); // Enables corner slow down with RTL + } +} + +float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned) +{ + // Sanitize inputs + if (!prev_wp_ned.isAllFinite() || !curr_wp_ned.isAllFinite() || !next_wp_ned.isAllFinite()) { + return NAN; + } + + const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned; + const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned; + + // Waypoint overlap + if (curr_to_next_wp_ned.norm() < FLT_EPSILON || curr_to_prev_wp_ned.norm() < FLT_EPSILON) { + return NAN; + } + + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + return acosf(cosin); +} + +} // RoverControl diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp new file mode 100644 index 0000000000..7f76710225 --- /dev/null +++ b/src/lib/rover_control/RoverControl.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file RoverControl.hpp + * + * Functions that are shared among the different rover modules. + * Also includes the parameters that are shared among the rover modules. + */ + +#pragma once +#include +#include +#include +#include +#include +#include + +using namespace matrix; +namespace RoverControl +{ +/** + * Applies acceleration/deceleration slew rate to a throttle setpoint. + * @param motor_setpoint Throttle setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param throttle_setpoint Throttle setpoint pre slew rate [-1, 1] + * @param current_motor_setpoint Currently applied motor input [-1, 1] + * @param max_accel Maximum allowed acceleration [m/s^2] + * @param max_decel Maximum allowed deceleration [m/s^2] + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s] + * @param dt Time since last update [s] + * @return Motor Setpoint [-1, 1] + */ +float throttleControl(SlewRate &motor_setpoint, float throttle_setpoint, float current_motor_setpoint, + float max_accel, float max_decel, float max_thr_spd, float dt); + +/** + * Applies yaw rate slew rate to a yaw setpoint and calculates the necessary yaw rate setpoint + * using a PID controller. + * @param adjusted_yaw_setpoint Yaw setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param pid_yaw Yaw PID (Updated by this function) + * @param yaw_slew_rate Yaw slew rate [rad/s] + * @param vehicle_yaw Measured vehicle yaw [rad] + * @param yaw_setpoint Yaw setpoint [rad] + * @param dt Time since last update [s] + * @return Yaw rate setpoint [rad/s] + */ +float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, float yaw_slew_rate, + float vehicle_yaw, float yaw_setpoint, float dt); +/** + * Applies acceleration/deceleration slew rate to a speed setpoint and calculates the necessary throttle setpoint + * using a feed forward term and PID controller. + * @param speed_with_rate_limit Speed setpoint with applied slew rate [-1, 1] (Updated by this function) + * @param pid_speed Speed PID (Updated by this function) + * @param speed_setpoint Speed setpoint [m/s] + * @param vehicle_speed Measured vehicle speed [m/s] + * @param max_accel Maximum allowed acceleration [m/s^2] + * @param max_decel Maximum allowed deceleration [m/s^2] + * @param max_thr_speed Speed at maximum throttle [m/s] + * @param dt Time since last update [s] + * @return Throttle setpoint [-1, 1] + */ +float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float speed_setpoint, + float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt); + +/** + * Projects positionSetpointTriplet waypoints from global to ned frame. + * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) + * @param prev_wp_ned Previous waypoint in NED frame (Updated by this function) + * @param next_wp_ned Next waypoint in NED frame (Updated by this function) + * @param position_setpoint_triplet Position Setpoint Triplet + * @param curr_pos Current position of the rover in global frame + * @param home_pos Home position in global frame + * @param global_ned_proj_ref Global to ned projection + */ +void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, + position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, + MapProjection &global_ned_proj_ref); + +/** + * Calculate and return the angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_wp_ned Previous waypoint in NED frame + * @param curr_wp_ned Current waypoint in NED frame + * @param next_wp_ned Next waypoint in NED frame + * @return Waypoint transition angle [rad] + */ +float calcWaypointTransitionAngle(Vector2f &prev_wp_ned, Vector2f &curr_wp_ned, Vector2f &next_wp_ned); + +} diff --git a/src/lib/rover_control/RoverControlTest.cpp b/src/lib/rover_control/RoverControlTest.cpp new file mode 100644 index 0000000000..508cdd4486 --- /dev/null +++ b/src/lib/rover_control/RoverControlTest.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the Pure Pursuit algorithm + * Run this test only using "make tests TESTFILTER=RoverControl" +******************************************************************/ + +#include +#include "RoverControl.hpp" + +TEST(calcWaypointTransitionAngle, invalidInputs) +{ + Vector2f prev_wp_ned(NAN, NAN); + Vector2f curr_wp_ned(10.f, 10.f); + Vector2f next_wp_ned(10.f, 10.f); + float prevInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + prev_wp_ned = Vector2f(10.f, 10.f); + curr_wp_ned = Vector2f(NAN, NAN); + float currInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + curr_wp_ned = Vector2f(10.f, 10.f); + next_wp_ned = Vector2f(NAN, NAN); + float nextInvalid = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(prevInvalid)); + EXPECT_FALSE(PX4_ISFINITE(currInvalid)); + EXPECT_FALSE(PX4_ISFINITE(nextInvalid)); + +} + +TEST(calcWaypointTransitionAngle, validInputs) +{ + // P -- C -- N + Vector2f prev_wp_ned(0.f, 0.f); + Vector2f curr_wp_ned(10.f, 0.f); + Vector2f next_wp_ned(20.f, 0.f); + const float angle1 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle1, M_PI_F); + + /** + * N + * / + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(20.f, 10.f); + const float angle2 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle2, M_PI_F - M_PI_4_F); + + /** + * N + * | + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(10.f, 10.f); + const float angle3 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle3, M_PI_2_F); + + /** + * N + * \ + * P -- C + */ + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(0.f, 10.f); + const float angle4 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FLOAT_EQ(angle4, M_PI_4_F); + + // P/C -- N + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(0.f, 0.f); + next_wp_ned = Vector2f(10.f, 0.f); + const float angle5 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(angle5)); + + // P -- C/N + prev_wp_ned = Vector2f(0.f, 0.f); + curr_wp_ned = Vector2f(10.f, 0.f); + next_wp_ned = Vector2f(10.f, 0.f); + const float angle6 = RoverControl::calcWaypointTransitionAngle(prev_wp_ned, curr_wp_ned, next_wp_ned); + EXPECT_FALSE(PX4_ISFINITE(angle6)); +} diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml new file mode 100644 index 0000000000..8450a03b06 --- /dev/null +++ b/src/lib/rover_control/module.yaml @@ -0,0 +1,191 @@ +module_name: Rover Control + +parameters: + - group: Rover Control + definitions: + RO_MAX_THR_SPEED: + description: + short: Speed the rover drives at maximum throttle + long: Used to linearly map speeds [m/s] to throttle values [-1. 1]. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RO_ACCEL_LIM: + description: + short: Acceleration limit + long: | + Set to -1 to disable. + For mecanum rovers this limit is used for longitudinal and lateral acceleration. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_DECEL_LIM: + description: + short: Deceleration limit + long: | + Set to -1 to disable. + Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + For mecanum rovers this limit is used for longitudinal and lateral deceleration. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_JERK_LIM: + description: + short: Jerk limit + long: | + Set to -1 to disable. + Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + For mecanum rovers this limit is used for longitudinal and lateral jerk. + type: float + unit: m/s^3 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_RATE_TH: + description: + short: Yaw rate measurement threshold + long: The minimum threshold for the yaw rate measurement not to be interpreted as zero. + type: float + unit: deg/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 3 + + RO_SPEED_TH: + description: + short: Speed measurement threshold + long: The minimum threshold for the speed measurement not to be interpreted as zero. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0.1 + + RO_YAW_STICK_DZ: + description: + short: Yaw stick deadzone + long: Percentage of stick input range that will be interpreted as zero around the stick centered value. + type: float + min: 0 + max: 1 + increment: 0.01 + decimal: 2 + default: 0.1 + + - group: Rover Rate Control + definitions: + RO_YAW_RATE_P: + description: + short: Proportional gain for closed loop yaw rate controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + RO_YAW_RATE_I: + description: + short: Integral gain for closed loop yaw rate controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + RO_YAW_RATE_LIM: + description: + short: Yaw rate limit + long: | + Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints + in Acro, Stabilized and Position mode. + type: float + unit: deg/s + min: 0 + max: 10000 + increment: 0.01 + decimal: 2 + default: 0 + + RO_YAW_ACCEL_LIM: + description: + short: Yaw acceleration limit + long: Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + - group: Rover Attitude Control + definitions: + RO_YAW_P: + description: + short: Proportional gain for closed loop yaw controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 3 + default: 0 + + - group: Rover Velocity Control + definitions: + RO_SPEED_P: + description: + short: Proportional gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 0 + + RO_SPEED_I: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 + + RO_SPEED_LIM: + description: + short: Speed limit + long: | + Used to cap speed setpoints and map controller inputs to speed setpoints + in Position mode. + type: float + unit: m/s + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 4b1da7d3a1..57b2a429de 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -102,9 +102,13 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); - add_optional_topic("rover_ackermann_guidance_status", 100); - add_optional_topic("rover_ackermann_setpoint", 100); - add_optional_topic("rover_ackermann_status", 100); + add_optional_topic("rover_attitude_setpoint", 100); + add_optional_topic("rover_attitude_status", 100); + add_optional_topic("rover_velocity_status", 100); + add_optional_topic("rover_rate_setpoint", 100); + add_optional_topic("rover_rate_status", 100); + add_optional_topic("rover_steering_setpoint", 100); + add_optional_topic("rover_throttle_setpoint", 100); add_optional_topic("rover_differential_guidance_status", 100); add_optional_topic("rover_differential_setpoint", 100); add_optional_topic("rover_differential_status", 100); diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp new file mode 100644 index 0000000000..1af95cbc1a --- /dev/null +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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 "AckermannAttControl.hpp" + +using namespace time_literals; + +AckermannAttControl::AckermannAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void AckermannAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(0.f); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void AckermannAttControl::updateAttControl() +{ + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate (actuator_motors.control[0], -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + + if (_vehicle_control_mode.flag_control_manual_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void AckermannAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } +} + +void AckermannAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + // Calculate yaw rate limit for slew rate + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) / + _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_slew_rate = math::min(max_possible_yaw_rate, _max_yaw_rate); + + float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, yaw_slew_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool AckermannAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp new file mode 100644 index 0000000000..2acf8de524 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include + +// Library includes +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann attitude control. + */ +class AckermannAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannAttControl. + * @param parent The parent ModuleParams object. + */ + AckermannAttControl(ModuleParams *parent); + ~AckermannAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + float _vehicle_yaw{0.f}; + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_ro_max_thr_speed].*/ + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt rename to src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt index e0574682f1..127927f09e 100644 --- a/src/modules/rover_ackermann/RoverAckermannControl/CMakeLists.txt +++ b/src/modules/rover_ackermann/AckermannAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(RoverAckermannControl - RoverAckermannControl.cpp +px4_add_library(AckermannAttControl + AckermannAttControl.cpp ) -target_link_libraries(RoverAckermannControl PUBLIC PID) -target_include_directories(RoverAckermannControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(AckermannAttControl PUBLIC PID) +target_include_directories(AckermannAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp new file mode 100644 index 0000000000..e9938b5bc8 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp @@ -0,0 +1,418 @@ +/**************************************************************************** + * + * 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 "AckermannPosVelControl.hpp" + +using namespace time_literals; + +AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void AckermannPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } + + if (_param_ra_wheel_base.get() > FLT_EPSILON && _max_yaw_rate > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get()); + } +} + +void AckermannPosVelControl::updatePosVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void AckermannPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void AckermannPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Position Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void AckermannPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_vehicle_speed_body_x) * yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void AckermannPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void AckermannPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void AckermannPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2)); + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + if (_mission_finished) { + _speed_body_x_setpoint = 0.f; + + } else { // Regular guidance algorithm + + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp, + _acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state, + _waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } +} + +void AckermannPosVelControl::updateAutoSubscriptions() +{ + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypointsAndAcceptanceRadius(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _prev_waypoint_transition_angle = _waypoint_transition_angle; + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Update acceptance radius + _prev_acceptance_radius = _acceptance_radius; + + if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { + _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), + _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get()); + + } else { + _acceptance_radius = _param_nav_acc_rad.get(); + } + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); +} + +float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transition_angle, + const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) +{ + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + float acceptance_radius = default_acceptance_radius; + const float theta = waypoint_transition_angle / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = _timestamp; + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min, + const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state, + const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) +{ + // Catch improper values + if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { + return cruising_speed; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); + const float cornering_speed = _max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + const float cornering_speed = _max_yaw_rate * turning_circle; + return math::constrain(cornering_speed, miss_speed_min, cruising_speed); + + } + + // Straight line speed + if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float straight_line_speed{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + + } else { + const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); + float cornering_speed = _max_yaw_rate * turning_circle; + cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); + straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::min(straight_line_speed, cruising_speed); + + } else { + return cruising_speed; + } + +} + +bool AckermannPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp new file mode 100644 index 0000000000..a993be28a5 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp @@ -0,0 +1,257 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Library includes +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for ackermann position control. + */ +class AckermannPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannPosVelControl. + * @param parent The parent ModuleParams object. + */ + AckermannPosVelControl(ModuleParams *parent); + ~AckermannPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Update global/NED waypoint coordinates and acceptance radius. + */ + void updateWaypointsAndAcceptanceRadius(); + + /** + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. + */ + float updateAcceptanceRadius(float waypoint_transition_angle, float default_acceptance_radius, + float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + + /** + * @brief Calculate the speed setpoint. During cornering the speed is restricted based on the radius of the corner. + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next corner with the + * desired cornering speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param miss_speed_min Minimum speed setpoint [m/s]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state, + float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + bool _prev_param_check_passed{true}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _acceptance_radius{0.5f}; + float _prev_acceptance_radius{0.5f}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_gain, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt new file mode 100644 index 0000000000..caa8fb9d5e --- /dev/null +++ b/src/modules/rover_ackermann/AckermannPosVelControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(AckermannPosVelControl + AckermannPosVelControl.cpp +) + +target_link_libraries(AckermannPosVelControl PUBLIC PID) +target_include_directories(AckermannPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp new file mode 100644 index 0000000000..3ec37eb844 --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * 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 "AckermannRateControl.hpp" + +using namespace time_literals; + +AckermannRateControl::AckermannRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void AckermannRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _yaw_rate_setpoint.setSlewRate(_param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F); +} + +void AckermannRateControl::updateRateControl() +{ + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + // Estimate forward speed based on throttle + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors; + _actuator_motors_sub.copy(&actuator_motors); + _estimated_speed_body_x = math::interpolate(actuator_motors.control[0], -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + _estimated_speed_body_x = fabsf(_estimated_speed_body_x) > _param_ro_speed_th.get() ? _estimated_speed_body_x : 0.f; + } + + if (_vehicle_control_mode.flag_control_manual_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void AckermannRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = matrix::sign(_estimated_speed_body_x) * math::interpolate + (manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } +} + +void AckermannRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float steering_setpoint{0.f}; + + if (fabsf(_estimated_speed_body_x) > FLT_EPSILON) { + // Set up feasible yaw rate setpoint + float max_possible_yaw_rate = fabsf(_estimated_speed_body_x) * tanf(_param_ra_max_str_ang.get()) / + _param_ra_wheel_base.get(); // Maximum possible yaw rate at current velocity + float yaw_rate_limit = math::min(max_possible_yaw_rate, _max_yaw_rate); + float constrained_yaw_rate = math::constrain(_rover_rate_setpoint.yaw_rate_setpoint, -yaw_rate_limit, yaw_rate_limit); + + if (_param_ro_yaw_accel_limit.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_yaw_rate_setpoint.getState() - _vehicle_yaw_rate) > fabsf(constrained_yaw_rate - + _vehicle_yaw_rate)) { + _yaw_rate_setpoint.setForcedValue(_vehicle_yaw_rate); + } + + _yaw_rate_setpoint.update(constrained_yaw_rate, _dt); + + } else { + _yaw_rate_setpoint.setForcedValue(constrained_yaw_rate); + } + + // Feed forward + steering_setpoint = atanf(_yaw_rate_setpoint.getState() * _param_ra_wheel_base.get() / _estimated_speed_body_x); + + // Feedback (Only when driving forwards because backwards driving is NMP and can introduce instability) + if (_estimated_speed_body_x > FLT_EPSILON) { + _pid_yaw_rate.setSetpoint(_yaw_rate_setpoint.getState()); + steering_setpoint += _pid_yaw_rate.update(_vehicle_yaw_rate, _dt); + } + + } else { + _pid_yaw_rate.resetIntegral(); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_steering_angle = math::interpolate(steering_setpoint, + -_param_ra_max_str_ang.get(), _param_ra_max_str_ang.get(), -1.f, 1.f); // Normalize steering setpoint + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool AckermannRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_max_thr_speed"), events::Log::Error, + "Invalid configuration of necessary parameter RO_MAX_THR_SPEED", _param_ro_max_thr_speed.get()); + } + + } + + if (_param_ra_wheel_base.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_wheel_base"), events::Log::Error, + "Invalid configuration of necessary parameter RA_WHEEL_BASE", _param_ra_wheel_base.get()); + } + + } + + if (_param_ra_max_str_ang.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_max_str_ang"), events::Log::Error, + "Invalid configuration of necessary parameter RA_MAX_STR_ANG", _param_ra_max_str_ang.get()); + } + + } + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("ackermann_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp new file mode 100644 index 0000000000..b7e194d70c --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp @@ -0,0 +1,139 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for ackermann rate control. + */ +class AckermannRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for AckermannRateControl. + * @param parent The parent ModuleParams object. + */ + AckermannRateControl(ModuleParams *parent); + ~AckermannRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + float _estimated_speed_body_x{0.f}; /*Vehicle speed estimated by interpolating [actuatorMotorSetpoint, _estimated_speed_body_x] + between [0, 0] and [1, _param_ro_max_thr_speed].*/ + float _max_yaw_rate{0.f}; + float _vehicle_yaw_rate{0.f}; + hrt_abstime _timestamp{0}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_speed_th + ) +}; diff --git a/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt b/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt new file mode 100644 index 0000000000..aa80c0734b --- /dev/null +++ b/src/modules/rover_ackermann/AckermannRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(AckermannRateControl + AckermannRateControl.cpp +) + +target_link_libraries(AckermannRateControl PUBLIC PID) +target_include_directories(AckermannRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 423b643bdc..93269c29b9 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverAckermannGuidance) -add_subdirectory(RoverAckermannControl) +add_subdirectory(AckermannRateControl) +add_subdirectory(AckermannAttControl) +add_subdirectory(AckermannPosVelControl) px4_add_module( MODULE modules__rover_ackermann @@ -41,11 +42,13 @@ px4_add_module( RoverAckermann.cpp RoverAckermann.hpp DEPENDS - RoverAckermannGuidance - RoverAckermannControl + AckermannRateControl + AckermannAttControl + AckermannPosVelControl px4_work_queue - SlewRate - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) + +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/rover_control/module.yaml) diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index 3ba1350659..a6acd00251 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of ackermann drive rovers + Enable support for rover_ackermann diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index 5fea7e5174..c76bbf9ebb 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * 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 @@ -33,14 +33,14 @@ #include "RoverAckermann.hpp" +using namespace time_literals; + RoverAckermann::RoverAckermann() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { - _rover_ackermann_setpoint_pub.advertise(); - _ax_filter.setAlpha(0.05); - _ay_filter.setAlpha(0.05); - _az_filter.setAlpha(0.05); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); } @@ -53,176 +53,116 @@ bool RoverAckermann::init() void RoverAckermann::updateParams() { ModuleParams::updateParams(); + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON && _param_ra_max_str_ang.get() > FLT_EPSILON) { + _servo_setpoint.setSlewRate((M_DEG_TO_RAD_F * _param_ra_str_rate_limit.get()) / _param_ra_max_str_ang.get()); + } + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _motor_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverAckermann::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish speed and steering setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = NAN; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = manual_control_setpoint.roll; - rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN; - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = NAN; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = NAN; - rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(manual_control_setpoint.roll, -1.f, 1.f, - -_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get()); - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_ra_max_speed.get(), _param_ra_max_speed.get()); - rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = NAN; - rover_ackermann_setpoint.lateral_acceleration_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_param_ra_max_lat_accel.get(), _param_ra_max_lat_accel.get()); - - if (fabsf(rover_ackermann_setpoint.lateral_acceleration_setpoint) > FLT_EPSILON - || fabsf(rover_ackermann_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _course_control = false; - - } else { // Course control if the steering input is zero (keep driving on a straight line) - if (!_course_control) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _course_control = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( - rover_ackermann_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate steering setpoint - const float steering_setpoint = _ackermann_guidance.calcDesiredSteering(_posctl_pure_pursuit, - target_waypoint_ned, _pos_ctl_start_position_ned, _curr_pos_ned, _param_ra_wheel_base.get(), - rover_ackermann_setpoint.forward_speed_setpoint, _vehicle_yaw, _param_ra_max_steer_angle.get(), _armed); - rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(_vehicle_forward_speed, - 2.f) * tanf(steering_setpoint) / _param_ra_wheel_base.get(); - } - - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _ackermann_guidance.computeGuidance(_vehicle_forward_speed, _vehicle_yaw, _nav_state, _armed); - break; - - default: // Unimplemented nav states will stop the rover - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = NAN; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = 0.f; - rover_ackermann_setpoint.lateral_acceleration_setpoint = NAN; - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - break; - } - - if (!_armed) { - _ackermann_control.resetControllers(); - } - - _ackermann_control.computeMotorCommands(_vehicle_forward_speed, _vehicle_yaw, _vehicle_lateral_acceleration); - -} - -void RoverAckermann::updateSubscriptions() { if (_parameter_update_sub.updated()) { updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { // Reset on mode change - _ackermann_control.resetControllers(); - _course_control = false; - } + _ackermann_pos_vel_control.updatePosVelControl(); + _ackermann_att_control.updateAttControl(); + _ackermann_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == 2; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - _vehicle_attitude_sub.copy(&vehicle_attitude); - _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); + generateActuatorSetpoint(); - if (PX4_ISFINITE(vehicle_local_position.ax)) { - _ax_filter.update(vehicle_local_position.ax); - } +} - if (PX4_ISFINITE(vehicle_local_position.ay)) { - _ay_filter.update(vehicle_local_position.ay); - } +void RoverAckermann::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; - if (PX4_ISFINITE(vehicle_local_position.az)) { - _az_filter.update(vehicle_local_position.az); - } - - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; - Vector3f acceleration_in_local_frame(_ax_filter.getState(), _ay_filter.getState(), _az_filter.getState()); - Vector3f acceleration_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(acceleration_in_local_frame); - _vehicle_lateral_acceleration = acceleration_in_body_frame(1); + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_steering_angle = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } +void RoverAckermann::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_motor_setpoint = actuator_motors.control[0]; + } + + if (_vehicle_control_mode.flag_armed) { + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = RoverControl::throttleControl(_motor_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_motor_setpoint, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (_actuator_servos_sub.updated()) { + actuator_servos_s actuator_servos{}; + _actuator_servos_sub.copy(&actuator_servos); + _current_servo_setpoint = actuator_servos.control[0]; + } + + if (_param_ra_str_rate_limit.get() > FLT_EPSILON + && _param_ra_max_str_ang.get() > FLT_EPSILON) { // Apply slew rate if configured + if (fabsf(_servo_setpoint.getState() - _current_servo_setpoint) > fabsf( + _rover_steering_setpoint.normalized_steering_angle - + _current_servo_setpoint)) { + _servo_setpoint.setForcedValue(_current_servo_setpoint); + } + + _servo_setpoint.update(_rover_steering_setpoint.normalized_steering_angle, _dt); + + } else { + _servo_setpoint.setForcedValue(_rover_steering_setpoint.normalized_steering_angle); + } + + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = _servo_setpoint.getState(); + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); +} + int RoverAckermann::task_spawn(int argc, char *argv[]) { RoverAckermann *instance = new RoverAckermann(); diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index b4e5db5b5e..635abff3f8 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * 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 @@ -39,35 +39,28 @@ #include #include #include + +// Libraries #include +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include - - -// Standard library includes -#include -#include -#include +#include +#include +#include +#include +#include +#include // Local includes -#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" -#include "RoverAckermannControl/RoverAckermannControl.hpp" - -using namespace time_literals; - -// Constants -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "AckermannRateControl/AckermannRateControl.hpp" +#include "AckermannAttControl/AckermannAttControl.hpp" +#include "AckermannPosVelControl/AckermannPosVelControl.hpp" class RoverAckermann : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -90,56 +83,65 @@ public: bool init(); - protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); + + /** + * @brief Generate and publish actuatorMotors/actuatorServos setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); // uORB subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_servos_sub{ORB_ID(actuator_servos)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; // uORB publications - uORB::Publication _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)}; + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; // Class instances - RoverAckermannGuidance _ackermann_guidance{this}; - RoverAckermannControl _ackermann_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + AckermannRateControl _ackermann_rate_control{this}; + AckermannAttControl _ackermann_att_control{this}; + AckermannPosVelControl _ackermann_pos_vel_control{this}; // Variables - matrix::Quatf _vehicle_attitude_quaternion{}; - int _nav_state{0}; - float _vehicle_forward_speed{0.f}; - float _vehicle_yaw{0.f}; - bool _armed{false}; - bool _course_control{false}; - Vector2f _pos_ctl_course_direction{}; - Vector2f _pos_ctl_start_position_ned{}; - Vector2f _curr_pos_ned{}; - float _vehicle_lateral_acceleration{0.f}; - AlphaFilter _ax_filter; - AlphaFilter _ay_filter; - AlphaFilter _az_filter; + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_servo_setpoint{0.f}; + float _current_motor_setpoint{0.f}; + + // Controllers + SlewRate _servo_setpoint{0.f}; + SlewRate _motor_setpoint{0.f}; // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_lat_accel, - (ParamFloat) _param_pp_lookahd_max - + (ParamInt) _param_r_rev, + (ParamFloat) _param_ra_str_rate_limit, + (ParamFloat) _param_ra_max_str_ang, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) - }; diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp deleted file mode 100644 index ad02e79141..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverAckermannControl.hpp" - -#include - -using namespace matrix; - -RoverAckermannControl::RoverAckermannControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_ackermann_status_pub.advertise(); -} - -void RoverAckermannControl::updateParams() -{ - ModuleParams::updateParams(); - - _pid_throttle.setGains(_param_ra_speed_p.get(), _param_ra_speed_i.get(), 0.f); - _pid_throttle.setIntegralLimit(1.f); - _pid_throttle.setOutputLimit(1.f); - - _pid_lat_accel.setGains(_param_ra_lat_accel_p.get(), _param_ra_lat_accel_i.get(), 0.f); - _pid_lat_accel.setIntegralLimit(1.f); - _pid_lat_accel.setOutputLimit(1.f); - - // Update slew rates - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) { - _forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get()); - } - - if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) / - _param_ra_max_steer_angle.get()); - } -} - -void RoverAckermannControl::computeMotorCommands(const float vehicle_forward_speed, const float vehicle_yaw, - const float vehicle_lateral_acceleration) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5_s) * 1e-6f; - - // Update ackermann setpoint - _rover_ackermann_setpoint_sub.update(&_rover_ackermann_setpoint); - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) { - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint, - vehicle_forward_speed, dt, false); - - } else if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_ackermann_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, dt, true); - - } - - // Closed loop lateral acceleration control (overrides steering setpoint) - if (PX4_ISFINITE(_rover_ackermann_setpoint.lateral_acceleration_setpoint)) { - float vehicle_forward_speed_temp{0.f}; - - if (PX4_ISFINITE(_rover_ackermann_setpoint.forward_speed_setpoint)) { // Use valid measurement if available - vehicle_forward_speed_temp = vehicle_forward_speed; - - } else if (PX4_ISFINITE(forward_speed_normalized) && _param_ra_max_thr_speed.get() > FLT_EPSILON) { - vehicle_forward_speed_temp = math::interpolate(forward_speed_normalized, - -1.f, 1.f, -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get()); - } - - if (fabsf(vehicle_forward_speed_temp) > FLT_EPSILON) { - float steering_setpoint = atanf(_param_ra_wheel_base.get() * - _rover_ackermann_setpoint.lateral_acceleration_setpoint / powf( - vehicle_forward_speed_temp, 2.f)); - - if (sign(vehicle_forward_speed_temp) == - 1) { // Only do closed loop control when driving forwards (backwards driving is non-minimum phase and can therefor introduce instability) - _pid_lat_accel.setSetpoint(_rover_ackermann_setpoint.lateral_acceleration_setpoint); - steering_setpoint += _pid_lat_accel.update(vehicle_lateral_acceleration, dt); - } - - _rover_ackermann_setpoint.steering_setpoint = math::constrain(steering_setpoint, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); - - } else { - _rover_ackermann_setpoint.steering_setpoint = 0.f; - } - } - - // Steering control - float steering_normalized{0.f}; - - if (PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint)) { - steering_normalized = math::interpolate(_rover_ackermann_setpoint.steering_setpoint, - -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint - - } else { // Use normalized setpoint - steering_normalized = PX4_ISFINITE(_rover_ackermann_setpoint.steering_setpoint_normalized) ? math::constrain( - _rover_ackermann_setpoint.steering_setpoint_normalized, -1.f, 1.f) : 0.f; - } - - if (_param_ra_max_steering_rate.get() > FLT_EPSILON - && _param_ra_max_steer_angle.get() > FLT_EPSILON) { // Apply slew rate - _steering_with_rate_limit.update(steering_normalized, dt); - - } else { - _steering_with_rate_limit.setForcedValue(steering_normalized); - } - - // Publish rover Ackermann status (logging) - _rover_ackermann_status.timestamp = _timestamp; - _rover_ackermann_status.measured_forward_speed = vehicle_forward_speed; - _rover_ackermann_status.steering_setpoint_normalized = steering_normalized; - _rover_ackermann_status.adjusted_steering_setpoint_normalized = _steering_with_rate_limit.getState(); - _rover_ackermann_status.measured_lateral_acceleration = vehicle_lateral_acceleration; - _rover_ackermann_status.pid_throttle_integral = _pid_throttle.getIntegral(); - _rover_ackermann_status.pid_lat_accel_integral = _pid_lat_accel.getIntegral(); - _rover_ackermann_status_pub.publish(_rover_ackermann_status); - - // Publish to motor - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = forward_speed_normalized; - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - - // Publish to servo - actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = _steering_with_rate_limit.getState(); - actuator_servos.timestamp = _timestamp; - _actuator_servos_pub.publish(actuator_servos); - -} - -float RoverAckermannControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, - const float vehicle_forward_speed, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = _param_ra_max_thr_speed.get() > FLT_EPSILON ? _param_ra_max_thr_speed.get() : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(forward_speed_setpoint) >= fabsf(_forward_speed_setpoint_with_accel_limit.getState())) { - if (_param_ra_max_accel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - _forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / slew_rate_normalization); - _forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - _forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - - } - - } else if (_param_ra_max_decel.get() > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - _forward_speed_setpoint_with_accel_limit.setSlewRate(_param_ra_max_decel.get() / slew_rate_normalization); - _forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - _forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - _rover_ackermann_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - - if (_param_ra_max_thr_speed.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), - -_param_ra_max_thr_speed.get(), _param_ra_max_thr_speed.get(), - -1.f, 1.f); - } - - _pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += _pid_throttle.update(vehicle_forward_speed, dt); - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -void RoverAckermannControl::resetControllers() -{ - _pid_throttle.resetIntegral(); - _pid_lat_accel.resetIntegral(); - _forward_speed_setpoint_with_accel_limit.setForcedValue(0.f); - _steering_with_rate_limit.setForcedValue(0.f); - -} diff --git a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp b/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp deleted file mode 100644 index 7f23caff4c..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannControl/RoverAckermannControl.hpp +++ /dev/null @@ -1,137 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include -#include - -using namespace matrix; -using namespace time_literals; - -/** - * @brief Class for ackermann rover guidance. - */ -class RoverAckermannControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverAckermannControl. - * @param parent The parent ModuleParams object. - */ - RoverAckermannControl(ModuleParams *parent); - ~RoverAckermannControl() = default; - - /** - * @brief Compute motor commands based on setpoints - * @param vehicle_forward_speed Measured speed in body x direction. Positiv: forwards, Negativ: backwards [m/s] - * @param vehicle_yaw Measured yaw [rad] - * @param vehicle_lateral_acceleration Measured acceleration in body y direction. Positiv: right, Negativ: left [m/s^s] - */ - void computeMotorCommands(float vehicle_forward_speed, float vehicle_yaw, float vehicle_lateral_acceleration); - - /** - * @brief Reset PID controllers and slew rates - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Compute normalized forward speed setpoint by applying slew rates - * to the forward speed setpoint and doing closed loop speed control if enabled. - * @param forward_speed_setpoint Forward speed setpoint [m/s]. - * @param vehicle_forward_speed Actual forward speed [m/s]. - * @param dt Time since last update [s]. - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float dt, bool normalized); - - // uORB subscriptions - uORB::Subscription _rover_ackermann_setpoint_sub{ORB_ID(rover_ackermann_setpoint)}; - rover_ackermann_setpoint_s _rover_ackermann_setpoint{}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; - uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; - rover_ackermann_status_s _rover_ackermann_status{}; - - // Variables - hrt_abstime _timestamp{0}; - - // Controllers - PID _pid_throttle; // The PID controller for the closed loop speed control - PID _pid_lat_accel; // The PID controller for the closed loop speed control - SlewRate _steering_with_rate_limit{0.f}; - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_ra_speed_p, - (ParamFloat) _param_ra_speed_i, - (ParamFloat) _param_ra_lat_accel_p, - (ParamFloat) _param_ra_lat_accel_i, - (ParamFloat) _param_ra_max_accel, - (ParamFloat) _param_ra_max_decel, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_thr_speed, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_max_steering_rate, - (ParamFloat) _param_ra_wheel_base, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp deleted file mode 100644 index e8c30083c4..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ /dev/null @@ -1,309 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverAckermannGuidance.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - _rover_ackermann_guidance_status_pub.advertise(); - updateParams(); -} - -void RoverAckermannGuidance::updateParams() -{ - ModuleParams::updateParams(); - - if (_param_ra_wheel_base.get() > FLT_EPSILON && _param_ra_max_lat_accel.get() > FLT_EPSILON - && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _min_speed = sqrt(_param_ra_wheel_base.get() * _param_ra_max_lat_accel.get() / tanf(_param_ra_max_steer_angle.get())); - } -} - -void RoverAckermannGuidance::computeGuidance(const float vehicle_forward_speed, - const float vehicle_yaw, const int nav_state, const bool armed) -{ - updateSubscriptions(); - - // Distances to waypoints - const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _prev_wp(0), _prev_wp(1)); - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - // Catch return to launch - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // Guidance logic - if (_mission_finished) { // Mission is finished - _desired_steering = 0.f; - _desired_speed = 0.f; - - } else if (_nav_cmd == 93) { // Catch delay command - _desired_speed = 0.f; - - } else { // Regular guidance algorithm - - _desired_speed = calcDesiredSpeed(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp, - _acceptance_radius, - _prev_acceptance_radius, _param_ra_max_decel.get(), _param_ra_max_jerk.get(), nav_state, _waypoint_transition_angle, - _prev_waypoint_transition_angle, _param_ra_max_speed.get()); - - _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - _param_ra_wheel_base.get(), _desired_speed, vehicle_yaw, _param_ra_max_steer_angle.get(), armed); - - } - - // Publish ackermann controller status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - _rover_ackermann_guidance_status.timestamp = timestamp; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - - // Publish speed and steering setpoints - rover_ackermann_setpoint_s rover_ackermann_setpoint{}; - rover_ackermann_setpoint.timestamp = timestamp; - rover_ackermann_setpoint.forward_speed_setpoint = _desired_speed; - rover_ackermann_setpoint.forward_speed_setpoint_normalized = NAN; - rover_ackermann_setpoint.steering_setpoint = NAN; - rover_ackermann_setpoint.steering_setpoint_normalized = NAN; - rover_ackermann_setpoint.lateral_acceleration_setpoint = powf(vehicle_forward_speed, - 2.f) * tanf(_desired_steering) / _param_ra_wheel_base.get(); - _rover_ackermann_setpoint_pub.publish(rover_ackermann_setpoint); - -} - -void RoverAckermannGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypointsAndAcceptanceRadius(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_navigator_mission_item_sub.updated()) { - navigator_mission_item_s navigator_mission_item{}; - _navigator_mission_item_sub.copy(&navigator_mission_item); - _nav_cmd = navigator_mission_item.nav_cmd; - } -} - -void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - _prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid - _curr_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if current waypoint is invalid - _next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL - - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - _prev_waypoint_transition_angle = _waypoint_transition_angle; - _waypoint_transition_angle = acosf(cosin); - - // Update acceptance radius - _prev_acceptance_radius = _acceptance_radius; - - if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { - _acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(), - _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get()); - - } else { - _acceptance_radius = _param_nav_acc_rad.get(); - } - - // Waypoint cruising speed - _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get()) : _param_ra_max_speed.get(); -} - -float RoverAckermannGuidance::updateAcceptanceRadius(const float waypoint_transition_angle, - const float default_acceptance_radius, const float acceptance_radius_gain, - const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) -{ - // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment - float acceptance_radius = default_acceptance_radius; - const float theta = waypoint_transition_angle / 2.f; - const float min_turning_radius = wheel_base / sinf(max_steer_angle); - const float acceptance_radius_temp = min_turning_radius / tanf(theta); - const float acceptance_radius_temp_scaled = acceptance_radius_gain * - acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects - acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, - acceptance_radius_max); - - // Publish updated acceptance radius - position_controller_status_s pos_ctrl_status{}; - pos_ctrl_status.acceptance_radius = acceptance_radius; - pos_ctrl_status.timestamp = hrt_absolute_time(); - _position_controller_status_pub.publish(pos_ctrl_status); - return acceptance_radius; -} - -float RoverAckermannGuidance::calcDesiredSpeed(const float cruising_speed, const float miss_speed_min, - const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, - const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state, - const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed) -{ - // Catch improper values - if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) { - return cruising_speed; - } - - // Cornering slow down effect - if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { - const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f); - const float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get()); - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - - } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - const float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get()); - return math::constrain(cornering_speed, miss_speed_min, cruising_speed); - - } - - // Straight line speed - if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { - float straight_line_speed{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp, 0.f); - - } else { - const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f); - float cornering_speed = sqrtf(turning_circle * _param_ra_max_lat_accel.get()); - cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed); - straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, - max_decel, distance_to_curr_wp - acc_rad, cornering_speed); - } - - return math::min(straight_line_speed, cruising_speed); - - } else { - return cruising_speed; - } - -} - -float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, - const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, - const float vehicle_yaw, const float max_steering, const bool armed) -{ - const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, - desired_speed); - const float lookahead_distance = pure_pursuit.getLookaheadDistance(); - const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); - // For logging - _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; - _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); - - float desired_steering{0.f}; - - if (!armed) { - return desired_steering; - } - - if (math::abs_t(heading_error) <= M_PI_2_F) { - desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); - - - } else { - desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - - sign(heading_error) * M_PI_2_F)) / lookahead_distance); - } - - return math::constrain(desired_steering, -max_steering, max_steering); - -} diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp deleted file mode 100644 index cf6fa56fc1..0000000000 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ /dev/null @@ -1,215 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard library includes -#include -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for ackermann drive guidance. - */ -class RoverAckermannGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverAckermannGuidance. - * @param parent The parent ModuleParams object. - */ - RoverAckermannGuidance(ModuleParams *parent); - ~RoverAckermannGuidance() = default; - - /** - * @brief Compute and publish speed and steering setpoints based on the mission plan. - * @param vehicle_forward_speed Forward speed of the vehicle [m/s] - * @param vehicle_yaw Yaw of the vehicle [rad] - * @param nav_state Vehicle navigation state. - * @param armed Vehicle arm state. - */ - void computeGuidance(float vehicle_forward_speed, float vehicle_yaw, int nav_state, bool armed); - - /** - * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to - * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). - * @param pure_pursuit Pure pursuit class instance. - * @param curr_wp_ned Current waypoint in NED frame [m]. - * @param prev_wp_ned Previous waypoint in NED frame [m]. - * @param curr_pos_ned Current position of the vehicle in NED frame [m]. - * @param wheel_base Rover wheelbase [m]. - * @param desired_speed Desired speed for the rover [m/s]. - * @param vehicle_yaw Current yaw of the rover [rad]. - * @param max_steering Maximum steering angle of the rover [rad]. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param armed Vehicle arm status - * @return Steering setpoint for the rover [rad]. - */ - float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering, bool armed); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions - */ - void updateSubscriptions(); - - /** - * @brief Update global/NED waypoint coordinates and acceptance radius. - */ - void updateWaypointsAndAcceptanceRadius(); - - /** - * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param default_acceptance_radius Default acceptance radius for waypoints [m]. - * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. - * @param acceptance_radius_max Maximum value for the acceptance radius [m]. - * @param wheel_base Rover wheelbase [m]. - * @param max_steer_angle Rover maximum steer angle [rad]. - * @return Updated acceptance radius [m]. - */ - float updateAcceptanceRadius(const float waypoint_transition_angle, float default_acceptance_radius, - float acceptance_radius_gain, - float acceptance_radius_max, float wheel_base, float max_steer_angle); - - - /** - * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse - * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a speed trajectory - * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the - * maximum acceleration and jerk. - * @param cruising_speed Cruising speed [m/s]. - * @param miss_speed_min Minimum speed setpoint [m/s]. - * @param distance_to_prev_wp Distance to the previous waypoint [m]. - * @param distance_to_curr_wp Distance to the current waypoint [m]. - * @param acc_rad Acceptance radius of the current waypoint [m]. - * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. - * @param max_accel Maximum allowed acceleration [m/s^2]. - * @param max_jerk Maximum allowed jerk [m/s^3]. - * @param nav_state Current nav_state of the rover. - * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad] - * @param max_speed Maximum speed setpoint [m/s] - * @return Speed setpoint [m/s]. - */ - float calcDesiredSpeed(float cruising_speed, float miss_speed_min, float distance_to_prev_wp, - float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state, - float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - uORB::Subscription _navigator_mission_item_sub{ORB_ID(navigator_mission_item)}; - - // uORB publications - uORB::Publication _rover_ackermann_setpoint_pub{ORB_ID(rover_ackermann_setpoint)}; - uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; - uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; - rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; - - // Class instances - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates - PurePursuit _pure_pursuit{this}; // Pure pursuit library - - // Rover variables - float _desired_steering{0.f}; - float _desired_speed{0.f}; - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - bool _mission_finished{false}; - - // Waypoint variables - Vector2d _home_position{}; - Vector2f _curr_wp_ned{}; - Vector2f _prev_wp_ned{}; - Vector2f _next_wp_ned{}; - Vector2d _curr_wp{}; - Vector2d _prev_wp{}; - Vector2d _next_wp{}; - float _acceptance_radius{0.5f}; - float _prev_acceptance_radius{0.5f}; - float _cruising_speed{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - uint _nav_cmd{0}; - float _min_speed{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_ra_wheel_base, - (ParamFloat) _param_ra_max_steer_angle, - (ParamFloat) _param_ra_acc_rad_max, - (ParamFloat) _param_ra_acc_rad_gain, - (ParamFloat) _param_ra_p_speed, - (ParamFloat) _param_ra_i_speed, - (ParamFloat) _param_ra_max_speed, - (ParamFloat) _param_ra_max_jerk, - (ParamFloat) _param_ra_max_decel, - (ParamFloat) _param_ra_max_lat_accel, - (ParamFloat) _param_nav_acc_rad - ) -}; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml index 0c779f494f..383c9e73b2 100644 --- a/src/modules/rover_ackermann/module.yaml +++ b/src/modules/rover_ackermann/module.yaml @@ -6,26 +6,37 @@ parameters: RA_WHEEL_BASE: description: short: Wheel base - long: Distance from the front to the rear axle + long: Distance from the front to the rear axle. type: float unit: m - min: 0.001 + min: 0 max: 100 increment: 0.001 decimal: 3 - default: 0.5 + default: 0 RA_MAX_STR_ANG: description: short: Maximum steering angle - long: The maximum angle that the rover can steer type: float unit: rad - min: 0.1 + min: 0 max: 1.5708 increment: 0.01 decimal: 2 - default: 0.5236 + default: 0 + + RA_STR_RATE_LIM: + description: + short: Steering rate limit + long: Set to -1 to disable. + type: float + unit: deg/s + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 RA_ACC_RAD_MAX: description: @@ -41,7 +52,7 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 3 + default: -1 RA_ACC_RAD_GAIN: description: @@ -55,142 +66,4 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 - - RA_SPEED_P: - description: - short: Proportional gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 default: 1 - - RA_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.001 - decimal: 3 - default: 1 - - RA_MAX_SPEED: - description: - short: Maximum allowed speed - long: | - This is the maximum allowed speed setpoint when driving in a mode that uses - closed loop speed control. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_THR_SPEED: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_ACCEL: - description: - short: Maximum acceleration for the rover - long: | - This is used for the acceleration slew rate. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_DECEL: - description: - short: Maximum deceleration for the rover - long: | - This is used for the deceleration slew rate, the feed-forward term - for the speed controller during missions and the corner slow down effect. - Note: For the corner slow down effect RA_MAX_JERK also has to be set. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_JERK: - description: - short: Maximum jerk - long: | - Limit for forwards acc/deceleration change. - This is used for the corner slow down effect. - Note: RA_MAX_DECEL also has to be set for this to be enabled. - type: float - unit: m/s^3 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_STR_RATE: - description: - short: Maximum steering rate for the rover - type: float - unit: deg/s - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 - - RA_MAX_LAT_ACCEL: - description: - short: Maximum allowed lateral acceleration - long: | - This parameter is used to cap the lateral acceleration and map controller inputs to desired lateral acceleration - in Acro, Stabilized and Position mode. - type: float - unit: m/s^2 - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 0.01 - - RA_LAT_ACCEL_P: - description: - short: Proportional gain for lateral acceleration controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - - RA_LAT_ACCEL_I: - description: - short: Integral gain for lateral acceleration controller - type: float - min: 0 - max: 100 - increment: 0.001 - decimal: 3 - default: 0 diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index 6458fb1fea..020cb43231 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -57,7 +57,7 @@ * @min 0.0 * @decimal 3 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f); @@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f); * @max 50.0 * @decimal 1 * @increment 0.1 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f); @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f); * @max 50.0 * @decimal 1 * @increment 0.5 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f); @@ -103,7 +103,7 @@ PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f); * @max 0.9 * @decimal 2 * @increment 0.05 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f); @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(GND_L1_DAMPING, 0.75f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f); @@ -133,7 +133,7 @@ PARAM_DEFINE_FLOAT(GND_THR_CRUISE, 0.1f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f); @@ -148,7 +148,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f); * @max 1.0 * @decimal 2 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); @@ -160,7 +160,7 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); * @max 1 * @value 0 open loop control * @value 1 close the loop with gps speed - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); @@ -174,7 +174,7 @@ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f); @@ -188,7 +188,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_P, 2.0f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f); @@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_I, 3.0f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f); @@ -216,7 +216,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_D, 0.001f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f); @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_IMAX, 1.0f); * @max 50.0 * @decimal 3 * @increment 0.005 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f); @@ -243,7 +243,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_THR_SC, 1.0f); * @max 40 * @decimal 1 * @increment 0.5 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); @@ -256,7 +256,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); * @max 40 * @decimal 1 * @increment 0.5 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); @@ -271,7 +271,7 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); * @max 3.14159 * @decimal 3 * @increment 0.01 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); @@ -282,6 +282,6 @@ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); * @min 0.0 * @max 400 * @decimal 1 - * @group Rover Position Control + * @group Rover Position Control (Deprecated) */ PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f); From 047578a844ff4a42049b19b59cf0579507bc6a18 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Mon, 10 Feb 2025 18:16:48 +0100 Subject: [PATCH 13/63] Disarm PWM ESCs before reboot --- boards/3dr/ctrl-zero-h7-oem-revg/src/init.c | 7 ++++++- boards/ark/fmu-v6x/src/init.c | 7 ++++++- boards/ark/fpv/src/init.c | 7 ++++++- boards/ark/pi6x/src/init.c | 7 ++++++- boards/av/x-v1/src/init.c | 7 ++++++- boards/cuav/7-nano/src/init.c | 7 ++++++- boards/cuav/nora/src/init.c | 7 ++++++- boards/cuav/x7pro/src/init.c | 7 ++++++- boards/cubepilot/cubeorange/src/init.c | 7 ++++++- boards/cubepilot/cubeorangeplus/src/init.c | 7 ++++++- boards/cubepilot/cubeyellow/src/init.c | 7 ++++++- boards/hkust/nxt-dual/src/init.c | 7 ++++++- boards/hkust/nxt-v1/src/init.c | 7 ++++++- boards/holybro/durandal-v1/src/init.c | 7 ++++++- boards/holybro/kakutef7/src/init.c | 7 ++++++- boards/holybro/kakuteh7/src/init.c | 7 ++++++- boards/holybro/kakuteh7mini/src/init.c | 7 ++++++- boards/holybro/kakuteh7v2/src/init.c | 7 ++++++- boards/holybro/pix32v5/src/init.c | 7 ++++++- boards/matek/h743-mini/src/init.c | 7 ++++++- boards/matek/h743-slim/src/init.c | 7 ++++++- boards/matek/h743/src/init.c | 7 ++++++- boards/micoair/h743-aio/src/init.c | 7 ++++++- boards/micoair/h743-v2/src/init.c | 7 ++++++- boards/micoair/h743/src/init.c | 7 ++++++- boards/modalai/fc-v1/src/init.c | 7 ++++++- boards/modalai/fc-v2/src/init.c | 7 ++++++- boards/mro/ctrl-zero-classic/src/init.c | 7 ++++++- boards/mro/ctrl-zero-f7-oem/src/init.c | 7 ++++++- boards/mro/ctrl-zero-f7/src/init.c | 7 ++++++- boards/mro/ctrl-zero-h7-oem/src/init.c | 7 ++++++- boards/mro/ctrl-zero-h7/src/init.c | 7 ++++++- boards/mro/pixracerpro/src/init.c | 7 ++++++- boards/mro/x21-777/src/init.c | 10 +++++----- boards/nxp/fmuk66-e/src/init.c | 7 ++++++- boards/nxp/fmuk66-v3/src/init.c | 7 ++++++- boards/nxp/tropic-community/src/init.c | 7 ++++++- boards/px4/fmu-v5/src/init.c | 7 ++++++- boards/px4/fmu-v5x/src/init.cpp | 7 ++++++- boards/px4/fmu-v6c/src/init.c | 7 ++++++- boards/px4/fmu-v6u/src/init.c | 7 ++++++- boards/px4/fmu-v6x/src/init.cpp | 7 ++++++- boards/px4/fmu-v6xrt/src/init.c | 7 ++++++- boards/siyi/n7/src/init.c | 7 ++++++- boards/sky-drones/smartap-airlink/src/init.cpp | 7 ++++++- boards/spracing/h7extreme/src/init.c | 7 ++++++- boards/x-mav/ap-h743v2/src/init.c | 7 ++++++- boards/zeroone/x6/src/init.c | 7 ++++++- 48 files changed, 287 insertions(+), 52 deletions(-) diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c index daaace48fb..0081dc62dd 100755 --- a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/ark/fmu-v6x/src/init.c b/boards/ark/fmu-v6x/src/init.c index 56c94f805b..6c1c2257ed 100644 --- a/boards/ark/fmu-v6x/src/init.c +++ b/boards/ark/fmu-v6x/src/init.c @@ -150,8 +150,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/ark/fpv/src/init.c b/boards/ark/fpv/src/init.c index 5e321b3b52..cbd9382238 100644 --- a/boards/ark/fpv/src/init.c +++ b/boards/ark/fpv/src/init.c @@ -145,8 +145,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/ark/pi6x/src/init.c b/boards/ark/pi6x/src/init.c index 90bb4fc128..4dcef47cac 100644 --- a/boards/ark/pi6x/src/init.c +++ b/boards/ark/pi6x/src/init.c @@ -141,8 +141,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/av/x-v1/src/init.c b/boards/av/x-v1/src/init.c index 8e12ee823e..0f455559f1 100644 --- a/boards/av/x-v1/src/init.c +++ b/boards/av/x-v1/src/init.c @@ -94,8 +94,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cuav/7-nano/src/init.c b/boards/cuav/7-nano/src/init.c index 3bd2164622..6ee02bb529 100644 --- a/boards/cuav/7-nano/src/init.c +++ b/boards/cuav/7-nano/src/init.c @@ -142,8 +142,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cuav/nora/src/init.c b/boards/cuav/nora/src/init.c index d4c262b76a..a8f3bf4ad3 100644 --- a/boards/cuav/nora/src/init.c +++ b/boards/cuav/nora/src/init.c @@ -113,8 +113,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cuav/x7pro/src/init.c b/boards/cuav/x7pro/src/init.c index d4c262b76a..a8f3bf4ad3 100644 --- a/boards/cuav/x7pro/src/init.c +++ b/boards/cuav/x7pro/src/init.c @@ -113,8 +113,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cubepilot/cubeorange/src/init.c b/boards/cubepilot/cubeorange/src/init.c index f7e1b3ca77..9bf3d5f58e 100644 --- a/boards/cubepilot/cubeorange/src/init.c +++ b/boards/cubepilot/cubeorange/src/init.c @@ -104,8 +104,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cubepilot/cubeorangeplus/src/init.c b/boards/cubepilot/cubeorangeplus/src/init.c index f7e1b3ca77..9bf3d5f58e 100644 --- a/boards/cubepilot/cubeorangeplus/src/init.c +++ b/boards/cubepilot/cubeorangeplus/src/init.c @@ -104,8 +104,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/cubepilot/cubeyellow/src/init.c b/boards/cubepilot/cubeyellow/src/init.c index 8ba8972402..0eb3701c04 100644 --- a/boards/cubepilot/cubeyellow/src/init.c +++ b/boards/cubepilot/cubeyellow/src/init.c @@ -102,8 +102,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/hkust/nxt-dual/src/init.c b/boards/hkust/nxt-dual/src/init.c index 657c0080c0..cd7d3d2162 100644 --- a/boards/hkust/nxt-dual/src/init.c +++ b/boards/hkust/nxt-dual/src/init.c @@ -100,8 +100,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/hkust/nxt-v1/src/init.c b/boards/hkust/nxt-v1/src/init.c index 657c0080c0..cd7d3d2162 100644 --- a/boards/hkust/nxt-v1/src/init.c +++ b/boards/hkust/nxt-v1/src/init.c @@ -100,8 +100,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/durandal-v1/src/init.c b/boards/holybro/durandal-v1/src/init.c index f8dc55d72f..283b44842e 100644 --- a/boards/holybro/durandal-v1/src/init.c +++ b/boards/holybro/durandal-v1/src/init.c @@ -144,8 +144,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakutef7/src/init.c b/boards/holybro/kakutef7/src/init.c index 9b8a0140f4..e4b6bae87d 100644 --- a/boards/holybro/kakutef7/src/init.c +++ b/boards/holybro/kakutef7/src/init.c @@ -124,8 +124,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakuteh7/src/init.c b/boards/holybro/kakuteh7/src/init.c index cf73735aed..9c609d4486 100644 --- a/boards/holybro/kakuteh7/src/init.c +++ b/boards/holybro/kakuteh7/src/init.c @@ -129,8 +129,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakuteh7mini/src/init.c b/boards/holybro/kakuteh7mini/src/init.c index c8f19df734..4691703e8f 100644 --- a/boards/holybro/kakuteh7mini/src/init.c +++ b/boards/holybro/kakuteh7mini/src/init.c @@ -127,8 +127,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/kakuteh7v2/src/init.c b/boards/holybro/kakuteh7v2/src/init.c index 7e55cc1770..bff6e6535a 100644 --- a/boards/holybro/kakuteh7v2/src/init.c +++ b/boards/holybro/kakuteh7v2/src/init.c @@ -127,8 +127,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/holybro/pix32v5/src/init.c b/boards/holybro/pix32v5/src/init.c index 2153838a60..fd2449ee7d 100644 --- a/boards/holybro/pix32v5/src/init.c +++ b/boards/holybro/pix32v5/src/init.c @@ -143,8 +143,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/matek/h743-mini/src/init.c b/boards/matek/h743-mini/src/init.c index bb1c6f975c..9ffa799caf 100644 --- a/boards/matek/h743-mini/src/init.c +++ b/boards/matek/h743-mini/src/init.c @@ -94,8 +94,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/matek/h743-slim/src/init.c b/boards/matek/h743-slim/src/init.c index f78e570c5a..899bb1ba4d 100644 --- a/boards/matek/h743-slim/src/init.c +++ b/boards/matek/h743-slim/src/init.c @@ -98,8 +98,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/matek/h743/src/init.c b/boards/matek/h743/src/init.c index a2a531882c..f24ad5eb7c 100644 --- a/boards/matek/h743/src/init.c +++ b/boards/matek/h743/src/init.c @@ -94,8 +94,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/micoair/h743-aio/src/init.c b/boards/micoair/h743-aio/src/init.c index 43f86f902d..c0e1f9776c 100644 --- a/boards/micoair/h743-aio/src/init.c +++ b/boards/micoair/h743-aio/src/init.c @@ -98,8 +98,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/micoair/h743-v2/src/init.c b/boards/micoair/h743-v2/src/init.c index 43f86f902d..c0e1f9776c 100644 --- a/boards/micoair/h743-v2/src/init.c +++ b/boards/micoair/h743-v2/src/init.c @@ -98,8 +98,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/micoair/h743/src/init.c b/boards/micoair/h743/src/init.c index 43f86f902d..c0e1f9776c 100644 --- a/boards/micoair/h743/src/init.c +++ b/boards/micoair/h743/src/init.c @@ -98,8 +98,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/modalai/fc-v1/src/init.c b/boards/modalai/fc-v1/src/init.c index beee10b875..254d5b3397 100644 --- a/boards/modalai/fc-v1/src/init.c +++ b/boards/modalai/fc-v1/src/init.c @@ -141,8 +141,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/modalai/fc-v2/src/init.c b/boards/modalai/fc-v2/src/init.c index dbc6ec4637..651c301072 100644 --- a/boards/modalai/fc-v2/src/init.c +++ b/boards/modalai/fc-v2/src/init.c @@ -139,8 +139,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-classic/src/init.c b/boards/mro/ctrl-zero-classic/src/init.c index daaace48fb..0081dc62dd 100644 --- a/boards/mro/ctrl-zero-classic/src/init.c +++ b/boards/mro/ctrl-zero-classic/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-f7-oem/src/init.c b/boards/mro/ctrl-zero-f7-oem/src/init.c index 0b0b07ff12..af04388898 100644 --- a/boards/mro/ctrl-zero-f7-oem/src/init.c +++ b/boards/mro/ctrl-zero-f7-oem/src/init.c @@ -132,8 +132,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-f7/src/init.c b/boards/mro/ctrl-zero-f7/src/init.c index 0b0b07ff12..af04388898 100644 --- a/boards/mro/ctrl-zero-f7/src/init.c +++ b/boards/mro/ctrl-zero-f7/src/init.c @@ -132,8 +132,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-h7-oem/src/init.c b/boards/mro/ctrl-zero-h7-oem/src/init.c index daaace48fb..0081dc62dd 100644 --- a/boards/mro/ctrl-zero-h7-oem/src/init.c +++ b/boards/mro/ctrl-zero-h7-oem/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/ctrl-zero-h7/src/init.c b/boards/mro/ctrl-zero-h7/src/init.c index daaace48fb..0081dc62dd 100644 --- a/boards/mro/ctrl-zero-h7/src/init.c +++ b/boards/mro/ctrl-zero-h7/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/pixracerpro/src/init.c b/boards/mro/pixracerpro/src/init.c index 481c35c336..6fe192857e 100644 --- a/boards/mro/pixracerpro/src/init.c +++ b/boards/mro/pixracerpro/src/init.c @@ -107,8 +107,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/mro/x21-777/src/init.c b/boards/mro/x21-777/src/init.c index 3f5f00e377..dd6edda517 100644 --- a/boards/mro/x21-777/src/init.c +++ b/boards/mro/x21-777/src/init.c @@ -106,13 +106,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } - /** - * On resets invoked from system (not boot) insure we establish a low - * output state (discharge the pins) on PWM pins before they become inputs. + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. */ - if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/nxp/fmuk66-e/src/init.c b/boards/nxp/fmuk66-e/src/init.c index affa94492e..e8580466b2 100644 --- a/boards/nxp/fmuk66-e/src/init.c +++ b/boards/nxp/fmuk66-e/src/init.c @@ -120,8 +120,13 @@ void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/nxp/fmuk66-v3/src/init.c b/boards/nxp/fmuk66-v3/src/init.c index 57e8633459..d5bc68d52e 100644 --- a/boards/nxp/fmuk66-v3/src/init.c +++ b/boards/nxp/fmuk66-v3/src/init.c @@ -120,8 +120,13 @@ void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/nxp/tropic-community/src/init.c b/boards/nxp/tropic-community/src/init.c index 8e5db86b37..a9c326f39c 100644 --- a/boards/nxp/tropic-community/src/init.c +++ b/boards/nxp/tropic-community/src/init.c @@ -134,8 +134,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_gpio_output(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v5/src/init.c b/boards/px4/fmu-v5/src/init.c index cfec81bae6..660a338b20 100644 --- a/boards/px4/fmu-v5/src/init.c +++ b/boards/px4/fmu-v5/src/init.c @@ -143,8 +143,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp index c4debd31ae..b743bf1310 100644 --- a/boards/px4/fmu-v5x/src/init.cpp +++ b/boards/px4/fmu-v5x/src/init.cpp @@ -145,8 +145,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6c/src/init.c b/boards/px4/fmu-v6c/src/init.c index 5a853b31b1..ff8d7a8b3c 100644 --- a/boards/px4/fmu-v6c/src/init.c +++ b/boards/px4/fmu-v6c/src/init.c @@ -137,8 +137,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6u/src/init.c b/boards/px4/fmu-v6u/src/init.c index 1563331909..ce67db5066 100644 --- a/boards/px4/fmu-v6u/src/init.c +++ b/boards/px4/fmu-v6u/src/init.c @@ -144,8 +144,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6x/src/init.cpp b/boards/px4/fmu-v6x/src/init.cpp index 4924120123..d914b75497 100644 --- a/boards/px4/fmu-v6x/src/init.cpp +++ b/boards/px4/fmu-v6x/src/init.cpp @@ -145,8 +145,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/px4/fmu-v6xrt/src/init.c b/boards/px4/fmu-v6xrt/src/init.c index 57ac95ce20..25faa213f5 100644 --- a/boards/px4/fmu-v6xrt/src/init.c +++ b/boards/px4/fmu-v6xrt/src/init.c @@ -153,8 +153,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_gpio_output(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/siyi/n7/src/init.c b/boards/siyi/n7/src/init.c index 46cf4ae4aa..bc8147ece1 100644 --- a/boards/siyi/n7/src/init.c +++ b/boards/siyi/n7/src/init.c @@ -136,8 +136,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/sky-drones/smartap-airlink/src/init.cpp b/boards/sky-drones/smartap-airlink/src/init.cpp index c4b4c1fa7e..73a9582f9b 100644 --- a/boards/sky-drones/smartap-airlink/src/init.cpp +++ b/boards/sky-drones/smartap-airlink/src/init.cpp @@ -142,8 +142,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/spracing/h7extreme/src/init.c b/boards/spracing/h7extreme/src/init.c index b241fb6286..38e7d0cbe8 100644 --- a/boards/spracing/h7extreme/src/init.c +++ b/boards/spracing/h7extreme/src/init.c @@ -125,8 +125,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/x-mav/ap-h743v2/src/init.c b/boards/x-mav/ap-h743v2/src/init.c index 657c0080c0..cd7d3d2162 100644 --- a/boards/x-mav/ap-h743v2/src/init.c +++ b/boards/x-mav/ap-h743v2/src/init.c @@ -100,8 +100,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } diff --git a/boards/zeroone/x6/src/init.c b/boards/zeroone/x6/src/init.c index cd6714af16..6320b5580e 100644 --- a/boards/zeroone/x6/src/init.c +++ b/boards/zeroone/x6/src/init.c @@ -144,8 +144,13 @@ __EXPORT void board_on_reset(int status) px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); } + /* + * On resets invoked from system (not boot) ensure we establish a low + * output state on PWM pins to disarm the ESC and prevent the reset from potentially + * spinning up the motors. + */ if (status >= 0) { - up_mdelay(6); + up_mdelay(100); } } From 8576b49e4975d9f9b87d1285e8b2db15f8069ac5 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Sat, 8 Feb 2025 13:08:27 -0700 Subject: [PATCH 14/63] make format: ignore libuavcan --- Tools/astyle/files_to_check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 6a96e55859..bec6856b75 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -12,6 +12,7 @@ exec find boards msg src platforms test \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/ins/vectornav/libvnc -prune -o \ -path src/drivers/uavcan/libdronecan -prune -o \ + -path src/drivers/uavcan/libuavcan -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ -path src/drivers/cyphal/libcanard -prune -o \ -path src/lib/crypto/monocypher -prune -o \ From 35da86c85ac5d05786003ebb13e45869c3d39f58 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 7 Feb 2025 16:39:33 +0300 Subject: [PATCH 15/63] improve unit tests for range fog detection Signed-off-by: RomanBapst --- src/modules/ekf2/test/test_SensorRangeFinder.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index c2e9d1c667..ecf69d83b5 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -354,14 +354,13 @@ TEST_F(SensorRangeFinderTest, blockedByFog) const Dcmf attitude{Eulerf(0.f, 0.f, 0.f)}; const uint64_t dt_update_us = 10e3; uint64_t dt_sensor_us = 3e5; - uint64_t duration_us = 5e5; + uint64_t duration_us = 2e6; updateSensorAtRate(_good_sample, duration_us, dt_update_us, dt_sensor_us); // THEN: the data should be marked as healthy EXPECT_TRUE(_range_finder.isDataHealthy()); EXPECT_TRUE(_range_finder.isHealthy()); - // WHEN: sensor is then blocked by fog // range jumps to value below 2m uint64_t t_now_us = _range_finder.getSampleAddress()->time_us; @@ -374,6 +373,7 @@ TEST_F(SensorRangeFinderTest, blockedByFog) // WHEN: the sensor is not blocked by fog anymore sample.rng = 5.f; + sample.time_us = _range_finder.getSampleAddress()->time_us; updateSensorAtRate(sample, duration_us, dt_update_us, dt_sensor_us); // THEN: the data should be marked as healthy again From 73ade6d05c3984da2de8a1f77ca9b2b9731ccdab Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 7 Feb 2025 16:40:04 +0300 Subject: [PATCH 16/63] range finder: remove potential deadlock Signed-off-by: RomanBapst --- .../ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index c0ae2a71eb..53ea72e40b 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -149,7 +149,7 @@ void SensorRangeFinder::updateStuckCheck() void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us) { - if (_max_fog_dist > 0.f && time_us - _time_last_valid_us < 1e6) { + if (_max_fog_dist > 0.f) { const float median_dist = _median_dist.apply(dist_bottom); const float factor = 2.f; // magic hardcoded factor From 806500fc4aeb35b86341493c5ef01289c6826d73 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Feb 2025 10:13:09 +0300 Subject: [PATCH 17/63] further improve logic Signed-off-by: RomanBapst --- .../range_finder/sensor_range_finder.cpp | 32 ++++++++++++------- .../range_finder/sensor_range_finder.hpp | 2 ++ 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 53ea72e40b..680dd06a74 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -76,25 +76,33 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) if (_is_sample_ready) { _is_sample_valid = false; - if (_sample.quality == 0) { - _time_bad_quality_us = current_time_us; + _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_last_valid_us; - } else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) { - // We did not receive bad quality data for some time + const bool quality_ok = isQualityOk(current_time_us); - if (isTiltOk() && isDataInRange()) { - updateStuckCheck(); - updateFogCheck(getDistBottom(), _sample.time_us); + if (!quality_ok || !isTiltOk() || !isDataInRange()) { + return; + } - if (!_is_stuck && !_is_blocked) { - _is_sample_valid = true; - _time_last_valid_us = _sample.time_us; - } - } + updateStuckCheck(); + + if (!_is_stuck) { + updateFogCheck(getDistBottom(), _sample.time_us); + } + + if (!_is_stuck && !_is_blocked) { + _is_sample_valid = true; + _time_last_valid_us = _sample.time_us; } } } +bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const +{ + const bool bad_quality = current_time_us - _time_bad_quality_us > _quality_hyst_us; + return bad_quality; +} + void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us) { // Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant. diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index fd53407fa8..e3e4f6e6cd 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -131,6 +131,7 @@ private: bool isDataContinuous() const { return _dt_data_lpf < 2e6f; } bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; } bool isDataInRange() const; + bool isQualityOk(uint64_t current_time_us) const; void updateStuckCheck(); void updateFogCheck(const float dist_bottom, const uint64_t time_us); @@ -184,6 +185,7 @@ private: float _max_fog_dist{0.f}; //< maximum distance at which the range finder could detect fog (m) math::MedianFilter _median_dist{}; float _prev_median_dist{0.f}; + }; } // namespace sensor From 2c4d38e303ea6593f9383a655899ebb275c9e434 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Feb 2025 10:15:17 +0300 Subject: [PATCH 18/63] fixed mistake Signed-off-by: RomanBapst --- .../ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 680dd06a74..35cbcd0c9e 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -76,7 +76,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) if (_is_sample_ready) { _is_sample_valid = false; - _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_last_valid_us; + _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us; const bool quality_ok = isQualityOk(current_time_us); From 8cedef2dc46db6cfea9a0e15b66ecafa01e43ac5 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Feb 2025 10:17:42 +0300 Subject: [PATCH 19/63] further cleanup Signed-off-by: RomanBapst --- .../EKF/aid_sources/range_finder/sensor_range_finder.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 35cbcd0c9e..67197a4a61 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -78,9 +78,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) _time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us; - const bool quality_ok = isQualityOk(current_time_us); - - if (!quality_ok || !isTiltOk() || !isDataInRange()) { + if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) { return; } @@ -99,8 +97,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const { - const bool bad_quality = current_time_us - _time_bad_quality_us > _quality_hyst_us; - return bad_quality; + return current_time_us - _time_bad_quality_us > _quality_hyst_us; } void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us) From 2b75d2e738eaab9950f483da4c3b948b4d4f8d1a Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 10 Feb 2025 16:38:44 +0300 Subject: [PATCH 20/63] run fog check independently from stuck check Signed-off-by: RomanBapst --- .../EKF/aid_sources/range_finder/sensor_range_finder.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index 67197a4a61..ab1c747e01 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -83,10 +83,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us) } updateStuckCheck(); - - if (!_is_stuck) { - updateFogCheck(getDistBottom(), _sample.time_us); - } + updateFogCheck(getDistBottom(), _sample.time_us); if (!_is_stuck && !_is_blocked) { _is_sample_valid = true; From 136f9f48fcc7f5ffa30ad5628a7b4f9e92be0e96 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 11 Feb 2025 12:12:11 +0100 Subject: [PATCH 21/63] FW Position controller: transform RWTO_PSP to rad when used (#24312) Signed-off-by: Silvan --- .../fw_pos_control/FixedwingPositionControl.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a15f1e8f71..8baddd55e6 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1858,8 +1858,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, POST_TOUCHDOWN_CLAMP_TIME, 0.0f, 1.0f); - pitch_max_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_max_rad; - pitch_min_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_min_rad; + pitch_max_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_max_rad; + pitch_min_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_min_rad; } // idle throttle may be >0 for internal combustion engines @@ -2078,8 +2080,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float touchdown_interpolator = math::constrain((seconds_since_flare_start - touchdown_time) / POST_TOUCHDOWN_CLAMP_TIME, 0.0f, 1.0f); - pitch_max_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_max_rad; - pitch_min_rad = touchdown_interpolator * _param_rwto_psp.get() + (1.0f - touchdown_interpolator) * pitch_min_rad; + pitch_max_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_max_rad; + pitch_min_rad = touchdown_interpolator * math::radians(_param_rwto_psp.get()) + (1.0f - touchdown_interpolator) * + pitch_min_rad; } // idle throttle may be >0 for internal combustion engines From 975ec30c9c3c588845c041080abe960d66b3986e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 29 Oct 2024 11:47:52 +0100 Subject: [PATCH 22/63] uxrce_dds_client: add DDS message versioning This adds "_v" + string(T::MESSAGE_VERSION) to the ROS topic if the message contains a MESSAGE_VERSION field (and the version is non-zero). --- src/modules/uxrce_dds_client/dds_topics.h.em | 25 +++++++++++++++++++- src/modules/uxrce_dds_client/utilities.hpp | 24 ++++++++++++++----- 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index 69c8fc58cc..81eadef1bb 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -39,11 +39,31 @@ static constexpr int max_topic_size = 512; static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too large, increase max_topic_size"); @[ end for]@ + + +// SFINAE to use R::MESSAGE_VERSION if it exists, and 0 otherwise +template +class MessageVersionHelper +{ + template + static constexpr uint32_t get(decltype(&C::MESSAGE_VERSION)) { return C::MESSAGE_VERSION; } + template + static constexpr uint32_t get(...) { return 0; } +public: + static constexpr uint32_t m = get(0); +}; + +template +static constexpr uint32_t get_message_version() { + return MessageVersionHelper::m; +} + struct SendSubscription { const struct orb_metadata *orb_meta; uxrObjectId data_writer; const char* dds_type_name; const char* topic; + uint32_t message_version; uint32_t topic_size; UcdrSerializeMethod ucdr_serialize_method; }; @@ -56,6 +76,7 @@ struct SendTopicsSubs { uxr_object_id(0, UXR_INVALID_ID), "@(pub['dds_type'])", "@(pub['topic'])", + get_message_version<@(pub['simple_base_type'])_s>(), ucdr_topic_size_@(pub['simple_base_type'])(), &ucdr_serialize_@(pub['simple_base_type']), }, @@ -101,6 +122,7 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) { // data writer not created yet create_data_writer(session, reliable_out_stream_id, participant_id, static_cast(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].topic, + send_subscriptions[idx].message_version, send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); } @@ -173,7 +195,8 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id @[ for idx, sub in enumerate(subscriptions + subscriptions_multi)]@ { uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 2; // use a bit larger queue size than internal - create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic'])", "@(sub['dds_type'])", queue_depth); + uint32_t message_version = get_message_version<@(sub['simple_base_type'])_s>(); + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic'])", message_version, "@(sub['dds_type'])", queue_depth); } @[ end for]@ diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 70886ec795..c2d038b3ed 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -23,29 +23,40 @@ uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) return uxrObjectId{}; } -static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic) +static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic, + uint32_t message_version = 0) { if (topic[0] == '/') { topic++; } + char version[16]; + + if (message_version != 0) { + snprintf(version, sizeof(version), "_v%u", (unsigned)message_version); + version[sizeof(version) - 1] = '\0'; + + } else { + version[0] = '\0'; + } + if (client_namespace != nullptr) { - int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s", client_namespace, topic); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s%s", client_namespace, topic, version); return (ret > 0 && ret < TOPIC_NAME_SIZE); } - int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s", topic); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s%s", topic, version); return (ret > 0 && ret < TOPIC_NAME_SIZE); } static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id, - ORB_ID orb_id, const char *client_namespace, const char *topic, const char *type_name, + ORB_ID orb_id, const char *client_namespace, const char *topic, uint32_t message_version, const char *type_name, uxrObjectId &datawriter_id) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, topic)) { + if (!generate_topic_name(topic_name, client_namespace, topic, message_version)) { PX4_ERR("topic path too long"); return false; } @@ -92,12 +103,13 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic, + uint32_t message_version, const char *type_name, uint16_t queue_depth) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, topic)) { + if (!generate_topic_name(topic_name, client_namespace, topic, message_version)) { PX4_ERR("topic path too long"); return false; } From f6bfa9812e8fdc876d78a565b141efeba8ea6b41 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 29 Oct 2024 14:50:08 +0100 Subject: [PATCH 23/63] msg: add message translation node for ROS --- Tools/astyle/files_to_check_code_style.sh | 1 + Tools/copy_to_ros_ws.sh | 34 + msg/px4_msgs_old/CMakeLists.txt | 76 +++ msg/px4_msgs_old/package.xml | 25 + msg/px4_msgs_old/rename_msg_type.py.in | 39 ++ msg/translation_node/CMakeLists.txt | 82 +++ msg/translation_node/README.md | 6 + msg/translation_node/package.xml | 27 + msg/translation_node/src/graph.h | 293 ++++++++ msg/translation_node/src/main.cpp | 39 ++ msg/translation_node/src/monitor.cpp | 60 ++ msg/translation_node/src/monitor.h | 23 + msg/translation_node/src/pub_sub_graph.cpp | 195 ++++++ msg/translation_node/src/pub_sub_graph.h | 58 ++ msg/translation_node/src/service_graph.cpp | 230 +++++++ msg/translation_node/src/service_graph.h | 76 +++ msg/translation_node/src/template_util.h | 64 ++ msg/translation_node/src/translation_util.h | 386 +++++++++++ msg/translation_node/src/translations.cpp | 5 + msg/translation_node/src/translations.h | 91 +++ msg/translation_node/src/util.h | 51 ++ msg/translation_node/test/graph.cpp | 623 ++++++++++++++++++ msg/translation_node/test/main.cpp | 16 + msg/translation_node/test/pub_sub.cpp | 350 ++++++++++ msg/translation_node/test/services.cpp | 215 ++++++ msg/translation_node/test/srv/TestV0.srv | 4 + msg/translation_node/test/srv/TestV1.srv | 4 + msg/translation_node/test/srv/TestV2.srv | 6 + .../translations/all_translations.h | 11 + .../example_translation_direct_v1.h | 30 + .../example_translation_multi_v2.h | 42 ++ .../example_translation_service_v1.h | 38 ++ 32 files changed, 3200 insertions(+) create mode 100755 Tools/copy_to_ros_ws.sh create mode 100644 msg/px4_msgs_old/CMakeLists.txt create mode 100644 msg/px4_msgs_old/package.xml create mode 100755 msg/px4_msgs_old/rename_msg_type.py.in create mode 100644 msg/translation_node/CMakeLists.txt create mode 100644 msg/translation_node/README.md create mode 100644 msg/translation_node/package.xml create mode 100644 msg/translation_node/src/graph.h create mode 100644 msg/translation_node/src/main.cpp create mode 100644 msg/translation_node/src/monitor.cpp create mode 100644 msg/translation_node/src/monitor.h create mode 100644 msg/translation_node/src/pub_sub_graph.cpp create mode 100644 msg/translation_node/src/pub_sub_graph.h create mode 100644 msg/translation_node/src/service_graph.cpp create mode 100644 msg/translation_node/src/service_graph.h create mode 100644 msg/translation_node/src/template_util.h create mode 100644 msg/translation_node/src/translation_util.h create mode 100644 msg/translation_node/src/translations.cpp create mode 100644 msg/translation_node/src/translations.h create mode 100644 msg/translation_node/src/util.h create mode 100644 msg/translation_node/test/graph.cpp create mode 100644 msg/translation_node/test/main.cpp create mode 100644 msg/translation_node/test/pub_sub.cpp create mode 100644 msg/translation_node/test/services.cpp create mode 100644 msg/translation_node/test/srv/TestV0.srv create mode 100644 msg/translation_node/test/srv/TestV1.srv create mode 100644 msg/translation_node/test/srv/TestV2.srv create mode 100644 msg/translation_node/translations/all_translations.h create mode 100644 msg/translation_node/translations/example_translation_direct_v1.h create mode 100644 msg/translation_node/translations/example_translation_multi_v2.h create mode 100644 msg/translation_node/translations/example_translation_service_v1.h diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index bec6856b75..266760d7af 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -8,6 +8,7 @@ if [ $# -gt 0 ]; then fi exec find boards msg src platforms test \ + -path msg/translation_node -prune -o \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/ins/vectornav/libvnc -prune -o \ diff --git a/Tools/copy_to_ros_ws.sh b/Tools/copy_to_ros_ws.sh new file mode 100755 index 0000000000..ef663aaaa9 --- /dev/null +++ b/Tools/copy_to_ros_ws.sh @@ -0,0 +1,34 @@ +#! /bin/bash +# Copy msgs and the message translation node into a ROS workspace directory + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +PX4_SRC_DIR="$DIR/.." + +WS_DIR="$1" +if [ ! -e "${WS_DIR}" ] +then + echo "Usage: $0 " + exit 1 +fi +WS_DIR="$WS_DIR"/src +if [ ! -e "${WS_DIR}" ] +then + echo "'src' directory not found inside ROS workspace (${WS_DIR})" + exit 1 +fi + +cp -ar "${PX4_SRC_DIR}"/msg/translation_node "${WS_DIR}" +cp -ar "${PX4_SRC_DIR}"/msg/px4_msgs_old "${WS_DIR}" +PX4_MSGS_DIR="${WS_DIR}"/px4_msgs +if [ ! -e "${PX4_MSGS_DIR}" ] +then + git clone https://github.com/PX4/px4_msgs.git "${PX4_MSGS_DIR}" + rm -rf "${PX4_MSGS_DIR}"/msg/*.msg + rm -rf "${PX4_MSGS_DIR}"/msg/versioned/*.msg + rm -rf "${PX4_MSGS_DIR}"/srv/*.srv +fi +cp -ar "${PX4_SRC_DIR}"/msg/*.msg "${PX4_MSGS_DIR}"/msg +mkdir -p "${PX4_MSGS_DIR}"/msg/versioned +cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg/versioned +cp -ar "${PX4_SRC_DIR}"/srv/*.srv "${PX4_MSGS_DIR}"/srv diff --git a/msg/px4_msgs_old/CMakeLists.txt b/msg/px4_msgs_old/CMakeLists.txt new file mode 100644 index 0000000000..add08bc670 --- /dev/null +++ b/msg/px4_msgs_old/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.5) + +project(px4_msgs_old) + +list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# ############################################################################## +# Generate ROS messages, ROS2 interfaces and IDL files # +# ############################################################################## + +# get all msg files +set(MSGS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/msg") +file(GLOB PX4_MSGS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${MSGS_DIR}/*.msg") + +# get all srv files +set(SRVS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/srv") +file(GLOB PX4_SRVS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${SRVS_DIR}/*.srv") + + + +# For the versioned topics, replace the namespace (px4_msgs_old -> px4_msgs) and message type name (Vx -> ), +# so that DDS does not reject the subscription/publication due to mismatching type +# rosidl_typesupport_fastrtps_cpp +set(rosidl_typesupport_fastrtps_cpp_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_cpp_wrapper.py) +file(TOUCH ${rosidl_typesupport_fastrtps_cpp_BIN}) + +# rosidl_typesupport_fastrtps_c +set(rosidl_typesupport_fastrtps_c_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_fastrtps_c_wrapper.py) +file(TOUCH ${rosidl_typesupport_fastrtps_c_BIN}) + +# rosidl_typesupport_introspection_cpp (for cyclonedds) +set(rosidl_typesupport_introspection_cpp_BIN ${CMAKE_CURRENT_BINARY_DIR}/rosidl_typesupport_introspection_cpp_wrapper.py) +file(TOUCH ${rosidl_typesupport_introspection_cpp_BIN}) + +# Generate introspection typesupport for C and C++ and IDL files +if(PX4_MSGS) + rosidl_generate_interfaces(${PROJECT_NAME} + ${PX4_MSGS} + ${PX4_SRVS} + DEPENDENCIES builtin_interfaces + ADD_LINTER_TESTS + ) +endif() + +# rosidl_typesupport_fastrtps_cpp +set(rosidl_typesupport_fastrtps_cpp_orig ${rosidl_typesupport_fastrtps_cpp_DIR}) +string(REPLACE "share/rosidl_typesupport_fastrtps_cpp/cmake" "lib/rosidl_typesupport_fastrtps_cpp/rosidl_typesupport_fastrtps_cpp" + rosidl_typesupport_fastrtps_cpp_orig ${rosidl_typesupport_fastrtps_cpp_DIR}) +set(original_script_path ${rosidl_typesupport_fastrtps_cpp_orig}) +configure_file(rename_msg_type.py.in ${rosidl_typesupport_fastrtps_cpp_BIN} @ONLY) + +# rosidl_typesupport_fastrtps_c +set(rosidl_typesupport_fastrtps_c_orig ${rosidl_typesupport_fastrtps_c_DIR}) +string(REPLACE "share/rosidl_typesupport_fastrtps_c/cmake" "lib/rosidl_typesupport_fastrtps_c/rosidl_typesupport_fastrtps_c" + rosidl_typesupport_fastrtps_c_orig ${rosidl_typesupport_fastrtps_c_DIR}) +set(original_script_path ${rosidl_typesupport_fastrtps_c_orig}) +configure_file(rename_msg_type.py.in ${rosidl_typesupport_fastrtps_c_BIN} @ONLY) + +# rosidl_typesupport_introspection_cpp +set(rosidl_typesupport_introspection_cpp_orig ${rosidl_typesupport_introspection_cpp_DIR}) +string(REPLACE "share/rosidl_typesupport_introspection_cpp/cmake" "lib/rosidl_typesupport_introspection_cpp/rosidl_typesupport_introspection_cpp" + rosidl_typesupport_introspection_cpp_orig ${rosidl_typesupport_introspection_cpp_DIR}) +set(original_script_path ${rosidl_typesupport_introspection_cpp_orig}) +configure_file(rename_msg_type.py.in ${rosidl_typesupport_introspection_cpp_BIN} @ONLY) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/msg/px4_msgs_old/package.xml b/msg/px4_msgs_old/package.xml new file mode 100644 index 0000000000..42b83c6f9a --- /dev/null +++ b/msg/px4_msgs_old/package.xml @@ -0,0 +1,25 @@ + + + + px4_msgs_old + 2.0.1 + Package with the ROS-equivalent of PX4 uORB msgs (old message definitions) + PX4 + BSD 3-Clause + + ament_cmake + rosidl_default_generators + + builtin_interfaces + ros_environment + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/msg/px4_msgs_old/rename_msg_type.py.in b/msg/px4_msgs_old/rename_msg_type.py.in new file mode 100755 index 0000000000..31d8054e95 --- /dev/null +++ b/msg/px4_msgs_old/rename_msg_type.py.in @@ -0,0 +1,39 @@ +#! /bin/python +import sys +import subprocess +import json +import os +import re + +original_script = "@original_script_path@" +args = sys.argv[1:] + +json_file = [arg for arg in args if arg.endswith('.json')][0] + +proc = subprocess.run(['python3', original_script] + args) +proc.check_returncode() + +def replace_namespace_and_type(content: str): + # Replace namespace type + content = content.replace('"px4_msgs_old"', '"px4_msgs"') + content = content.replace('"px4_msgs_old::msg"', '"px4_msgs::msg"') + # Replace versioned type with non-versioned one + content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+"', '\\1"', content) + # Services + content = content.replace('"px4_msgs_old::srv"', '"px4_msgs::srv"') + content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Request"', '\\1_Request"', content) + content = re.sub(r'("[a-zA-Z0-9]+)V[0-9]+_Response"', '\\1_Response"', content) + return content + +with open(json_file, 'r') as f: + data = json.load(f) + output_dir = data['output_dir'] + + # Iterate files recursively + for root, dirs, files in os.walk(output_dir): + for file in files: + with open(os.path.join(root, file), 'r+') as f: + content = f.read() + f.seek(0) + f.write(replace_namespace_and_type(content)) + f.truncate() diff --git a/msg/translation_node/CMakeLists.txt b/msg/translation_node/CMakeLists.txt new file mode 100644 index 0000000000..4880709ce2 --- /dev/null +++ b/msg/translation_node/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.8) +project(translation_node) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Werror) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(px4_msgs_old REQUIRED) + +if(DEFINED ENV{ROS_DISTRO}) + set(ROS_DISTRO $ENV{ROS_DISTRO}) +else() + set(ROS_DISTRO "rolling") +endif() + + +add_library(${PROJECT_NAME}_lib + src/monitor.cpp + src/pub_sub_graph.cpp + src/service_graph.cpp + src/translations.cpp +) +ament_target_dependencies(${PROJECT_NAME}_lib rclcpp px4_msgs px4_msgs_old) +add_executable(${PROJECT_NAME}_bin + src/main.cpp +) +target_link_libraries(${PROJECT_NAME}_bin ${PROJECT_NAME}_lib) +target_include_directories(${PROJECT_NAME}_bin PUBLIC src) +ament_target_dependencies(${PROJECT_NAME}_bin rclcpp px4_msgs px4_msgs_old) +install(TARGETS + ${PROJECT_NAME}_bin + DESTINATION lib/${PROJECT_NAME}) + +option(DISABLE_SERVICES "Disable services" OFF) +if(${ROS_DISTRO} STREQUAL "humble") + message(WARNING "Disabling services for ROS humble (API is not supported)") + target_compile_definitions(${PROJECT_NAME}_lib PRIVATE DISABLE_SERVICES) + set(DISABLE_SERVICES ON) +endif() + +if(BUILD_TESTING) + find_package(std_msgs REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(rosidl_default_generators REQUIRED) + ament_lint_auto_find_test_dependencies() + + set(SRV_FILES + test/srv/TestV0.srv + test/srv/TestV1.srv + test/srv/TestV2.srv + ) + rosidl_generate_interfaces(${PROJECT_NAME} ${SRV_FILES}) + + # Unit tests + set(TEST_SRC + test/graph.cpp + test/main.cpp + test/pub_sub.cpp + ) + if (NOT DISABLE_SERVICES) + list(APPEND TEST_SRC test/services.cpp) + endif() + ament_add_gtest(${PROJECT_NAME}_unit_tests + ${TEST_SRC} + ) + target_include_directories(${PROJECT_NAME}_unit_tests PRIVATE ${CMAKE_CURRENT_LIST_DIR}) + target_compile_options(${PROJECT_NAME}_unit_tests PRIVATE -Wno-error=sign-compare) # There is a warning from gtest internal + target_link_libraries(${PROJECT_NAME}_unit_tests ${PROJECT_NAME}_lib) + rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_unit_tests "${cpp_typesupport_target}") + ament_target_dependencies(${PROJECT_NAME}_unit_tests + std_msgs + rclcpp + ) +endif() + +ament_package() diff --git a/msg/translation_node/README.md b/msg/translation_node/README.md new file mode 100644 index 0000000000..69e0843bb5 --- /dev/null +++ b/msg/translation_node/README.md @@ -0,0 +1,6 @@ +# Message Translations + +This package contains a message translation node and a set of old message conversion methods. +It allows to run applications that are compiled with one set of message versions against a PX4 with another set of message versions, without having to change either the application or the PX4 side. + +For details, see https://docs.px4.io/main/en/ros2/px4_ros2_msg_translation_node.html. diff --git a/msg/translation_node/package.xml b/msg/translation_node/package.xml new file mode 100644 index 0000000000..7d6e03511b --- /dev/null +++ b/msg/translation_node/package.xml @@ -0,0 +1,27 @@ + + + + translation_node + 0.0.0 + Message version translation node + PX4 + BSD 3-Clause + + ament_cmake + rosidl_default_generators + + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + std_msgs + + rclcpp + px4_msgs + px4_msgs_old + + + ament_cmake + + diff --git a/msg/translation_node/src/graph.h b/msg/translation_node/src/graph.h new file mode 100644 index 0000000000..0feac52fa9 --- /dev/null +++ b/msg/translation_node/src/graph.h @@ -0,0 +1,293 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include "util.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// This implements a directed graph with potential cycles used for translation. +// There are 2 types of nodes: messages (e.g. publication/subscription endpoints) and +// translations. Translation nodes are always in between message nodes, and can have N input messages +// and M output messages. + +struct MessageIdentifier { + std::string topic_name; + MessageVersionType version; + + bool operator==(const MessageIdentifier& other) const { + return topic_name == other.topic_name && version == other.version; + } + bool operator!=(const MessageIdentifier& other) const { + return !(*this == other); + } +}; + +template<> +struct std::hash +{ + std::size_t operator()(const MessageIdentifier& s) const noexcept + { + std::size_t h1 = std::hash{}(s.topic_name); + std::size_t h2 = std::hash{}(s.version); + return h1 ^ (h2 << 1); + } +}; + + +using MessageBuffer = std::shared_ptr; + +template +class MessageNode; +template +class Graph; + +template +using MessageNodePtrT = std::shared_ptr>; + +template +class TranslationNode { +public: + using TranslationCB = std::function&, std::vector&)>; + + TranslationNode(std::vector> inputs, + std::vector> outputs, + TranslationCB translation_db) + : _inputs(std::move(inputs)), _outputs(std::move(outputs)), _translation_cb(std::move(translation_db)) { + assert(_inputs.size() <= kMaxNumInputs); + + _input_buffers.resize(_inputs.size()); + for (unsigned i = 0; i < _inputs.size(); ++i) { + _input_buffers[i] = _inputs[i]->buffer(); + } + + _output_buffers.resize(_outputs.size()); + for (unsigned i = 0; i < _outputs.size(); ++i) { + _output_buffers[i] = _outputs[i]->buffer(); + } + } + + void setInputReady(unsigned index) { + _inputs_ready.set(index); + } + + bool translate() { + if (_inputs_ready.count() == _input_buffers.size()) { + _translation_cb(_input_buffers, _output_buffers); + _inputs_ready.reset(); + return true; + } + return false; + } + + const std::vector>& inputs() const { return _inputs; } + const std::vector>& outputs() const { return _outputs; } + +private: + static constexpr int kMaxNumInputs = 32; + + const std::vector> _inputs; + std::vector _input_buffers; ///< Cached buffers from _inputs.buffer() + const std::vector> _outputs; + std::vector _output_buffers; + const TranslationCB _translation_cb; + + std::bitset _inputs_ready; +}; + +template +using TranslationNodePtrT = std::shared_ptr>; + + +template +class MessageNode { +public: + + explicit MessageNode(NodeData node_data, size_t index, MessageBuffer message_buffer) + : _buffer(std::move(message_buffer)), _data(std::move(node_data)), _index(index) {} + + MessageBuffer& buffer() { return _buffer; } + + void addTranslationInput(TranslationNodePtrT node, unsigned input_index) { + _translations.push_back(Translation{std::move(node), input_index}); + } + + NodeData& data() { return _data; } + + void resetNodes() { + _translations.clear(); + } + +private: + struct Translation { + TranslationNodePtrT node; ///< Counterpart to the TranslationNode::_inputs + unsigned input_index; ///< Index into the TranslationNode::_inputs + }; + MessageBuffer _buffer; + std::vector _translations; + + NodeData _data; + + const size_t _index; + + friend class Graph; +}; + +template +class Graph { +public: + using MessageNodePtr = MessageNodePtrT; + + ~Graph() { + // Explicitly reset the nodes array to break up potential cycles and prevent memory leaks + for (auto& [id, node] : _nodes) { + node->resetNodes(); + } + } + + /** + * @brief Add a message node if it does not exist already + */ + bool addNodeIfNotExists(const IdType& id, NodeData node_data, const MessageBuffer& message_buffer) { + if (_nodes.find(id) != _nodes.end()) { + return false; + } + // Node that we cannot remove nodes due to using the index as an array index + const size_t index = _nodes.size(); + _nodes.insert({id, std::make_shared>(std::move(node_data), index, message_buffer)}); + return true; + } + + /** + * @brief Add a translation edge with N inputs and M output nodes. All nodes must already exist. + */ + void addTranslation(const typename TranslationNode::TranslationCB& translation_cb, + const std::vector& inputs, const std::vector& outputs) { + auto init = [this](const std::vector& from, std::vector>& to) { + for (unsigned i=0; i < from.size(); ++i) { + auto node_iter = _nodes.find(from[i]); + assert(node_iter != _nodes.end()); + to[i] = node_iter->second; + } + }; + std::vector> input_nodes(inputs.size()); + init(inputs, input_nodes); + std::vector> output_nodes(outputs.size()); + init(outputs, output_nodes); + + auto translation_node = std::make_shared>(std::move(input_nodes), std::move(output_nodes), translation_cb); + for (unsigned i=0; i < translation_node->inputs().size(); ++i) { + translation_node->inputs()[i]->addTranslationInput(translation_node, i); + } + } + + + /** + * @brief Translate a message node in the graph. + * + * @param node The message node to translate. + * @param on_translated A callback function that is called for translated nodes (with an updated message buffer). + * This will not be called for the provided node. + */ + void translate(const MessageNodePtr& node, + const std::function& on_translated) { + resetNodesVisited(); + + // Iterate all reachable nodes from a given node using the BFS (shortest path) algorithm, + // while using translation nodes as barriers (only continue when all inputs are ready) + + std::queue queue; + _node_visited[node->_index] = true; + queue.push(node); + + while (!queue.empty()) { + MessageNodePtr current = queue.front(); + queue.pop(); + for (auto& translation : current->_translations) { + const bool any_output_visited = + std::any_of(translation.node->outputs().begin(), translation.node->outputs().end(), [&](const MessageNodePtr& next_node) { + return _node_visited[next_node->_index]; + }); + // If any output node has already been visited, skip this translation node (prevents translating + // backwards, from where we came from already) + if (any_output_visited) { + continue; + } + translation.node->setInputReady(translation.input_index); + // Iterate the output nodes only if the translation node is ready + if (translation.node->translate()) { + + for (auto &next_node : translation.node->outputs()) { + if (_node_visited[next_node->_index]) { + continue; + } + _node_visited[next_node->_index] = true; + on_translated(next_node); + queue.push(next_node); + } + } + } + } + } + + std::optional findNode(const IdType& id) const { + auto iter = _nodes.find(id); + if (iter == _nodes.end()) { + return std::nullopt; + } + return iter->second; + } + + void iterateNodes(const std::function& cb) const { + for (const auto& [id, node] : _nodes) { + cb(id, node); + } + } + + /** + * Iterate all reachable nodes from a given node using the BFS (shortest path) algorithm + */ + void iterateBFS(const MessageNodePtr& node, const std::function& cb) { + resetNodesVisited(); + + std::queue queue; + _node_visited[node->_index] = true; + queue.push(node); + cb(node); + + while (!queue.empty()) { + MessageNodePtr current = queue.front(); + queue.pop(); + for (auto& translation : current->_translations) { + for (auto& next_node : translation.node->outputs()) { + if (_node_visited[next_node->_index]) { + continue; + } + _node_visited[next_node->_index] = true; + queue.push(next_node); + + cb(next_node); + } + } + } + } + + +private: + void resetNodesVisited() { + _node_visited.resize(_nodes.size()); + std::fill(_node_visited.begin(), _node_visited.end(), false); + } + + std::unordered_map _nodes; + std::vector _node_visited; ///< Cached, to avoid the need to re-allocate on each iteration +}; diff --git a/msg/translation_node/src/main.cpp b/msg/translation_node/src/main.cpp new file mode 100644 index 0000000000..218583d85d --- /dev/null +++ b/msg/translation_node/src/main.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include + +#include + +#include "../translations/all_translations.h" +#include "pub_sub_graph.h" +#include "service_graph.h" +#include "monitor.h" + +using namespace std::chrono_literals; + +class RosTranslationNode : public rclcpp::Node +{ +public: + RosTranslationNode() : Node("translation_node") + { + _pub_sub_graph = std::make_unique(*this, RegisteredTranslations::instance().topicTranslations()); + _service_graph = std::make_unique(*this, RegisteredTranslations::instance().serviceTranslations()); + _monitor = std::make_unique(*this, _pub_sub_graph.get(), _service_graph.get()); + } + +private: + std::unique_ptr _pub_sub_graph; + std::unique_ptr _service_graph; + rclcpp::TimerBase::SharedPtr _node_update_timer; + std::unique_ptr _monitor; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/msg/translation_node/src/monitor.cpp b/msg/translation_node/src/monitor.cpp new file mode 100644 index 0000000000..9746ec3f89 --- /dev/null +++ b/msg/translation_node/src/monitor.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include "monitor.h" +using namespace std::chrono_literals; + +Monitor::Monitor(rclcpp::Node &node, PubSubGraph* pub_sub_graph, ServiceGraph* service_graph) + : _node(node), _pub_sub_graph(pub_sub_graph), _service_graph(service_graph) { + + // Monitor subscriptions & publishers + // TODO: event-based + _node_update_timer = _node.create_wall_timer(1s, [this]() { + updateNow(); + }); +} + +void Monitor::updateNow() { + + // Topics + if (_pub_sub_graph != nullptr) { + std::vector topic_info; + const auto topics = _node.get_topic_names_and_types(); + for (const auto &[topic_name, topic_types]: topics) { + auto publishers = _node.get_publishers_info_by_topic(topic_name); + auto subscribers = _node.get_subscriptions_info_by_topic(topic_name); + // Filter out self + int num_publishers = 0; + for (const auto &publisher: publishers) { + num_publishers += publisher.node_name() != _node.get_name(); + } + int num_subscribers = 0; + for (const auto &subscriber: subscribers) { + num_subscribers += subscriber.node_name() != _node.get_name(); + } + + if (num_subscribers > 0 || num_publishers > 0) { + topic_info.emplace_back(PubSubGraph::TopicInfo{topic_name, num_subscribers, num_publishers}); + } + } + _pub_sub_graph->updateCurrentTopics(topic_info); + } + + // Services +#ifndef DISABLE_SERVICES // ROS Humble does not support the count_services() call + if (_service_graph != nullptr) { + std::vector service_info; + const auto services = _node.get_service_names_and_types(); + for (const auto& [service_name, service_types] : services) { + const int num_services = _node.get_node_graph_interface()->count_services(service_name); + const int num_clients = _node.get_node_graph_interface()->count_clients(service_name); + // We cannot filter out our own node, as we don't have that info. + // We could use `get_service_names_and_types_by_node`, but then we would not get + // services by non-ros nodes (e.g. microxrce dds bridge) + service_info.emplace_back(ServiceGraph::ServiceInfo{service_name, num_services, num_clients}); + } + _service_graph->updateCurrentServices(service_info); + } +#endif +} diff --git a/msg/translation_node/src/monitor.h b/msg/translation_node/src/monitor.h new file mode 100644 index 0000000000..cd335aa83b --- /dev/null +++ b/msg/translation_node/src/monitor.h @@ -0,0 +1,23 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include "pub_sub_graph.h" +#include "service_graph.h" +#include + +class Monitor { +public: + explicit Monitor(rclcpp::Node &node, PubSubGraph* pub_sub_graph, ServiceGraph* service_graph); + + void updateNow(); + +private: + rclcpp::Node &_node; + PubSubGraph* _pub_sub_graph{nullptr}; + ServiceGraph* _service_graph{nullptr}; + rclcpp::TimerBase::SharedPtr _node_update_timer; +}; diff --git a/msg/translation_node/src/pub_sub_graph.cpp b/msg/translation_node/src/pub_sub_graph.cpp new file mode 100644 index 0000000000..f75d20fe96 --- /dev/null +++ b/msg/translation_node/src/pub_sub_graph.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include "pub_sub_graph.h" +#include "util.h" + +PubSubGraph::PubSubGraph(rclcpp::Node &node, const TopicTranslations &translations) : _node(node) { + + std::unordered_map> known_versions; + + for (const auto& topic : translations.topics()) { + const std::string full_topic_name = getFullTopicName(_node.get_effective_namespace(), topic.id.topic_name); + _known_topics_warned.insert({full_topic_name, false}); + + const MessageIdentifier id{full_topic_name, topic.id.version}; + NodeDataPubSub node_data{topic.subscription_factory, topic.publication_factory, id, topic.max_serialized_message_size}; + _pub_sub_graph.addNodeIfNotExists(id, std::move(node_data), topic.message_buffer); + known_versions[full_topic_name].insert(id.version); + } + + auto get_full_topic_names = [this](std::vector ids) { + for (auto& id : ids) { + id.topic_name = getFullTopicName(_node.get_effective_namespace(), id.topic_name); + } + return ids; + }; + + for (const auto& translation : translations.translations()) { + const std::vector inputs = get_full_topic_names(translation.inputs); + const std::vector outputs = get_full_topic_names(translation.outputs); + _pub_sub_graph.addTranslation(translation.cb, inputs, outputs); + } + + printTopicInfo(known_versions); + handleLargestTopic(known_versions); +} + +void PubSubGraph::updateCurrentTopics(const std::vector &topics) { + + _pub_sub_graph.iterateNodes([](const MessageIdentifier& type, const Graph::MessageNodePtr& node) { + node->data().has_external_publisher = false; + node->data().has_external_subscriber = false; + node->data().visited = false; + }); + + for (const auto& info : topics) { + const auto [non_versioned_topic_name, version] = getNonVersionedTopicName(info.topic_name); + auto maybe_node = _pub_sub_graph.findNode({non_versioned_topic_name, version}); + if (!maybe_node) { + auto known_topic_iter = _known_topics_warned.find(non_versioned_topic_name); + if (known_topic_iter != _known_topics_warned.end() && !known_topic_iter->second) { + RCLCPP_WARN(_node.get_logger(), "No translation available for version %i of topic %s", version, non_versioned_topic_name.c_str()); + known_topic_iter->second = true; + } + continue; + } + const auto& node = maybe_node.value(); + + if (info.num_publishers > 0) { + node->data().has_external_publisher = true; + } + if (info.num_subscribers > 0) { + node->data().has_external_subscriber = true; + } + } + + // Iterate connected graph segments + _pub_sub_graph.iterateNodes([this](const MessageIdentifier& type, const Graph::MessageNodePtr& node) { + if (node->data().visited) { + return; + } + node->data().visited = true; + + // Count the number of external subscribers and publishers for each connected graph + int num_publishers = 0; + int num_subscribers = 0; + int num_subscribers_without_publisher = 0; + + _pub_sub_graph.iterateBFS(node, [&](const Graph::MessageNodePtr& node) { + if (node->data().has_external_publisher) { + ++num_publishers; + } + if (node->data().has_external_subscriber) { + ++num_subscribers; + if (!node->data().has_external_publisher) { + ++num_subscribers_without_publisher; + } + } + }); + + // We need to instantiate publishers and subscribers if: + // - there are multiple publishers and at least 1 subscriber + // - there is 1 publisher and at least 1 subscriber on another node + // Note that in case of splitting or merging topics, this might create more entities than actually needed + const bool require_translation = (num_publishers >= 2 && num_subscribers >= 1) + || (num_publishers == 1 && num_subscribers_without_publisher >= 1); + if (require_translation) { + _pub_sub_graph.iterateBFS(node, [&](const Graph::MessageNodePtr& node) { + node->data().visited = true; + // Has subscriber(s)? + if (node->data().has_external_subscriber && !node->data().publication) { + RCLCPP_INFO(_node.get_logger(), "Found subscriber for topic '%s', version: %i, adding publisher", node->data().topic_name.c_str(), node->data().version); + node->data().publication = node->data().publication_factory(_node); + } else if (!node->data().has_external_subscriber && node->data().publication) { + RCLCPP_INFO(_node.get_logger(), "No subscribers for topic '%s', version: %i, removing publisher", node->data().topic_name.c_str(), node->data().version); + node->data().publication.reset(); + } + // Has publisher(s)? + if (node->data().has_external_publisher && !node->data().subscription) { + RCLCPP_INFO(_node.get_logger(), "Found publisher for topic '%s', version: %i, adding subscriber", node->data().topic_name.c_str(), node->data().version); + node->data().subscription = node->data().subscription_factory(_node, [this, node_cpy=node]() { + onSubscriptionUpdate(node_cpy); + }); + } else if (!node->data().has_external_publisher && node->data().subscription) { + RCLCPP_INFO(_node.get_logger(), "No publishers for topic '%s', version: %i, removing subscriber", node->data().topic_name.c_str(), node->data().version); + node->data().subscription.reset(); + } + }); + + } else { + // Reset any publishers or subscribers + _pub_sub_graph.iterateBFS(node, [&](const Graph::MessageNodePtr& node) { + node->data().visited = true; + if (node->data().publication) { + RCLCPP_INFO(_node.get_logger(), "Removing publisher for topic '%s', version: %i", + node->data().topic_name.c_str(), node->data().version); + node->data().publication.reset(); + } + if (node->data().subscription) { + RCLCPP_INFO(_node.get_logger(), "Removing subscriber for topic '%s', version: %i", + node->data().topic_name.c_str(), node->data().version); + node->data().subscription.reset(); + } + }); + } + }); +} + +void PubSubGraph::onSubscriptionUpdate(const Graph::MessageNodePtr& node) { + _pub_sub_graph.translate( + node, + [this](const Graph::MessageNodePtr& node) { + if (node->data().publication != nullptr) { + const auto ret = rcl_publish(node->data().publication->get_publisher_handle().get(), + node->buffer().get(), nullptr); + if (ret != RCL_RET_OK) { + RCLCPP_WARN_ONCE(_node.get_logger(), "Failed to publish on topic '%s', version: %i", + node->data().topic_name.c_str(), node->data().version); + } + } + }); + +} + +void PubSubGraph::printTopicInfo(const std::unordered_map>& known_versions) const { + // Print info about known versions + RCLCPP_INFO(_node.get_logger(), "Registered pub/sub topics and versions:"); + for (const auto& [topic_name, version_set] : known_versions) { + if (version_set.empty()) { + continue; + } + const std::string versions = std::accumulate(std::next(version_set.begin()), version_set.end(), + std::to_string(*version_set.begin()), // start with first element + [](std::string a, auto&& b) { + return std::move(a) + ", " + std::to_string(b); + }); + RCLCPP_INFO(_node.get_logger(), "- %s: %s", topic_name.c_str(), versions.c_str()); + } +} + + +void PubSubGraph::handleLargestTopic(const std::unordered_map> &known_versions) { + // FastDDS caches some type information per DDS participant when first creating a publisher or subscriber for a given + // type. The information that is relevant for us is the maximum serialized message size. + // Since different versions can have different sizes, we need to ensure the first publication or subscription + // happens with the version of the largest size. Otherwise, an out-of-memory exception can be triggered. + // And the type must continue to be in use (so we cannot delete it) + for (const auto& [topic_name, versions] : known_versions) { + size_t max_serialized_message_size = 0; + const PublicationFactoryCB* publication_factory_for_max = nullptr; + for (auto version : versions) { + const auto& node = _pub_sub_graph.findNode(MessageIdentifier{topic_name, version}); + assert(node); + const auto& node_data = node.value()->data(); + if (node_data.max_serialized_message_size > max_serialized_message_size) { + max_serialized_message_size = node_data.max_serialized_message_size; + publication_factory_for_max = &node_data.publication_factory; + } + } + if (publication_factory_for_max) { + _largest_topic_publications.emplace_back((*publication_factory_for_max)(_node)); + } + } +} diff --git a/msg/translation_node/src/pub_sub_graph.h b/msg/translation_node/src/pub_sub_graph.h new file mode 100644 index 0000000000..3dcc96fa9e --- /dev/null +++ b/msg/translation_node/src/pub_sub_graph.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include "translations.h" +#include "translation_util.h" +#include "graph.h" + +class PubSubGraph { +public: + struct TopicInfo { + std::string topic_name; ///< fully qualified topic name (with namespace) + int num_subscribers; ///< does not include this node's subscribers + int num_publishers; ///< does not include this node's publishers + }; + + PubSubGraph(rclcpp::Node& node, const TopicTranslations& translations); + + void updateCurrentTopics(const std::vector& topics); + +private: + struct NodeDataPubSub { + explicit NodeDataPubSub(SubscriptionFactoryCB subscription_factory, PublicationFactoryCB publication_factory, + const MessageIdentifier& id, size_t max_serialized_message_size) + : subscription_factory(std::move(subscription_factory)), publication_factory(std::move(publication_factory)), + topic_name(id.topic_name), version(id.version), max_serialized_message_size(max_serialized_message_size) + { } + + const SubscriptionFactoryCB subscription_factory; + const PublicationFactoryCB publication_factory; + const std::string topic_name; + const MessageVersionType version; + const size_t max_serialized_message_size; + + // Keep track if there's currently a publisher/subscriber + bool has_external_publisher{false}; + bool has_external_subscriber{false}; + + rclcpp::SubscriptionBase::SharedPtr subscription; + rclcpp::PublisherBase::SharedPtr publication; + + bool visited{false}; + }; + + void onSubscriptionUpdate(const Graph::MessageNodePtr& node); + void printTopicInfo(const std::unordered_map>& known_versions) const; + void handleLargestTopic(const std::unordered_map>& known_versions); + + rclcpp::Node& _node; + Graph _pub_sub_graph; + std::unordered_map _known_topics_warned; + + std::vector _largest_topic_publications; +}; diff --git a/msg/translation_node/src/service_graph.cpp b/msg/translation_node/src/service_graph.cpp new file mode 100644 index 0000000000..66a389647c --- /dev/null +++ b/msg/translation_node/src/service_graph.cpp @@ -0,0 +1,230 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include "service_graph.h" + +#include + +using namespace std::chrono_literals; + +ServiceGraph::ServiceGraph(rclcpp::Node &node, const ServiceTranslations& translations) + : _node(node) { + + std::unordered_map> known_versions; + + for (const auto& service : translations.nodes()) { + const std::string full_topic_name = getFullTopicName(_node.get_effective_namespace(), service.id.topic_name); + _known_services_warned.insert({full_topic_name, false}); + + const MessageIdentifier id{full_topic_name, service.id.version}; + auto node_data = std::make_shared(service, id); + _request_graph.addNodeIfNotExists(id, node_data, service.message_buffer_request); + _response_graph.addNodeIfNotExists(id, node_data, service.message_buffer_response); + known_versions[full_topic_name].insert(id.version); + } + + auto get_full_topic_names = [this](std::vector ids) { + for (auto& id : ids) { + id.topic_name = getFullTopicName(_node.get_effective_namespace(), id.topic_name); + } + return ids; + }; + + for (const auto& translation : translations.requestTranslations()) { + const std::vector inputs = get_full_topic_names(translation.inputs); + const std::vector outputs = get_full_topic_names(translation.outputs); + _request_graph.addTranslation(translation.cb, inputs, outputs); + } + for (const auto& translation : translations.responseTranslations()) { + const std::vector inputs = get_full_topic_names(translation.inputs); + const std::vector outputs = get_full_topic_names(translation.outputs); + _response_graph.addTranslation(translation.cb, inputs, outputs); + } + + printServiceInfo(known_versions); + handleLargestTopic(known_versions); + + _cleanup_timer = _node.create_wall_timer(10s, [this]() { + cleanupStaleRequests(); + }); +} + +void ServiceGraph::updateCurrentServices(const std::vector &services) { + _request_graph.iterateNodes([](const MessageIdentifier& type, const GraphForService::MessageNodePtr& node) { + node->data()->has_service = false; + node->data()->has_client = false; + node->data()->visited = false; + }); + + for (const auto& info : services) { + const auto [non_versioned_topic_name, version] = getNonVersionedTopicName(info.service_name); + auto maybe_node = _request_graph.findNode({non_versioned_topic_name, version}); + if (!maybe_node) { + auto known_topic_iter = _known_services_warned.find(non_versioned_topic_name); + if (known_topic_iter != _known_services_warned.end() && !known_topic_iter->second) { + RCLCPP_WARN(_node.get_logger(), "No translation available for version %i of service %s", version, non_versioned_topic_name.c_str()); + known_topic_iter->second = true; + } + continue; + } + const auto& node = maybe_node.value(); + + if (info.num_services > 0) { + node->data()->has_service = true; + } + if (info.num_clients > 0) { + node->data()->has_client = true; + } + } + + // Iterate connected graph segments + _request_graph.iterateNodes([this](const MessageIdentifier& type, const GraphForService::MessageNodePtr& node) { + if (node->data()->visited) { + return; + } + node->data()->visited = true; + + // Check if there's a reachable node with a service + int num_services = 0; + + _request_graph.iterateBFS(node, [&](const GraphForService::MessageNodePtr& node) { + if (node->data()->has_service && !node->data()->service) { + ++num_services; + } + }); + + // We need to instantiate a service and clients if there's exactly one external service. + if (num_services > 1 ) { + RCLCPP_ERROR_ONCE(_node.get_logger(), "Found %i services for service '%s', skipping this service", + num_services, node->data()->service_name.c_str()); + } else if (num_services == 1) { + _request_graph.iterateBFS(node, [&](const GraphForService::MessageNodePtr& node) { + node->data()->visited = true; + if (node->data()->has_service && !node->data()->client && !node->data()->service) { + RCLCPP_INFO(_node.get_logger(), "Found service for '%s', version: %i, adding client", node->data()->service_name.c_str(), node->data()->version); + auto tuple = node->data()->client_factory(_node, [this, tmp_node=node](rmw_request_id_t& request) { + onResponse(request, tmp_node); + }); + node->data()->client = std::get<0>(tuple); + node->data()->client_send_cb = std::get<1>(tuple); + + } else if (!node->data()->has_service && !node->data()->service && node->data()->has_client) { + RCLCPP_INFO(_node.get_logger(), "Found client for '%s', version: %i, adding service", node->data()->service_name.c_str(), node->data()->version); + node->data()->service = node->data()->service_factory(_node, [this, tmp_node=node](std::shared_ptr req_id) { + onNewRequest(std::move(req_id), tmp_node); + }); + } + }); + + } else { + // Reset any service or client + _request_graph.iterateBFS(node, [&](const GraphForService::MessageNodePtr& node) { + node->data()->visited = true; + if (node->data()->service) { + RCLCPP_INFO(_node.get_logger(), "Removing service for '%s', version: %i", + node->data()->service_name.c_str(), node->data()->version); + node->data()->service.reset(); + } + if (node->data()->client) { + RCLCPP_INFO(_node.get_logger(), "Removing client for '%s', version: %i", + node->data()->service_name.c_str(), node->data()->version); + node->data()->client.reset(); + } + }); + } + }); +} + +void ServiceGraph::printServiceInfo(const std::unordered_map>& known_versions) const { + // Print info about known versions + RCLCPP_INFO(_node.get_logger(), "Registered services and versions:"); + for (const auto& [topic_name, version_set] : known_versions) { + if (version_set.empty()) { + continue; + } + const std::string versions = std::accumulate(std::next(version_set.begin()), version_set.end(), + std::to_string(*version_set.begin()), // start with first element + [](std::string a, auto&& b) { + return std::move(a) + ", " + std::to_string(b); + }); + RCLCPP_INFO(_node.get_logger(), "- %s: %s", topic_name.c_str(), versions.c_str()); + } +} + +void ServiceGraph::handleLargestTopic(const std::unordered_map> &known_versions) { + // See PubSubGraph::handleLargestTopic for an explanation why this is needed + unsigned index = 0; + for (const auto& [topic_name, versions] : known_versions) { + std::array max_serialized_message_size{0, 0}; + std::array publication_factory_for_max{nullptr, nullptr}; + for (auto version : versions) { + const auto& node = _request_graph.findNode(MessageIdentifier{topic_name, version}); + assert(node); + const auto& node_data = node.value()->data(); + for (unsigned i = 0; i < max_serialized_message_size.size(); ++i) { + if (node_data->max_serialized_message_size[i] > max_serialized_message_size[i]) { + max_serialized_message_size[i] = node_data->max_serialized_message_size[i]; + publication_factory_for_max[i] = &node_data->publication_factory[i]; + } + } + } + for (unsigned i = 0; i < max_serialized_message_size.size(); ++i) { + if (publication_factory_for_max[i]) { + const std::string tmp_topic_name = "dummy_topic" + std::to_string(index++); + _largest_topic_publications.emplace_back((*publication_factory_for_max[i])(_node, tmp_topic_name)); + } + } + } +} + +void ServiceGraph::onNewRequest(std::shared_ptr req_id, GraphForService::MessageNodePtr node) { + bool service_called = false; + _request_graph.translate(node, [this, &service_called, &req_id, original_node=node](const GraphForService::MessageNodePtr& node) { + if (node->data()->client && node->data()->client_send_cb && !service_called) { + service_called = true; + const int64_t client_request_id = node->data()->client_send_cb(node->buffer()); + node->data()->ongoing_requests[client_request_id] = Request{req_id, original_node->data(), _node.now()}; + } + }); +} + +void ServiceGraph::onResponse(rmw_request_id_t &req_id, GraphForService::MessageNodePtr node) { + auto iter = node->data()->ongoing_requests.find(req_id.sequence_number); + if (iter == node->data()->ongoing_requests.end()) { + RCLCPP_ERROR(_node.get_logger(), "Got response with unknown request %li", req_id.sequence_number); + return; + } + bool service_called = false; + auto response_node = _response_graph.findNode({node->data()->service_name, node->data()->version}); + assert(response_node); + _response_graph.translate(response_node.value(), [this, &service_called, &iter](const GraphForService::MessageNodePtr &node) { + if (node->data()->service && !service_called && iter->second.original_node_data == node->data()) { + const rcl_ret_t ret = rcl_send_response(node->data()->service->get_service_handle().get(), + iter->second.original_request_id.get(), node->buffer().get()); + if (ret != RCL_RET_OK) { + RCLCPP_ERROR(_node.get_logger(), "Failed to send response: %s", rcl_get_error_string().str); + } + service_called = true; + } + }); + + node->data()->ongoing_requests.erase(iter); +} + +void ServiceGraph::cleanupStaleRequests() { + static const auto kRequestTimeout = 20s; + _request_graph.iterateNodes([this](const MessageIdentifier& type, const GraphForService::MessageNodePtr& node) { + for (auto it = node->data()->ongoing_requests.begin(); it != node->data()->ongoing_requests.end();) { + const auto& request = it->second; + if (_node.now() - request.timestamp_received > kRequestTimeout) { + RCLCPP_INFO(_node.get_logger(), "Request timed out, dropping ongoing request for '%s', version: %i, request id: %li", + node->data()->service_name.c_str(), node->data()->version, request.original_request_id->sequence_number); + it = node->data()->ongoing_requests.erase(it); + } else { + ++it; + } + } + }); +} diff --git a/msg/translation_node/src/service_graph.h b/msg/translation_node/src/service_graph.h new file mode 100644 index 0000000000..f290bc1f35 --- /dev/null +++ b/msg/translation_node/src/service_graph.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include "translations.h" +#include "translation_util.h" +#include "graph.h" + +class ServiceGraph { +public: + struct ServiceInfo { + std::string service_name; ///< fully qualified service name (with namespace) + int num_services; ///< This can include a service created by the translation node + int num_clients; ///< This can include a client created by the translation node + }; + + ServiceGraph(rclcpp::Node &node, const ServiceTranslations& translations); + + void updateCurrentServices(const std::vector& services); + +private: + struct NodeDataService; + using GraphForService = Graph>; + + void printServiceInfo(const std::unordered_map> &known_versions) const; + void handleLargestTopic(const std::unordered_map>& known_versions); + + void onNewRequest(std::shared_ptr req_id, GraphForService::MessageNodePtr node); + void onResponse(rmw_request_id_t& req_id, GraphForService::MessageNodePtr node); + void cleanupStaleRequests(); + + struct Request { + std::shared_ptr original_request_id; + std::shared_ptr original_node_data{nullptr}; + rclcpp::Time timestamp_received; + }; + struct NodeDataService { + explicit NodeDataService(const Service& service, const MessageIdentifier& id) + : service_factory(service.service_factory), client_factory(service.client_factory), + service_name(id.topic_name), version(id.version), + publication_factory{service.publication_factory_request, service.publication_factory_response}, + max_serialized_message_size{service.max_serialized_message_size_request, service.max_serialized_message_size_response} + { } + + const ServiceFactoryCB service_factory; + const ClientFactoryCB client_factory; + const std::string service_name; + const MessageVersionType version; + const std::array publication_factory; // Request/Response + const std::array max_serialized_message_size; + + // Keep track if there's currently a client/service + bool has_service{false}; + bool has_client{false}; + + rclcpp::ClientBase::SharedPtr client; + ClientSendCB client_send_cb; + rclcpp::ServiceBase::SharedPtr service; + + std::unordered_map ongoing_requests; ///< Ongoing service calls for this node + + bool visited{false}; + }; + + rclcpp::Node& _node; + GraphForService _request_graph; + GraphForService _response_graph; + std::unordered_map _known_services_warned; + rclcpp::TimerBase::SharedPtr _cleanup_timer; + + std::vector _largest_topic_publications; +}; diff --git a/msg/translation_node/src/template_util.h b/msg/translation_node/src/template_util.h new file mode 100644 index 0000000000..8566f34d5e --- /dev/null +++ b/msg/translation_node/src/template_util.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include + +/** + * Helper struct to store template parameter packs + */ +template +struct Pack { +}; + +/** + * Struct for a template parameter pack with access to the individual types + */ +template +struct TypesArray { + template + struct TypeHelper { + using Type = T; + using Next = TypeHelper; + }; + + using Type1 = typename TypeHelper::Type; + using Type2 = typename TypeHelper::Next::Type; + using Type3 = typename TypeHelper::Next::Next::Type; + using Type4 = typename TypeHelper::Next::Next::Next::Type; + using Type5 = typename TypeHelper::Next::Next::Next::Next::Type; + using Type6 = typename TypeHelper::Next::Next::Next::Next::Next::Type; + + using args = Pack; +}; + + +/** + * Helper for call_translation_function() + */ +template +inline void call_translation_function_impl(F f, Pack, Pack, + const std::vector>& messages_in, + std::vector>& messages_out, + std::integer_sequence, std::integer_sequence) +{ + f(*static_cast(messages_in[Is].get())..., *static_cast(messages_out[Os].get())...); +} + +/** + * Call a translation function F which takes the arguments (const ArgsIn&..., ArgsOut&...), + * by passing messages_in and messages_out as arguments. + * Note that sizeof(ArgsIn) == messages_in.length() && sizeof(ArgsOut) == messages_out.length() must hold. + */ +template +inline void call_translation_function(F f, Pack pack_in, Pack pack_out, + const std::vector>& messages_in, + std::vector>& messages_out) { + call_translation_function_impl(f, pack_in, pack_out, messages_in, messages_out, + std::index_sequence_for{}, std::index_sequence_for{}); +} diff --git a/msg/translation_node/src/translation_util.h b/msg/translation_node/src/translation_util.h new file mode 100644 index 0000000000..c5ac4f8153 --- /dev/null +++ b/msg/translation_node/src/translation_util.h @@ -0,0 +1,386 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include "translations.h" +#include "util.h" +#include "template_util.h" + +#include +#include + +class RegisteredTranslations { +public: + + RegisteredTranslations(RegisteredTranslations const&) = delete; + void operator=(RegisteredTranslations const&) = delete; + + + static RegisteredTranslations& instance() { + static RegisteredTranslations instance; + return instance; + } + + /** + * @brief Register a translation class with 1 input and 1 output message. + * + * The translation class has the form: + * + * ``` + * class MyTranslation { + * public: + * using MessageOlder = px4_msgs_old::msg::VehicleAttitudeV2; + * + * using MessageNewer = px4_msgs::msg::VehicleAttitude; + * + * static constexpr const char* kTopic = "fmu/out/vehicle_attitude"; + * + * static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + * // set msg_older from msg_newer + * } + * }; + * ``` + */ + template + void registerDirectTranslation() { + const std::string topic_name = T::kTopic; + _topic_translations.addTopic(getTopicForMessageType(topic_name)); + _topic_translations.addTopic(getTopicForMessageType(topic_name)); + + // Translation callbacks + auto translation_cb_from_older = [](const std::vector& older_msg, std::vector& newer_msg) { + T::fromOlder(*(const typename T::MessageOlder*)older_msg[0].get(), *(typename T::MessageNewer*)newer_msg[0].get()); + }; + auto translation_cb_to_older = [](const std::vector& newer_msg, std::vector& older_msg) { + T::toOlder(*(const typename T::MessageNewer*)newer_msg[0].get(), *(typename T::MessageOlder*)older_msg[0].get()); + }; + _topic_translations.addTranslation({translation_cb_from_older, + {MessageIdentifier{topic_name, T::MessageOlder::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageNewer::MESSAGE_VERSION}}}); + _topic_translations.addTranslation({translation_cb_to_older, + {MessageIdentifier{topic_name, T::MessageNewer::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageOlder::MESSAGE_VERSION}}}); + } + + /** + * @brief Register a translation class for a service. + * + * The translation class has the form: + * + * ``` + * class MyServiceTranslation { + * public: + * using MessageOlder = px4_msgs_old::srv::VehicleCommandV0; + * using MessageNewer = px4_msgs::srv::VehicleCommand; + * + * static constexpr const char* kTopic = "fmu/vehicle_command"; + * + * static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + * // set msg_older from msg_newer + * } + * + * static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + * // set msg_newer from msg_older + * } + * + * static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + * // set msg_older from msg_newer + * } + * }; + * ``` + */ + template + void registerServiceDirectTranslation() { + const std::string topic_name = T::kTopic; + _service_translations.addNode(getServiceForMessageType(topic_name)); + _service_translations.addNode(getServiceForMessageType(topic_name)); + // Add translations + { // Request + auto translation_cb_from_older = [](const std::vector &older_msg, + std::vector &newer_msg) { + T::fromOlder(*(const typename T::MessageOlder::Request *) older_msg[0].get(), + *(typename T::MessageNewer::Request *) newer_msg[0].get()); + }; + auto translation_cb_to_older = [](const std::vector &newer_msg, + std::vector &older_msg) { + T::toOlder(*(const typename T::MessageNewer::Request *) newer_msg[0].get(), + *(typename T::MessageOlder::Request *) older_msg[0].get()); + }; + _service_translations.addRequestTranslation({translation_cb_from_older, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}}); + _service_translations.addRequestTranslation({translation_cb_to_older, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}}); + } + { // Response + auto translation_cb_from_older = [](const std::vector &older_msg, + std::vector &newer_msg) { + T::fromOlder(*(const typename T::MessageOlder::Response *) older_msg[0].get(), + *(typename T::MessageNewer::Response *) newer_msg[0].get()); + }; + auto translation_cb_to_older = [](const std::vector &newer_msg, + std::vector &older_msg) { + T::toOlder(*(const typename T::MessageNewer::Response *) newer_msg[0].get(), + *(typename T::MessageOlder::Response *) older_msg[0].get()); + }; + _service_translations.addResponseTranslation({translation_cb_from_older, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}}); + _service_translations.addResponseTranslation({translation_cb_to_older, + {MessageIdentifier{topic_name, T::MessageNewer::Request::MESSAGE_VERSION}}, + {MessageIdentifier{topic_name, T::MessageOlder::Request::MESSAGE_VERSION}}}); + } + + } + + /** + * @brief Register a translation class with N input and M output messages. + * + * The translation class has the form: + * ``` + * class MyTranslation { + * public: + * using MessagesOlder = TypesArray; + * static constexpr const char* kTopicsOlder[] = { + * "fmu/out/vehicle_global_position", + * "fmu/out/vehicle_local_position", + * ... + * }; + * + * using MessagesNewer = TypesArray; + * static constexpr const char* kTopicsNewer[] = { + * "fmu/out/vehicle_global_position", + * "fmu/out/vehicle_local_position", + * ... + * }; + * + * static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, ... + * MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2, ...) { + * // Set msg_newerX from msg_olderX + * } + * + * static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, ... + * MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2, ...) { + * // Set msg_olderX from msg_newerX + * } + * }; + * ``` + */ + template + void registerTranslation() { + const auto topics_older = getTopicsForMessageType(typename T::MessagesOlder::args(), T::kTopicsOlder); + std::vector topics_older_identifiers; + for (const auto& topic : topics_older) { + _topic_translations.addTopic(topic); + topics_older_identifiers.emplace_back(topic.id); + } + const auto topics_newer = getTopicsForMessageType(typename T::MessagesNewer::args(),T::kTopicsNewer); + std::vector topics_newer_identifiers; + for (const auto& topic : topics_newer) { + _topic_translations.addTopic(topic); + topics_newer_identifiers.emplace_back(topic.id); + } + + // Translation callbacks + const auto translation_cb_from_older = [](const std::vector& older_msgs, std::vector& newer_msgs) { + call_translation_function(&T::fromOlder, typename T::MessagesOlder::args(), typename T::MessagesNewer::args(), older_msgs, newer_msgs); + }; + const auto translation_cb_to_older = [](const std::vector& newer_msgs, std::vector& older_msgs) { + call_translation_function(&T::toOlder, typename T::MessagesNewer::args(), typename T::MessagesOlder::args(), newer_msgs, older_msgs); + }; + { + // Older -> Newer + Translation translation; + translation.cb = translation_cb_from_older; + translation.inputs = topics_older_identifiers; + translation.outputs = topics_newer_identifiers; + _topic_translations.addTranslation(std::move(translation)); + } + { + // Newer -> Older + Translation translation; + translation.cb = translation_cb_to_older; + translation.inputs = topics_newer_identifiers; + translation.outputs = topics_older_identifiers; + _topic_translations.addTranslation(std::move(translation)); + } + } + + const TopicTranslations& topicTranslations() const { return _topic_translations; } + const ServiceTranslations& serviceTranslations() const { return _service_translations; } + +protected: + RegisteredTranslations() = default; +private: + template + static size_t getMaxSerializedMessageSize() { + const auto type_handle = rclcpp::get_message_type_support_handle(); + const auto fastrtps_handle = rosidl_typesupport_cpp::get_message_typesupport_handle_function(&type_handle, "rosidl_typesupport_fastrtps_cpp"); + if (fastrtps_handle) { + const auto *callbacks = static_cast(fastrtps_handle->data); + char bound_info; + return callbacks->max_serialized_size(bound_info); + } + return 0; + } + + template + static Topic getTopicForMessageType(const std::string& topic_name) { + Topic ret{}; + ret.id.topic_name = topic_name; + ret.id.version = RosMessageType::MESSAGE_VERSION; + auto message_buffer = std::make_shared(); + ret.message_buffer = std::static_pointer_cast(message_buffer); + + // Subscription/Publication factory methods + const std::string topic_name_versioned = getVersionedTopicName(topic_name, ret.id.version); + ret.subscription_factory = [topic_name_versioned, message_buffer](rclcpp::Node& node, + const std::function& on_topic_cb) -> rclcpp::SubscriptionBase::SharedPtr { + return std::dynamic_pointer_cast( + // Note: template instantiation of subscriptions slows down compilation considerably, see + // https://github.com/ros2/rclcpp/issues/1949 + node.create_subscription(topic_name_versioned, rclcpp::QoS(1).best_effort(), + [on_topic_cb=on_topic_cb, message_buffer](typename RosMessageType::UniquePtr msg) -> void { + *message_buffer = *msg; + on_topic_cb(); + })); + }; + ret.publication_factory = [topic_name_versioned](rclcpp::Node& node) -> rclcpp::PublisherBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_publisher(topic_name_versioned, rclcpp::QoS(1).best_effort())); + }; + + ret.max_serialized_message_size = getMaxSerializedMessageSize(); + + return ret; + } + + template + static Service getServiceForMessageType(const std::string& topic_name) { + Service ret{}; + ret.id.topic_name = topic_name; + ret.id.version = RosMessageType::Request::MESSAGE_VERSION; + auto message_buffer_request = std::make_shared(); + ret.message_buffer_request = std::static_pointer_cast(message_buffer_request); + auto message_buffer_response = std::make_shared(); + ret.message_buffer_response = std::static_pointer_cast(message_buffer_response); + + // Service/client factory methods + const std::string topic_name_versioned = getVersionedTopicName(topic_name, ret.id.version); + ret.service_factory = [topic_name_versioned, message_buffer_request](rclcpp::Node& node, + const std::function req_id)>& on_request_cb) -> rclcpp::ServiceBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_service(topic_name_versioned, + [on_request_cb=on_request_cb, message_buffer_request]( + typename rclcpp::Service::SharedPtr service, + std::shared_ptr req_id, + const std::shared_ptr request + ) -> void { + *message_buffer_request = *request; + on_request_cb(std::move(req_id)); + })); + }; + ret.client_factory = [topic_name_versioned, message_buffer_response](rclcpp::Node& node, + const std::function& on_response_cb) { + auto client = node.create_client(topic_name_versioned); + client->set_on_new_response_callback([client, message_buffer_response, on_response_cb](size_t num) { + for (size_t i = 0; i < num; i++) { + rmw_request_id_t request_id{}; + if (client->take_response(*message_buffer_response, request_id)) { + on_response_cb(request_id); + } + } + }); + const auto send_request = [client](MessageBuffer request) { + auto result = client->async_send_request(std::static_pointer_cast(request)); + // We don't need the client to keep track of ongoing requests, so we remove it right away + // to prevent leaks + client->remove_pending_request(result.request_id); + return result.request_id; + }; + return std::make_tuple(std::dynamic_pointer_cast(client), send_request); + }; + + ret.publication_factory_request = [](rclcpp::Node& node, const std::string& topic_name) -> rclcpp::PublisherBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_publisher( + topic_name,rclcpp::QoS(1).best_effort().avoid_ros_namespace_conventions(true))); + }; + ret.publication_factory_response = [](rclcpp::Node& node, const std::string& topic_name) -> rclcpp::PublisherBase::SharedPtr { + return std::dynamic_pointer_cast( + node.create_publisher( + topic_name,rclcpp::QoS(1).best_effort().avoid_ros_namespace_conventions(true))); + }; + + ret.max_serialized_message_size_request = getMaxSerializedMessageSize(); + ret.max_serialized_message_size_response = getMaxSerializedMessageSize(); + + return ret; + } + + template + static std::vector getTopicsForMessageTypeImpl(const char* const topics[], std::integer_sequence) { + std::vector ret { + getTopicForMessageType(topics[Is])... + }; + return ret; + } + + template + static std::vector getTopicsForMessageType(Pack, const char* const (&topics)[N]) { + static_assert(N == sizeof...(RosMessageTypes), "Number of topics does not match number of message types"); + return getTopicsForMessageTypeImpl(topics, std::index_sequence_for{}); + } + + TopicTranslations _topic_translations; + ServiceTranslations _service_translations; +}; + +template +class RegistrationHelperDirect { +public: + explicit RegistrationHelperDirect(const char* dummy) { + // There's something strange: when there is no argument passed, the + // compiler removes the static object completely. I don't know + // why but this dummy variable prevents that. + (void)dummy; + RegisteredTranslations::instance().registerDirectTranslation(); + } + explicit RegistrationHelperDirect(const char* dummy, bool for_service) { + (void)dummy; + RegisteredTranslations::instance().registerServiceDirectTranslation(); + } + RegistrationHelperDirect(RegistrationHelperDirect const&) = delete; + void operator=(RegistrationHelperDirect const&) = delete; +}; + +#define REGISTER_TOPIC_TRANSLATION_DIRECT(class_name) \ + RegistrationHelperDirect class_name##_registration_direct("dummy"); + +#define REGISTER_SERVICE_TRANSLATION_DIRECT(class_name) \ + RegistrationHelperDirect class_name##_service_registration_direct("dummy", true); + +template +class TopicRegistrationHelperGeneric { +public: + explicit TopicRegistrationHelperGeneric(const char* dummy) { + (void)dummy; + RegisteredTranslations::instance().registerTranslation(); + } + TopicRegistrationHelperGeneric(TopicRegistrationHelperGeneric const&) = delete; + void operator=(TopicRegistrationHelperGeneric const&) = delete; +}; + +#define REGISTER_TOPIC_TRANSLATION(class_name) \ + TopicRegistrationHelperGeneric class_name##_registration_generic("dummy"); diff --git a/msg/translation_node/src/translations.cpp b/msg/translation_node/src/translations.cpp new file mode 100644 index 0000000000..0b7d8b0c93 --- /dev/null +++ b/msg/translation_node/src/translations.cpp @@ -0,0 +1,5 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#include "translations.h" diff --git a/msg/translation_node/src/translations.h b/msg/translation_node/src/translations.h new file mode 100644 index 0000000000..3982d6a1c0 --- /dev/null +++ b/msg/translation_node/src/translations.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" +#include "graph.h" + +#include + + +using TranslationCB = std::function&, std::vector&)>; +using SubscriptionFactoryCB = std::function& on_topic_cb)>; +using PublicationFactoryCB = std::function; +using NamedPublicationFactoryCB = std::function; +using ServiceFactoryCB = std::function req_id)>& on_request_cb)>; +using ClientSendCB = std::function; +using ClientFactoryCB = std::function(rclcpp::Node&, const std::function& on_response_cb)>; + +struct Topic { + MessageIdentifier id; + + SubscriptionFactoryCB subscription_factory; + PublicationFactoryCB publication_factory; + + std::shared_ptr message_buffer; + size_t max_serialized_message_size{}; +}; + +struct Service { + MessageIdentifier id; + + ServiceFactoryCB service_factory; + ClientFactoryCB client_factory; + + NamedPublicationFactoryCB publication_factory_request; + NamedPublicationFactoryCB publication_factory_response; + + std::shared_ptr message_buffer_request; + size_t max_serialized_message_size_request{}; + + std::shared_ptr message_buffer_response; + size_t max_serialized_message_size_response{}; +}; + +struct Translation { + TranslationCB cb; + std::vector inputs; + std::vector outputs; +}; + +class TopicTranslations { +public: + TopicTranslations() = default; + + void addTopic(Topic topic) { _topics.push_back(std::move(topic)); } + void addTranslation(Translation translation) { _translations.push_back(std::move(translation)); } + + const std::vector& topics() const { return _topics; } + const std::vector& translations() const { return _translations; } +private: + std::vector _topics; + std::vector _translations; +}; + +class ServiceTranslations { +public: + ServiceTranslations() = default; + + void addNode(Service node) { _nodes.push_back(std::move(node)); } + void addRequestTranslation(Translation translation) { _request_translations.push_back(std::move(translation)); } + void addResponseTranslation(Translation translation) { _response_translations.push_back(std::move(translation)); } + + const std::vector& nodes() const { return _nodes; } + const std::vector& requestTranslations() const { return _request_translations; } + const std::vector& responseTranslations() const { return _response_translations; } +private: + std::vector _nodes; + std::vector _request_translations; + std::vector _response_translations; +}; diff --git a/msg/translation_node/src/util.h b/msg/translation_node/src/util.h new file mode 100644 index 0000000000..8d584598e9 --- /dev/null +++ b/msg/translation_node/src/util.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include +#include + +using MessageVersionType = uint32_t; + +static inline std::string getVersionedTopicName(const std::string& topic_name, MessageVersionType version) { + // version == 0 can be used to transition from non-versioned topics to versioned ones + if (version == 0) { + return topic_name; + } + return topic_name + "_v" + std::to_string(version); +} + +static inline std::pair getNonVersionedTopicName(const std::string& topic_name) { + // topic name has the form _v, or just (with version=0) + auto pos = topic_name.find_last_of("_v"); + // Ensure there's at least one more char after the found string + if (pos == std::string::npos || pos + 2 > topic_name.length()) { + return std::make_pair(topic_name, 0); + } + std::string non_versioned_topic_name = topic_name.substr(0, pos - 1); + std::string version = topic_name.substr(pos + 1); + // Ensure only digits are in the version string + for (char c : version) { + if (!std::isdigit(c)) { + return std::make_pair(topic_name, 0); + } + } + return std::make_pair(non_versioned_topic_name, std::stol(version)); +} + +/** + * Get the full topic name, including namespace from a topic name. + * namespace_name should be set to Node::get_effective_namespace() + */ +static inline std::string getFullTopicName(const std::string& namespace_name, const std::string& topic_name) { + std::string full_topic_name = topic_name; + if (!full_topic_name.empty() && full_topic_name[0] != '/') { + if (namespace_name.empty() || namespace_name.back() != '/') { + full_topic_name = '/' + full_topic_name; + } + full_topic_name = namespace_name + full_topic_name; + } + return full_topic_name; +} diff --git a/msg/translation_node/test/graph.cpp b/msg/translation_node/test/graph.cpp new file mode 100644 index 0000000000..e0d1f360cb --- /dev/null +++ b/msg/translation_node/test/graph.cpp @@ -0,0 +1,623 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include + + +TEST(graph, basic) +{ + struct NodeData { + bool iterated{false}; + bool translated{false}; + }; + Graph graph; + + const int32_t message1_value = 3; + const int32_t offset = 4; + + // Add 2 nodes + const MessageIdentifier id1{"topic_name", 1}; + auto buffer1 = std::make_shared(); + *buffer1 = message1_value; + EXPECT_TRUE(graph.addNodeIfNotExists(id1, {}, buffer1)); + EXPECT_FALSE(graph.addNodeIfNotExists(id1, {}, std::make_shared())); + const MessageIdentifier id2{"topic_name", 4}; + auto buffer2 = std::make_shared(); + *buffer2 = 773; + EXPECT_TRUE(graph.addNodeIfNotExists(id2, {}, buffer2)); + + // Search nodes + EXPECT_TRUE(graph.findNode(id1).has_value()); + EXPECT_TRUE(graph.findNode(id2).has_value()); + + // Add 1 translation + auto translation_cb = [&offset](const std::vector& a, std::vector& b) { + auto a_value = static_cast(a[0].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value + offset; + }; + graph.addTranslation(translation_cb, {id1}, {id2}); + + // Iteration from id1 must reach id2 + auto node1 = graph.findNode(id1).value(); + auto node2 = graph.findNode(id2).value(); + auto iterate_cb = [](const Graph::MessageNodePtr& node) { + node->data().iterated = true; + }; + graph.iterateBFS(node1, iterate_cb); + EXPECT_TRUE(node1->data().iterated); + EXPECT_TRUE(node2->data().iterated); + node1->data().iterated = false; + node2->data().iterated = false; + + // Iteration from id2 must not reach id1 + graph.iterateBFS(node2, iterate_cb); + EXPECT_FALSE(node1->data().iterated); + EXPECT_TRUE(node2->data().iterated); + + // Test translation + graph.translate(node1, + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + EXPECT_FALSE(node1->data().translated); + EXPECT_TRUE(node2->data().translated); + EXPECT_EQ(*buffer1, message1_value); + EXPECT_EQ(*buffer2, message1_value + offset); +} + + +TEST(graph, multi_path) +{ + // Multiple paths with cycles + struct NodeData { + unsigned iterated_idx{0}; + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 6; + std::array ids{{ + {"topic_name", 1}, + {"topic_name", 2}, + {"topic_name", 3}, + {"topic_name", 4}, + {"topic_name", 5}, + {"topic_name", 6}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + // Translations + std::bitset<32> translated; + + auto get_translation_cb = [&translated](unsigned bit) { + auto translation_cb = [&translated, bit](const std::vector &a, std::vector &b) { + auto a_value = static_cast(a[0].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value | (1 << bit); + translated.set(bit); + }; + return translation_cb; + }; + + // Graph: + // ___ 2 -- 3 -- 4 + // | | + // 1 _______| + // | + // 5 + // | + // 6 + + unsigned next_bit = 0; + // Connect each node to the previous and next, except the last 3 + for (unsigned i=0; i < num_nodes - 3; ++i) { + graph.addTranslation(get_translation_cb(next_bit++), {ids[i]}, {ids[i+1]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[i+1]}, {ids[i]}); + } + + // Connect the first to the 3rd as well + graph.addTranslation(get_translation_cb(next_bit++), {ids[0]}, {ids[2]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[2]}, {ids[0]}); + + // Connect the second last to the first one + graph.addTranslation(get_translation_cb(next_bit++), {ids[0]}, {ids[num_nodes-2]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-2]}, {ids[0]}); + + // Connect the second last to the last one + graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-1]}, {ids[num_nodes-2]}); + graph.addTranslation(get_translation_cb(next_bit++), {ids[num_nodes-2]}, {ids[num_nodes-1]}); + + unsigned iteration_idx = 1; + graph.iterateBFS(graph.findNode(ids[0]).value(), [&iteration_idx](const Graph::MessageNodePtr& node) { + assert(node->data().iterated_idx == 0); + node->data().iterated_idx = iteration_idx++; + }); + + EXPECT_EQ(graph.findNode(ids[0]).value()->data().iterated_idx, 1); + // We're a bit stricter than we would have to be: ids[1,2,4] would be allowed to have any of the values (2,3,4) + EXPECT_EQ(graph.findNode(ids[1]).value()->data().iterated_idx, 2); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().iterated_idx, 3); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().iterated_idx, 4); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().iterated_idx, 5); + + + // Translation + graph.translate(graph.findNode(ids[0]).value(), + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + + // All nodes should be translated except the first + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + for (unsigned i = 1; i < num_nodes; ++i) { + EXPECT_EQ(graph.findNode(ids[i]).value()->data().translated, true) << "node[" << i << "]"; + } + + // Ensure the correct edges were used for translations + EXPECT_EQ("00000000000000000000100101010001", translated.to_string()); + + // Ensure correct translation path taken for each node (which is stored in the buffers), + // and translation callback got called + EXPECT_EQ(*buffers[0], 0); + EXPECT_EQ(*buffers[1], 0b1); + EXPECT_EQ(*buffers[2], 0b1000000); + EXPECT_EQ(*buffers[3], 0b1010000); + EXPECT_EQ(*buffers[4], 0b100000000); + EXPECT_EQ(*buffers[5], 0b100100000000); + + for (unsigned i=0; i < num_nodes; ++i) { + printf("node[%i]: translated: %i, buffer: %i\n", i, graph.findNode(ids[i]).value()->data().translated, + *buffers[i]); + } +} + +TEST(graph, multi_links) { + // Multiple topics (merging / splitting) + struct NodeData { + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 6; + std::array ids{{ + {"topic1", 1}, + {"topic2", 1}, + {"topic1", 2}, + {"topic3", 1}, + {"topic4", 1}, + {"topic1", 3}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + + // Graph + // ___ + // 1 - | | --- + // | | - 3 - | | - 6 + // 2 - | | --- + // | --- + // | ___ + // --- | | - 4 + // | | - 5 + // --- + + // Translations + auto translation_cb_merge = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 1); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value1 | *a_value2; + }; + auto translation_cb_split = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 2); + auto a_value = static_cast(a[0].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value & 0x0000ffffu; + *b_value2 = *a_value & 0xffff0000u; + }; + auto translation_cb_direct = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 1); + auto a_value = static_cast(a[0].get()); + auto b_value = static_cast(b[0].get()); + *b_value = *a_value; + }; + + auto addTranslation = [&](const std::vector& inputs, const std::vector& outputs) { + assert(inputs.size() <= 2); + assert(outputs.size() <= 2); + if (inputs.size() == 1) { + if (outputs.size() == 1) { + graph.addTranslation(translation_cb_direct, inputs, outputs); + graph.addTranslation(translation_cb_direct, outputs, inputs); + } else { + graph.addTranslation(translation_cb_split, inputs, outputs); + graph.addTranslation(translation_cb_merge, outputs, inputs); + } + } else { + assert(outputs.size() == 1); + graph.addTranslation(translation_cb_merge, inputs, outputs); + graph.addTranslation(translation_cb_split, outputs, inputs); + } + }; + addTranslation({ids[0], ids[1]}, {ids[2]}); + addTranslation({ids[1]}, {ids[3], ids[4]}); + addTranslation({ids[2]}, {ids[5]}); + + auto translate_node = [&](const MessageIdentifier& id) { + graph.translate(graph.findNode(id).value(), + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + + }; + auto reset_translated = [&]() { + for (const auto& id : ids) { + graph.findNode(id).value()->data().translated = false; + } + }; + + // Updating node 2 should trigger an output for nodes 4+5 (splitting) + *buffers[0] = 0xa00000b0; + *buffers[1] = 0x0f00000f; + translate_node(ids[1]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + EXPECT_EQ(*buffers[3], 0x0000000f); + EXPECT_EQ(*buffers[4], 0x0f000000); + + reset_translated(); + + // Now updating node 1 should update nodes 3+6 (merging, both inputs available now) + translate_node(ids[0]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true); + EXPECT_EQ(*buffers[2], 0xaf0000bf); + EXPECT_EQ(*buffers[5], 0xaf0000bf); + + reset_translated(); + + // Another update must not trigger any other updates + translate_node(ids[0]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + + reset_translated(); + + // Backwards: updating node 6 should trigger updates for 1+2, but also 4+5 + *buffers[5] = 0xc00000d0; + translate_node(ids[5]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + EXPECT_EQ(*buffers[0], 0x000000d0); + EXPECT_EQ(*buffers[1], 0xc0000000); + EXPECT_EQ(*buffers[2], 0xc00000d0); + EXPECT_EQ(*buffers[3], 0); + EXPECT_EQ(*buffers[4], 0xc0000000); + EXPECT_EQ(*buffers[5], 0xc00000d0); +} + +TEST(graph, multi_links2) { + // Multiple topics (merging / splitting) + struct NodeData { + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 8; + std::array ids{{ + {"topic1", 1}, + {"topic2", 1}, + {"topic3", 1}, + {"topic1", 2}, + {"topic2", 2}, + {"topic1", 3}, + {"topic2", 3}, + {"topic3", 3}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + + // Graph + // ___ ___ + // 1 - | | | | - 6 + // | | - 4 - | | + // 2 - | | | | - 7 + // | | - 5 - | | + // 3 - | | | | - 8 + // --- --- + + // Translations + auto translation_cb_32 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 3); + assert(b.size() == 2); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto a_value3 = static_cast(a[2].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value1 | *a_value2; + *b_value2 = *a_value3; + }; + auto translation_cb_23 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 3); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + auto b_value3 = static_cast(b[2].get()); + *b_value1 = *a_value1 & 0x0000ffffu; + *b_value2 = *a_value1 & 0xffff0000u; + *b_value3 = *a_value2; + }; + graph.addTranslation(translation_cb_32, {ids[0], ids[1], ids[2]}, {ids[3], ids[4]}); + graph.addTranslation(translation_cb_23, {ids[3], ids[4]}, {ids[0], ids[1], ids[2]}); + + graph.addTranslation(translation_cb_23, {ids[3], ids[4]}, {ids[5], ids[6], ids[7]}); + graph.addTranslation(translation_cb_32, {ids[5], ids[6], ids[7]}, {ids[3], ids[4]}); + + + auto translate_node = [&](const MessageIdentifier& id) { + graph.translate(graph.findNode(id).value(), + [](auto&& node) { + assert(!node->data().translated); + node->data().translated = true; + }); + }; + auto reset_translated = [&]() { + for (const auto& id : ids) { + graph.findNode(id).value()->data().translated = false; + } + }; + + // Updating nodes 1+2+3 should update nodes 6+7+8 + *buffers[0] = 0xa00000b0; + *buffers[1] = 0x0f00000f; + *buffers[2] = 0x0c00000c; + translate_node(ids[1]); + translate_node(ids[0]); + translate_node(ids[2]); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[7]).value()->data().translated, true); + EXPECT_EQ(*buffers[3], 0xa00000b0 | 0x0f00000f); + EXPECT_EQ(*buffers[4], 0x0c00000c); + EXPECT_EQ(*buffers[5], (0xa00000b0 | 0x0f00000f) & 0x0000ffffu); + EXPECT_EQ(*buffers[6], (0xa00000b0 | 0x0f00000f) & 0xffff0000u); + EXPECT_EQ(*buffers[7], 0x0c00000c); + + reset_translated(); + + // Now updating nodes 6+7+8 should update nodes 1+2+3 + *buffers[5] = 0xa00000b0; + *buffers[6] = 0x0f00000f; + *buffers[7] = 0x0c00000c; + translate_node(ids[5]); + translate_node(ids[6]); + translate_node(ids[7]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(*buffers[3], 0xa00000b0 | 0x0f00000f); + EXPECT_EQ(*buffers[4], 0x0c00000c); + EXPECT_EQ(*buffers[0], (0xa00000b0 | 0x0f00000f) & 0x0000ffffu); + EXPECT_EQ(*buffers[1], (0xa00000b0 | 0x0f00000f) & 0xffff0000u); + EXPECT_EQ(*buffers[2], 0x0c00000c); +} + +TEST(graph, multi_links3) { + // Multiple topics (cannot use the shortest path) + struct NodeData { + bool translated{false}; + }; + Graph graph; + + static constexpr unsigned num_nodes = 7; + std::array ids{{ + {"topic1", 1}, + {"topic2", 1}, + {"topic1", 2}, + {"topic1", 3}, + {"topic1", 4}, + {"topic2", 4}, + {"topic1", 5}, + }}; + + std::array, num_nodes> buffers{{ + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + std::make_shared(), + }}; + + // Nodes + for (unsigned i = 0; i < num_nodes; ++i) { + EXPECT_TRUE(graph.addNodeIfNotExists(ids[i], {}, buffers[i])); + } + + + // Graph + // ___ ___ ___ ___ + // 1 - | | - 3 - | | - 4 - | | - 5 - | | - 7 + // | | --- --- | | + // | | | | + // 2 - | | --------------------- 6 - | | + // --- --- + + // Translations + auto translation_cb_21 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 1); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value1 = static_cast(b[0].get()); + *b_value1 = *a_value1 | *a_value2; + }; + auto translation_cb_22 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 2); + assert(b.size() == 2); + auto a_value1 = static_cast(a[0].get()); + auto a_value2 = static_cast(a[1].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value1; + *b_value2 = *a_value2; + }; + auto translation_cb_12 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 2); + auto a_value1 = static_cast(a[0].get()); + auto b_value1 = static_cast(b[0].get()); + auto b_value2 = static_cast(b[1].get()); + *b_value1 = *a_value1 & 0x0000ffffu; + *b_value2 = *a_value1 & 0xffff0000u; + }; + auto translation_cb_11 = [](const std::vector &a, std::vector &b) { + assert(a.size() == 1); + assert(b.size() == 1); + auto a_value1 = static_cast(a[0].get()); + auto b_value1 = static_cast(b[0].get()); + *b_value1 = *a_value1 + 1; + }; + graph.addTranslation(translation_cb_22, {ids[0], ids[1]}, {ids[2], ids[5]}); + graph.addTranslation(translation_cb_22, {ids[2], ids[5]}, {ids[0], ids[1]}); + graph.addTranslation(translation_cb_11, {ids[2]}, {ids[3]}); + graph.addTranslation(translation_cb_11, {ids[3]}, {ids[2]}); + graph.addTranslation(translation_cb_11, {ids[3]}, {ids[4]}); + graph.addTranslation(translation_cb_11, {ids[4]}, {ids[3]}); + graph.addTranslation(translation_cb_21, {ids[4], ids[5]}, {ids[6]}); + graph.addTranslation(translation_cb_12, {ids[6]}, {ids[4], ids[5]}); + + + auto translate_node = [&](const MessageIdentifier& id) { + graph.translate(graph.findNode(id).value(), + [](auto&& node) { + assert(!node->data().translated); + assert(!node->data().translated); + node->data().translated = true; + }); + }; + auto reset_translated = [&]() { + for (const auto& id : ids) { + graph.findNode(id).value()->data().translated = false; + } + }; + + // Updating nodes 1+2 should update node 7 + *buffers[0] = 0xa00000b0; + *buffers[1] = 0x0a00000b; + translate_node(ids[1]); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, false); + translate_node(ids[0]); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[3]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[5]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true); + EXPECT_EQ(*buffers[2], 0xa00000b0); + EXPECT_EQ(*buffers[3], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[4], 0xa00000b0 + 2); + EXPECT_EQ(*buffers[5], 0x0a00000b); + EXPECT_EQ(*buffers[6], ((0xa00000b0 + 2) | 0x0a00000b)); + + reset_translated(); + + // Now updating nodes 4+6 should update the rest + *buffers[3] = 0xa00000b0; + *buffers[5] = 0x0f00000f; + translate_node(ids[3]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, false); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, false); + translate_node(ids[5]); + EXPECT_EQ(graph.findNode(ids[0]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[1]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[2]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[4]).value()->data().translated, true); + EXPECT_EQ(graph.findNode(ids[6]).value()->data().translated, true); + EXPECT_EQ(*buffers[0], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[1], 0x0f00000f); + EXPECT_EQ(*buffers[2], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[4], 0xa00000b0 + 1); + EXPECT_EQ(*buffers[6], (0xa00000b0 + 1) | 0x0f00000f); +} diff --git a/msg/translation_node/test/main.cpp b/msg/translation_node/test/main.cpp new file mode 100644 index 0000000000..9d7e484c70 --- /dev/null +++ b/msg/translation_node/test/main.cpp @@ -0,0 +1,16 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + const int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/msg/translation_node/test/pub_sub.cpp b/msg/translation_node/test/pub_sub.cpp new file mode 100644 index 0000000000..3ccd02a9b8 --- /dev/null +++ b/msg/translation_node/test/pub_sub.cpp @@ -0,0 +1,350 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include +#include +#include +using namespace std::chrono_literals; + +// Define a custom struct with MESSAGE_VERSION field that can be used in ROS pubs and subs +#define DEFINE_VERSIONED_ROS_MESSAGE_TYPE(CUSTOM_TYPE_NAME, ROS_TYPE_NAME, THIS_MESSAGE_VERSION) \ + struct CUSTOM_TYPE_NAME : public ROS_TYPE_NAME { \ + CUSTOM_TYPE_NAME() = default; \ + CUSTOM_TYPE_NAME(const ROS_TYPE_NAME& msg) : ROS_TYPE_NAME(msg) {} \ + static constexpr uint32_t MESSAGE_VERSION = THIS_MESSAGE_VERSION; \ + }; \ + template<> \ + struct rclcpp::TypeAdapter \ + { \ + using is_specialized = std::true_type; \ + using custom_type = CUSTOM_TYPE_NAME; \ + using ros_message_type = ROS_TYPE_NAME; \ + static void convert_to_ros_message(const custom_type & source, ros_message_type & destination) \ + { \ + destination = source; \ + } \ + static void convert_to_custom(const ros_message_type & source, custom_type & destination) \ + { \ + destination = source; \ + } \ + }; \ + RCLCPP_USING_CUSTOM_TYPE_AS_ROS_MESSAGE_TYPE(CUSTOM_TYPE_NAME, ROS_TYPE_NAME); + +class PubSubGraphTest : public testing::Test +{ +protected: + void SetUp() override + { + _test_node = std::make_shared("test_node"); + _app_node = std::make_shared("app_node"); + _executor.add_node(_test_node); + _executor.add_node(_app_node); + + for (auto& node : {_app_node, _test_node}) { + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + } + + bool spinWithTimeout(const std::function& predicate) { + const auto start = _app_node->now(); + while (_app_node->now() - start < 5s) { + _executor.spin_some(); + if (predicate()) { + return true; + } + } + return false; + } + + std::shared_ptr _test_node; + std::shared_ptr _app_node; + rclcpp::executors::SingleThreadedExecutor _executor; +}; + +class RegisteredTranslationsTest : public RegisteredTranslations { +public: + RegisteredTranslationsTest() = default; +}; + + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(Float32Versioned, std_msgs::msg::Float32, 1u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(ColorRGBAVersioned, std_msgs::msg::ColorRGBA, 2u); + +class DirectTranslationTest { +public: + using MessageOlder = Float32Versioned; + using MessageNewer = ColorRGBAVersioned; + + static constexpr const char* kTopic = "test/direct_translation"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + msg_newer.r = 1.f; + msg_newer.g = msg_older.data; + msg_newer.b = 2.f; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + msg_older.data = msg_newer.r + msg_newer.g + msg_newer.b; + } +}; + + +TEST_F(PubSubGraphTest, DirectTranslation) +{ + RegisteredTranslationsTest registered_translations; + registered_translations.registerDirectTranslation(); + + PubSubGraph graph(*_test_node, registered_translations.topicTranslations()); + Monitor monitor(*_test_node, &graph, nullptr); + + const std::string topic_name = DirectTranslationTest::kTopic; + const std::string topic_name_older_version = getVersionedTopicName(topic_name, DirectTranslationTest::MessageOlder::MESSAGE_VERSION); + const std::string topic_name_newer_version = getVersionedTopicName(topic_name, DirectTranslationTest::MessageNewer::MESSAGE_VERSION); + + { + // Create publisher + subscriber + int num_topic_updates = 0; + DirectTranslationTest::MessageNewer latest_data{}; + auto publisher = _app_node->create_publisher(topic_name_older_version, + rclcpp::QoS(1).best_effort()); + auto subscriber = _app_node->create_subscription(topic_name_newer_version, + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data, this]( + DirectTranslationTest::MessageNewer::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated: %.3f", (double) msg->g); + latest_data = *msg; + ++num_topic_updates; + }); + + monitor.updateNow(); + + // Wait until there is a subscriber & publisher + ASSERT_TRUE(spinWithTimeout([&subscriber, &publisher]() { + return subscriber->get_publisher_count() > 0 && publisher->get_subscription_count() > 0; + })) << "Timeout, no publisher/subscriber found"; + + // Publish some data & wait for it to arrive + for (int i = 0; i < 10; ++i) { + DirectTranslationTest::MessageOlder msg_older; + msg_older.data = (float) i; + publisher->publish(msg_older); + + ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() { + return num_topic_updates == i + 1; + })) << "Timeout, topic update not received, i=" << i; + + // Check data + EXPECT_FLOAT_EQ(latest_data.r, 1.f); + EXPECT_FLOAT_EQ(latest_data.g, (float) i); + EXPECT_FLOAT_EQ(latest_data.b, 2.f); + } + } + + // Now check the translation into the other direction + { + int num_topic_updates = 0; + DirectTranslationTest::MessageOlder latest_data{}; + auto publisher = _app_node->create_publisher(topic_name_newer_version, + rclcpp::QoS(1).best_effort()); + auto subscriber = _app_node->create_subscription(topic_name_older_version, + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data, this]( + DirectTranslationTest::MessageOlder::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated: %.3f", (double) msg->data); + latest_data = *msg; + ++num_topic_updates; + }); + + monitor.updateNow(); + + // Wait until there is a subscriber & publisher + ASSERT_TRUE(spinWithTimeout([&subscriber, &publisher]() { + return subscriber->get_publisher_count() > 0 && publisher->get_subscription_count() > 0; + })) << "Timeout, no publisher/subscriber found"; + + // Publish some data & wait for it to arrive + for (int i = 0; i < 10; ++i) { + DirectTranslationTest::MessageNewer msg_newer; + msg_newer.r = (float)i; + msg_newer.g = (float)i * 10.f; + msg_newer.b = (float)i * 100.f; + publisher->publish(msg_newer); + + ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() { + return num_topic_updates == i + 1; + })) << "Timeout, topic update not received, i=" << i; + + // Check data + EXPECT_FLOAT_EQ(latest_data.data, 111.f * (float)i); + } + } +} + + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV1, std_msgs::msg::Float32, 1u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV1, std_msgs::msg::Float64, 1u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeCV1, std_msgs::msg::Int64, 1u); + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV2, std_msgs::msg::ColorRGBA, 2u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV2, std_msgs::msg::Int64, 2u); + +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeAV3, std_msgs::msg::Float64, 3u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeBV3, std_msgs::msg::Int64, 3u); +DEFINE_VERSIONED_ROS_MESSAGE_TYPE(MessageTypeCV3, std_msgs::msg::Float32, 3u); + +class TranslationMultiTestV2 { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + "test/multi_translation_topic_c", + }; + static_assert(MessageTypeAV1::MESSAGE_VERSION == 1); + static_assert(MessageTypeBV1::MESSAGE_VERSION == 1); + static_assert(MessageTypeCV1::MESSAGE_VERSION == 1); + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + }; + static_assert(MessageTypeAV2::MESSAGE_VERSION == 2); + static_assert(MessageTypeBV2::MESSAGE_VERSION == 2); + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + const MessagesOlder::Type3 &msg_older3, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2) { + msg_newer1.r = msg_older1.data; + msg_newer1.g = (float)msg_older2.data; + msg_newer1.b = (float)msg_older3.data; + msg_newer2.data = msg_older3.data * 10; + } + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2, MessagesOlder::Type3 &msg_older3) { + msg_older1.data = msg_newer1.r; + msg_older2.data = msg_newer1.g; + msg_older3.data = msg_newer2.data / 10; + } +}; + +class TranslationMultiTestV3 { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + }; + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "test/multi_translation_topic_a", + "test/multi_translation_topic_b", + "test/multi_translation_topic_c", + }; + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2, MessagesNewer::Type3 &msg_newer3) { + msg_newer1.data = msg_older1.r; + msg_newer2.data = (int64_t)msg_older1.g; + msg_newer3.data = (float)msg_older2.data + 100; + } + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, const MessagesNewer::Type3 &msg_newer3, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2) { + msg_older1.r = (float)msg_newer1.data; + msg_older1.g = (float)msg_newer2.data; + msg_older2.data = (int64_t)msg_newer3.data - 100; + } +}; + +TEST_F(PubSubGraphTest, TranslationMulti) { + RegisteredTranslationsTest registered_translations; + // Register 3 different message versions, with 3 types -> 2 types -> 3 types + registered_translations.registerTranslation(); + registered_translations.registerTranslation(); + + PubSubGraph graph(*_test_node, registered_translations.topicTranslations()); + Monitor monitor(*_test_node, &graph, nullptr); + + const std::string topic_name_a = TranslationMultiTestV2::kTopicsOlder[0]; + const std::string topic_name_b = TranslationMultiTestV2::kTopicsOlder[1]; + const std::string topic_name_c = TranslationMultiTestV2::kTopicsOlder[2]; + + // Create publishers for version 1 + subscribers for version 3 + int num_topic_updates = 0; + MessageTypeAV3 latest_data_a{}; + MessageTypeBV3 latest_data_b{}; + MessageTypeCV3 latest_data_c{}; + auto publisher_a = _app_node->create_publisher(getVersionedTopicName(topic_name_a, MessageTypeAV1::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort()); + auto publisher_b = _app_node->create_publisher(getVersionedTopicName(topic_name_b, MessageTypeBV1::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort()); + auto publisher_c = _app_node->create_publisher(getVersionedTopicName(topic_name_c, MessageTypeCV1::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort()); + auto subscriber_a = _app_node->create_subscription(getVersionedTopicName(topic_name_a, MessageTypeAV3::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_a, this]( + MessageTypeAV3::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (A): %.3f", (double) msg->data); + latest_data_a = *msg; + ++num_topic_updates; + }); + auto subscriber_b = _app_node->create_subscription(getVersionedTopicName(topic_name_b, MessageTypeBV3::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_b, this]( + MessageTypeBV3::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (B): %.3f", (double) msg->data); + latest_data_b = *msg; + ++num_topic_updates; + }); + auto subscriber_c = _app_node->create_subscription(getVersionedTopicName(topic_name_c, MessageTypeCV3::MESSAGE_VERSION), + rclcpp::QoS(1).best_effort(), [&num_topic_updates, &latest_data_c, this]( + MessageTypeCV3::UniquePtr msg) -> void { + RCLCPP_DEBUG(_app_node->get_logger(), "Topic updated (C): %.3f", (double) msg->data); + latest_data_c = *msg; + ++num_topic_updates; + }); + + monitor.updateNow(); + + // Wait until there is a subscriber & publisher + ASSERT_TRUE(spinWithTimeout([&]() { + return subscriber_a->get_publisher_count() > 0 && subscriber_b->get_publisher_count() > 0 && subscriber_c->get_publisher_count() > 0 && + publisher_a->get_subscription_count() > 0 && publisher_b->get_subscription_count() > 0 && publisher_c->get_subscription_count() > 0; + })) << "Timeout, no publisher/subscriber found"; + + // Publish some data & wait for it to arrive + for (int i = 0; i < 10; ++i) { + MessageTypeAV1 msg_older_a; + msg_older_a.data = (float) i; + publisher_a->publish(msg_older_a); + + MessageTypeBV1 msg_older_b; + msg_older_b.data = (float) i * 10.f; + publisher_b->publish(msg_older_b); + + MessageTypeCV1 msg_older_c; + msg_older_c.data = i * 100; + publisher_c->publish(msg_older_c); + + ASSERT_TRUE(spinWithTimeout([&num_topic_updates, i]() { + return num_topic_updates == (i + 1) * 3; + })) << "Timeout, topic update not received, i=" << i << ", num updates=" << num_topic_updates; + + // Check data + EXPECT_FLOAT_EQ(latest_data_a.data, (float)i); + EXPECT_FLOAT_EQ(latest_data_b.data, (float)i * 10.f); + EXPECT_FLOAT_EQ(latest_data_c.data, ((float)i * 100.f) * 10.f + 100.f); + } +} diff --git a/msg/translation_node/test/services.cpp b/msg/translation_node/test/services.cpp new file mode 100644 index 0000000000..f98569b55d --- /dev/null +++ b/msg/translation_node/test/services.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ + +#include +#include +#include +#include + +#include +#include +#include + +using namespace std::chrono_literals; + + +class ServiceTest : public testing::Test +{ +protected: + void SetUp() override + { + _test_node = std::make_shared("test_node"); + _app_node = std::make_shared("app_node"); + _executor.add_node(_test_node); + _executor.add_node(_app_node); + + for (auto& node : {_app_node, _test_node}) { + auto ret = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + RCLCPP_ERROR( + node->get_logger(), "Error setting severity: %s", + rcutils_get_error_string().str); + rcutils_reset_error(); + } + } + } + + bool spinWithTimeout(const std::function& predicate) { + const auto start = _app_node->now(); + while (_app_node->now() - start < 5s) { + _executor.spin_some(); + if (predicate()) { + return true; + } + } + return false; + } + + std::shared_ptr _test_node; + std::shared_ptr _app_node; + rclcpp::executors::SingleThreadedExecutor _executor; +}; + +class RegisteredTranslationsTest : public RegisteredTranslations { +public: + RegisteredTranslationsTest() = default; +}; + + +class ServiceTestV0V1 { +public: + using MessageOlder = translation_node::srv::TestV0; + using MessageNewer = translation_node::srv::TestV1; + + static constexpr const char* kTopic = "test/service"; + + static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + msg_newer.request_a = msg_older.request_a; + } + + static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + msg_older.request_a = msg_newer.request_a; + } + + static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + msg_newer.response_a = msg_older.response_a; + } + + static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + msg_older.response_a = msg_newer.response_a; + } +}; + +class ServiceTestV1V2 { +public: + using MessageOlder = translation_node::srv::TestV1; + using MessageNewer = translation_node::srv::TestV2; + + static constexpr const char* kTopic = "test/service"; + + static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + msg_newer.request_a = msg_older.request_a; + msg_newer.request_b = 1234; + } + + static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + msg_older.request_a = msg_newer.request_a + msg_newer.request_b; + } + + static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + msg_newer.response_a = msg_older.response_a; + msg_newer.response_b = 32; + } + + static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + msg_older.response_a = msg_newer.response_a + msg_newer.response_b; + } +}; + + +TEST_F(ServiceTest, Test) +{ + RegisteredTranslationsTest registered_translations; + registered_translations.registerServiceDirectTranslation(); + registered_translations.registerServiceDirectTranslation(); + + ServiceGraph graph(*_test_node, registered_translations.serviceTranslations()); + Monitor monitor(*_test_node, nullptr, &graph); + + const std::string topic_name = ServiceTestV1V2::kTopic; + const std::string topic_name_v0 = getVersionedTopicName(topic_name, ServiceTestV0V1::MessageOlder::Request::MESSAGE_VERSION); + const std::string topic_name_v1 = getVersionedTopicName(topic_name, ServiceTestV0V1::MessageNewer::Request::MESSAGE_VERSION); + const std::string topic_name_v2 = getVersionedTopicName(topic_name, ServiceTestV1V2::MessageNewer::Request::MESSAGE_VERSION); + + + // Create service + clients + int num_service_requests = 0; + auto service = _app_node->create_service(topic_name_v0, [&num_service_requests]( + const ServiceTestV0V1::MessageOlder::Request::SharedPtr request, ServiceTestV0V1::MessageOlder::Response::SharedPtr response) { + response->response_a = request->request_a + 1; + ++num_service_requests; + }); + auto client0 = _app_node->create_client(topic_name_v0); + auto client1 = _app_node->create_client(topic_name_v1); + auto client2 = _app_node->create_client(topic_name_v2); + + monitor.updateNow(); + + // Wait until there is a service for each client + ASSERT_TRUE(spinWithTimeout([&client0, &client1, &client2]() { + return client0->service_is_ready() && client1->service_is_ready() && client2->service_is_ready(); + })) << "Timeout, no service for clients found: " << client0->service_is_ready() << client1->service_is_ready() << client2->service_is_ready(); + + + + // Make some requests + int expected_num_service_requests = 1; + + // Client 1 + for (int i = 0; i < 10; ++i) { + auto request = std::make_shared(); + ServiceTestV0V1::MessageNewer::Response response; + request->request_a = i; + bool got_response = false; + client1->async_send_request(request, [&got_response, &response](rclcpp::Client::SharedFuture result) { + got_response = true; + response = *result.get(); + }); + + ASSERT_TRUE(spinWithTimeout([&got_response]() { + return got_response; + })) << "Timeout, reply not received, i=" << i; + + // Check data + EXPECT_EQ(response.response_a, i + 1); + EXPECT_EQ(num_service_requests, expected_num_service_requests); + ++expected_num_service_requests; + } + + // Client 0 + for (int i = 0; i < 10; ++i) { + auto request = std::make_shared(); + ServiceTestV0V1::MessageOlder::Response response; + request->request_a = i * 10; + bool got_response = false; + client0->async_send_request(request, [&got_response, &response](rclcpp::Client::SharedFuture result) { + got_response = true; + response = *result.get(); + }); + + ASSERT_TRUE(spinWithTimeout([&got_response]() { + return got_response; + })) << "Timeout, reply not received, i=" << i; + + // Check data + EXPECT_EQ(response.response_a, i * 10 + 1); + EXPECT_EQ(num_service_requests, expected_num_service_requests); + ++expected_num_service_requests; + } + + // Client 2 + for (int i = 0; i < 10; ++i) { + auto request = std::make_shared(); + ServiceTestV1V2::MessageNewer::Response response; + request->request_a = i * 10; + request->request_b = i; + bool got_response = false; + client2->async_send_request(request, [&got_response, &response](rclcpp::Client::SharedFuture result) { + got_response = true; + response = *result.get(); + }); + + ASSERT_TRUE(spinWithTimeout([&got_response]() { + return got_response; + })) << "Timeout, reply not received, i=" << i; + + // Check data + EXPECT_EQ(response.response_a, i + i * 10 + 1); + EXPECT_EQ(response.response_b, 32); + EXPECT_EQ(num_service_requests, expected_num_service_requests); + ++expected_num_service_requests; + } +} diff --git a/msg/translation_node/test/srv/TestV0.srv b/msg/translation_node/test/srv/TestV0.srv new file mode 100644 index 0000000000..4b06c4af35 --- /dev/null +++ b/msg/translation_node/test/srv/TestV0.srv @@ -0,0 +1,4 @@ +uint32 MESSAGE_VERSION = 0 +uint8 request_a +--- +uint64 response_a diff --git a/msg/translation_node/test/srv/TestV1.srv b/msg/translation_node/test/srv/TestV1.srv new file mode 100644 index 0000000000..d5efaac707 --- /dev/null +++ b/msg/translation_node/test/srv/TestV1.srv @@ -0,0 +1,4 @@ +uint32 MESSAGE_VERSION = 1 +uint64 request_a +--- +uint8 response_a diff --git a/msg/translation_node/test/srv/TestV2.srv b/msg/translation_node/test/srv/TestV2.srv new file mode 100644 index 0000000000..da8b79325f --- /dev/null +++ b/msg/translation_node/test/srv/TestV2.srv @@ -0,0 +1,6 @@ +uint32 MESSAGE_VERSION = 2 +uint8 request_a +uint64 request_b +--- +uint16 response_a +uint64 response_b diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h new file mode 100644 index 0000000000..2b3c2030b4 --- /dev/null +++ b/msg/translation_node/translations/all_translations.h @@ -0,0 +1,11 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +#include + +//#include "example_translation_direct_v1.h" +//#include "example_translation_multi_v2.h" +//#include "example_translation_service_v1.h" diff --git a/msg/translation_node/translations/example_translation_direct_v1.h b/msg/translation_node/translations/example_translation_direct_v1.h new file mode 100644 index 0000000000..b2c51aa628 --- /dev/null +++ b/msg/translation_node/translations/example_translation_direct_v1.h @@ -0,0 +1,30 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ExampleTopic v0 <--> v1 +#include +#include + +class ExampleTopicV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::ExampleTopicV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::ExampleTopic; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/example_topic"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(ExampleTopicV1Translation); diff --git a/msg/translation_node/translations/example_translation_multi_v2.h b/msg/translation_node/translations/example_translation_multi_v2.h new file mode 100644 index 0000000000..9c55deace6 --- /dev/null +++ b/msg/translation_node/translations/example_translation_multi_v2.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ExampleTopic and OtherTopic v1 <--> v2 +#include +#include +#include +#include + +class ExampleTopicOtherTopicV2Translation { +public: + using MessagesOlder = TypesArray; + static constexpr const char* kTopicsOlder[] = { + "fmu/out/example_topic", + "fmu/out/other_topic", + }; + static_assert(px4_msgs_old::msg::ExampleTopicV1::MESSAGE_VERSION == 1); + static_assert(px4_msgs_old::msg::OtherTopicV1::MESSAGE_VERSION == 1); + + using MessagesNewer = TypesArray; + static constexpr const char* kTopicsNewer[] = { + "fmu/out/example_topic", + "fmu/out/other_topic", + }; + static_assert(px4_msgs::msg::ExampleTopic::MESSAGE_VERSION == 2); + static_assert(px4_msgs::msg::OtherTopic::MESSAGE_VERSION == 2); + + static void fromOlder(const MessagesOlder::Type1 &msg_older1, const MessagesOlder::Type2 &msg_older2, + MessagesNewer::Type1 &msg_newer1, MessagesNewer::Type2 &msg_newer2) { + // Set msg_newer1, msg_newer2 from msg_older1, msg_older2 + } + + static void toOlder(const MessagesNewer::Type1 &msg_newer1, const MessagesNewer::Type2 &msg_newer2, + MessagesOlder::Type1 &msg_older1, MessagesOlder::Type2 &msg_older2) { + // Set msg_older1, msg_older2 from msg_newer1, msg_newer2 + } +}; + +REGISTER_TOPIC_TRANSLATION(ExampleTopicOtherTopicV2Translation); diff --git a/msg/translation_node/translations/example_translation_service_v1.h b/msg/translation_node/translations/example_translation_service_v1.h new file mode 100644 index 0000000000..e98599ab01 --- /dev/null +++ b/msg/translation_node/translations/example_translation_service_v1.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * Copyright (c) 2024 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate ExampleService v0 <--> v1 +#include +#include + +class ExampleServiceV1Translation { +public: + using MessageOlder = px4_msgs_old::srv::ExampleServiceV0; + static_assert(MessageOlder::Request::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::srv::ExampleService; + static_assert(MessageNewer::Request::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/example_service"; + + static void fromOlder(const MessageOlder::Request &msg_older, MessageNewer::Request &msg_newer) { + // Request: set msg_newer from msg_older + } + + static void toOlder(const MessageNewer::Request &msg_newer, MessageOlder::Request &msg_older) { + // Request: set msg_older from msg_newer + } + + static void fromOlder(const MessageOlder::Response &msg_older, MessageNewer::Response &msg_newer) { + // Response: set msg_newer from msg_older + } + + static void toOlder(const MessageNewer::Response &msg_newer, MessageOlder::Response &msg_older) { + // Response: set msg_older from msg_newer + } +}; + +REGISTER_SERVICE_TRANSLATION_DIRECT(ExampleServiceV1Translation); From 947cc7bcbed266c3420003df13d5060b56ded819 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 29 Oct 2024 16:08:31 +0100 Subject: [PATCH 24/63] ci: run unit tests for ros translation node --- .github/workflows/ros_translation_node.yml | 34 ++++++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 .github/workflows/ros_translation_node.yml diff --git a/.github/workflows/ros_translation_node.yml b/.github/workflows/ros_translation_node.yml new file mode 100644 index 0000000000..877233349f --- /dev/null +++ b/.github/workflows/ros_translation_node.yml @@ -0,0 +1,34 @@ +name: ROS translation node tests +on: + push: + branches: + - 'main' + pull_request: + branches: + - '*' +defaults: + run: + shell: bash +jobs: + build_and_test: + name: Build and test + runs-on: ubuntu-latest + container: + image: rostooling/setup-ros-docker:ubuntu-jammy-latest + steps: + - name: Setup ROS 2 + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble + - name: Checkout repository + uses: actions/checkout@v3 + + - name: Build and test + run: | + ros_ws=/ros_ws + mkdir -p $ros_ws/src + ./Tools/copy_to_ros_ws.sh $ros_ws + cd $ros_ws + source /opt/ros/humble/setup.sh + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --event-handlers=console_cohesion+ + ./build/translation_node/translation_node_unit_tests From 5a2fc5ef7946c31d2f44560e0be7e5c09f3c344e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 31 Oct 2024 14:52:58 +0100 Subject: [PATCH 25/63] ci: check if a versioned .msg file is changed, a new version is added as well --- .github/workflows/ros_translation_node.yml | 17 +++++- Tools/ci/check_msg_versioning.sh | 67 ++++++++++++++++++++++ 2 files changed, 81 insertions(+), 3 deletions(-) create mode 100755 Tools/ci/check_msg_versioning.sh diff --git a/.github/workflows/ros_translation_node.yml b/.github/workflows/ros_translation_node.yml index 877233349f..8b62d92bc9 100644 --- a/.github/workflows/ros_translation_node.yml +++ b/.github/workflows/ros_translation_node.yml @@ -1,4 +1,4 @@ -name: ROS translation node tests +name: ROS Translation Node Tests on: push: branches: @@ -12,7 +12,7 @@ defaults: jobs: build_and_test: name: Build and test - runs-on: ubuntu-latest + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] container: image: rostooling/setup-ros-docker:ubuntu-jammy-latest steps: @@ -21,7 +21,18 @@ jobs: with: required-ros-distributions: humble - name: Checkout repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + # Workaround for https://github.com/actions/runner/issues/2033 + - name: ownership workaround + run: git config --system --add safe.directory '*' + + - name: Check .msg file versioning + if: github.event_name == 'pull_request' + run: | + ./Tools/ci/check_msg_versioning.sh ${{ github.event.pull_request.base.sha }} ${{github.event.pull_request.head.sha}} - name: Build and test run: | diff --git a/Tools/ci/check_msg_versioning.sh b/Tools/ci/check_msg_versioning.sh new file mode 100755 index 0000000000..c10d4819c2 --- /dev/null +++ b/Tools/ci/check_msg_versioning.sh @@ -0,0 +1,67 @@ +#! /bin/bash +# Copy a git diff between two commits if msg versioning is added + +DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) + +PX4_SRC_DIR="$DIR/.." + +BASE_COMMIT="$1" +HEAD_COMMIT="$2" +if [ -z "${BASE_COMMIT}" ] || [ -z "${HEAD_COMMIT}" ] +then + echo "Usage: $0 " + exit 1 +fi + +failed=0 + +# Iterate git diff files between BASE_COMMIT and HEAD_COMMIT +modified_files="$(git --no-pager diff --no-color --name-only --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}")" +all_files="$( git --no-pager diff --no-color --name-only "${BASE_COMMIT}"..."${HEAD_COMMIT}")" +for file in ${modified_files} +do + if [[ "$file" == msg/versioned/*.msg ]]; then + echo "Modified msg file: ${file}" + # A modified versioned .msg file requires: + # - Incrementing the version + # - An old .msg version exists + # - A translation header exists and is included + diff=$(git --no-pager diff --no-color --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}" -- "${file}") + old_version=$(echo "${diff}" | sed -n 's/^-uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') + new_version=$(echo "${diff}" | sed -n 's/^+uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') + + # Check that the version is incremented + if [ -z "${new_version}" ] || [ -z "${old_version}" ]; then + echo "ERROR: Missing version update for ${file}" + failed=1 + else + if [ $((new_version-old_version)) -ne 1 ]; then + echo "ERROR: Version not incremented by +1 for ${file}" + failed=1 + fi + fi + + # Check that an old version exists + filename=$(basename -- "$file") + filename="${filename%.*}" + old_version_file="px4_msgs_old/msg/${filename}V${old_version}.msg" + if [[ "${all_files}" != *"${old_version_file}"* ]]; then + echo "ERROR: Old message version does not exist for ${file} (missing ${old_version_file})" + failed=1 + fi + + # Check that a translation header got added by checking for a modification to all_translations.h + # If it is changed, we assume a new header got added too, so we don't explicitly check for that + if [[ "${modified_files}" != *"translations/all_translations.h"* ]]; then + echo "ERROR: missing modification to translations/all_translations.h" + failed=1 + fi + fi +done + +if [ ${failed} -ne 0 ]; then + echo "" + echo "ERROR: missing message versioning due to changed .msg file(s) (see above)" + echo "Check the documentation under https://docs.px4.io/main/en/ros2/px4_ros2_msg_translation_node.html for how to add a new version" + exit 1 +fi From b96b4fb68d5e4fcfaf1e6874a77e2eb9023b0803 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 5 Feb 2025 08:08:11 +0100 Subject: [PATCH 26/63] ci: run ROS humble + jazzy for translation node --- .github/workflows/ros_translation_node.yml | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ros_translation_node.yml b/.github/workflows/ros_translation_node.yml index 8b62d92bc9..5a65064edb 100644 --- a/.github/workflows/ros_translation_node.yml +++ b/.github/workflows/ros_translation_node.yml @@ -13,13 +13,19 @@ jobs: build_and_test: name: Build and test runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] + strategy: + fail-fast: false + matrix: + config: + - {ros_version: "humble", ubuntu: "jammy"} + - {ros_version: "jazzy", ubuntu: "noble"} container: - image: rostooling/setup-ros-docker:ubuntu-jammy-latest + image: rostooling/setup-ros-docker:ubuntu-${{ matrix.config.ubuntu }}-latest steps: - - name: Setup ROS 2 + - name: Setup ROS 2 (${{ matrix.config.ros_version }}) uses: ros-tooling/setup-ros@v0.7 with: - required-ros-distributions: humble + required-ros-distributions: ${{ matrix.config.ros_version }} - name: Checkout repository uses: actions/checkout@v4 with: @@ -40,6 +46,7 @@ jobs: mkdir -p $ros_ws/src ./Tools/copy_to_ros_ws.sh $ros_ws cd $ros_ws - source /opt/ros/humble/setup.sh + source /opt/ros/${{ matrix.config.ros_version }}/setup.sh colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --event-handlers=console_cohesion+ + source ./install/setup.sh ./build/translation_node/translation_node_unit_tests From 98cba19f50b5e83c900b2fd48947705777f368a3 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Tue, 11 Feb 2025 19:17:53 -0700 Subject: [PATCH 27/63] esc_battery: report temperature Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- src/modules/esc_battery/EscBattery.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 6a6a21f02a..0fb4a3efad 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -91,17 +91,25 @@ EscBattery::Run() const uint8_t online_esc_count = math::countSetBits(esc_status.esc_online_flags); float average_voltage_v = 0.0f; float total_current_a = 0.0f; + float average_temperature_c = 0.0f; for (unsigned i = 0; i < esc_status.esc_count; ++i) { if ((1 << i) & esc_status.esc_online_flags) { average_voltage_v += esc_status.esc[i].esc_voltage; total_current_a += esc_status.esc[i].esc_current; + + if (PX4_ISFINITE(esc_status.esc[i].esc_temperature)) { + average_temperature_c += esc_status.esc[i].esc_temperature; + } } } average_voltage_v /= online_esc_count; + total_current_a /= online_esc_count; + average_temperature_c /= online_esc_count; _battery.setConnected(true); + _battery.updateTemperature(average_temperature_c); _battery.updateVoltage(average_voltage_v); _battery.updateCurrent(total_current_a); _battery.updateAndPublishBatteryStatus(esc_status.timestamp); From 8acac41163596f95b1efa6d369b2fd75f0d1f639 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 12 Feb 2025 07:48:04 -0900 Subject: [PATCH 28/63] ark: pi6x: fix CDCACM_PRODUCTID (#24320) --- boards/ark/pi6x/nuttx-config/bootloader/defconfig | 2 +- boards/ark/pi6x/nuttx-config/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/ark/pi6x/nuttx-config/bootloader/defconfig b/boards/ark/pi6x/nuttx-config/bootloader/defconfig index 745251793b..bba1ad3f39 100644 --- a/boards/ark/pi6x/nuttx-config/bootloader/defconfig +++ b/boards/ark/pi6x/nuttx-config/bootloader/defconfig @@ -32,7 +32,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK BL Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/ark/pi6x/nuttx-config/nsh/defconfig b/boards/ark/pi6x/nuttx-config/nsh/defconfig index 0e944e53c9..57ff449302 100644 --- a/boards/ark/pi6x/nuttx-config/nsh/defconfig +++ b/boards/ark/pi6x/nuttx-config/nsh/defconfig @@ -76,7 +76,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 From 195961ae83190083526614f0fb923584f42fff04 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Tue, 11 Feb 2025 19:47:29 -0800 Subject: [PATCH 29/63] ci: only push dev container on commit to main --- .github/workflows/dev_container.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index f64d29b684..b3f9542ead 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -96,6 +96,6 @@ jobs: platforms: | linux/amd64 provenance: mode=max - push: true + push: ${{ github.event_name != 'pull_request' }} cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max From b77797b4903e2cb5ba460834f835c94ee76e32ae Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 12 Feb 2025 15:28:40 -0900 Subject: [PATCH 30/63] tools: px_uploader.py: change RuntimeErorr to printf warning for fw_maxsize greater than fw_maxsize (#24321) --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 34f3be05ca..6d8c7c69dc 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -708,7 +708,7 @@ class uploader: # https://github.com/PX4/Firmware/blob/master/src/drivers/boards/common/stm32/board_mcu_version.c#L125-L144 if self.fw_maxsize > fw.property('image_maxsize') and not force: - raise RuntimeError(f"Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes). Please use the correct board configuration to avoid lacking critical functionality.") + print(f"WARNING: Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes)") else: # If we're still on bootloader v4 on a Pixhawk, we don't know if we # have the silicon errata and therefore need to flash px4_fmu-v2 From 96461cdd7d9f576e17f61f093dd110ec17a23610 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Feb 2025 11:49:08 +0100 Subject: [PATCH 31/63] ci: ignore changes to comments or constants in versioned message check These can be changed without version increment --- Tools/ci/check_msg_versioning.sh | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Tools/ci/check_msg_versioning.sh b/Tools/ci/check_msg_versioning.sh index c10d4819c2..37c593d277 100755 --- a/Tools/ci/check_msg_versioning.sh +++ b/Tools/ci/check_msg_versioning.sh @@ -26,6 +26,15 @@ do # - Incrementing the version # - An old .msg version exists # - A translation header exists and is included + + # Ignore changes to comments or constants + content_a=$(git show "${BASE_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + content_b=$(git show "${HEAD_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + if [ "${content_a}" == "${content_b}" ]; then + echo "No version update required for ${file}" + continue + fi + diff=$(git --no-pager diff --no-color --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}" -- "${file}") old_version=$(echo "${diff}" | sed -n 's/^-uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') new_version=$(echo "${diff}" | sed -n 's/^+uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') From a151d85a1ca2ed85459a4572dafee46c84dbe377 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Thu, 13 Feb 2025 16:29:21 -0700 Subject: [PATCH 32/63] ARK Cannodes disable mag bias estimator by default (#24327) --- boards/ark/can-gps/init/rc.board_defaults | 2 +- boards/ark/can-rtk-gps/init/rc.board_defaults | 2 +- boards/ark/cannode/init/rc.board_defaults | 1 + boards/ark/septentrio-gps/init/rc.board_defaults | 2 +- boards/ark/teseo-gps/init/rc.board_defaults | 2 +- 5 files changed, 5 insertions(+), 4 deletions(-) diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults index 13e16a24f1..3d085fe527 100644 --- a/boards/ark/can-gps/init/rc.board_defaults +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 99b30c747f..85d6a92063 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -7,7 +7,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 param set-default GPS_1_GNSS 63 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/cannode/init/rc.board_defaults b/boards/ark/cannode/init/rc.board_defaults index 7d149ce5ad..9fe7db23e0 100644 --- a/boards/ark/cannode/init/rc.board_defaults +++ b/boards/ark/cannode/init/rc.board_defaults @@ -3,6 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 pwm_out start diff --git a/boards/ark/septentrio-gps/init/rc.board_defaults b/boards/ark/septentrio-gps/init/rc.board_defaults index 0db1234ce9..6ae5883882 100644 --- a/boards/ark/septentrio-gps/init/rc.board_defaults +++ b/boards/ark/septentrio-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/teseo-gps/init/rc.board_defaults b/boards/ark/teseo-gps/init/rc.board_defaults index 610b5b418f..c765117f40 100644 --- a/boards/ark/teseo-gps/init/rc.board_defaults +++ b/boards/ark/teseo-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 tone_alarm start From eb18edf5eb3c47d736a565d552b438bf3b715825 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Thu, 13 Feb 2025 21:49:09 -0800 Subject: [PATCH 33/63] Don't allow spacecraft module parameters for VOXL 2 builds (#24336) - VOXL 2 builds use DISABLE_PARAMS_MODULE_SCOPING for parameters. The new spacecraft module has duplicate symbols with the control_allocator module and so this kills the VOXL 2 build --- src/lib/parameters/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index b87e652cad..81b609080a 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -62,6 +62,10 @@ if(DISABLE_PARAMS_MODULE_SCOPING) # allow those timer configurations to be skipped. if ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES pwm_out)) message(STATUS "Skipping pwm file path ${file_path} for VOXL2") + # Spacecraft has duplicate parameter names which kills the VOXL 2 build. VOXL 2 does not + # support the spacecraft module so we skip adding the parameters. + elseif ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES spacecraft)) + message(STATUS "Skipping spacecraft file path ${file_path} for VOXL2") else() list(APPEND module_config_files "${file_path}") endif() From f2471861a38cc43bdd23e9642fffb86fddcf4c87 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Thu, 13 Feb 2025 10:13:26 +0100 Subject: [PATCH 34/63] Fix GPS RTCM instance selection --- src/drivers/gps/gps.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index aeeba755e3..ba6f24070f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -547,8 +547,13 @@ void GPS::handleInjectDataTopic() for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { const bool exists = _orb_inject_data_sub[instance].advertised(); - if (exists) { - if (_orb_inject_data_sub[instance].copy(&msg)) { + if (exists && _orb_inject_data_sub[instance].copy(&msg)) { + /* Don't select the own RTCM instance. In case it has a lower + * instance number, it will be selected and will be rejected + * later in the code, resulting in no RTCM injection at all. + */ + if (msg.device_id != get_device_id()) { + // Only use the message if it is up to date if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true; From 430be081315a905d10acccfb5953429c571a9bd3 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Fri, 14 Feb 2025 14:54:42 +0100 Subject: [PATCH 35/63] Add payload tx/rx timeouts to DDS * Add tx/rx timeouts * Code style & tx default timeout * Clarify TX/RX disable value --- src/modules/uxrce_dds_client/module.yaml | 24 +++ .../uxrce_dds_client/uxrce_dds_client.cpp | 158 ++++++++++++------ .../uxrce_dds_client/uxrce_dds_client.h | 20 ++- 3 files changed, 147 insertions(+), 55 deletions(-) diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 4e5db21160..773475f1f5 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -105,3 +105,27 @@ parameters: category: System reboot_required: true default: 0 + + UXRCE_DDS_TX_TO: + description: + short: TX rate timeout configuration + long: | + Specifies after how many seconds without sending data the DDS connection is reestablished. + A value less than one disables the TX rate timeout. + type: int32 + category: System + reboot_required: true + default: 3 + unit: s + + UXRCE_DDS_RX_TO: + description: + short: RX rate timeout configuration + long: | + Specifies after how many seconds without receiving data the DDS connection is reestablished. + A value less than one disables the RX rate timeout. + type: int32 + category: System + reboot_required: true + default: -1 + unit: s diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 339bedd7a9..b550912791 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -209,7 +209,7 @@ void UxrceddsClient::deinit() _comm = nullptr; } -bool UxrceddsClient::setup_session(uxrSession *session) +bool UxrceddsClient::setupSession(uxrSession *session) { _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); _synchronize_timestamps = (_param_uxrce_dds_synct.get() > 0); @@ -379,7 +379,7 @@ bool UxrceddsClient::setup_session(uxrSession *session) return true; } -void UxrceddsClient::delete_session(uxrSession *session) +void UxrceddsClient::deleteSession(uxrSession *session) { delete_repliers(); @@ -472,6 +472,20 @@ static void fillMessageFormatResponse(const message_format_request_s &message_fo message_format_response.timestamp = hrt_absolute_time(); } +void UxrceddsClient::calculateTxRxRate() +{ + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_status_update > 1_s) { + float dt = (now - _last_status_update) / 1e6f; + _last_payload_tx_rate = (_subs->num_payload_sent - _last_num_payload_sent) / dt; + _last_payload_rx_rate = (_pubs->num_payload_received - _last_num_payload_received) / dt; + _last_num_payload_sent = _subs->num_payload_sent; + _last_num_payload_received = _pubs->num_payload_received; + _last_status_update = now; + } +} + void UxrceddsClient::handleMessageFormatRequest() { message_format_request_s message_format_request; @@ -483,6 +497,87 @@ void UxrceddsClient::handleMessageFormatRequest() } } +void UxrceddsClient::checkConnectivity(uxrSession *session) +{ + // Reset TX zero counter, when data is sent + if (_last_payload_tx_rate > 0) { + _num_tx_rate_zero = 0; + } + + // Reset RX zero counter, when data is received + if (_last_payload_rx_rate > 0) { + _num_rx_rate_zero = 0; + } + + const hrt_abstime now = hrt_absolute_time(); + + // Start ping and tx/rx rate monitoring, unless we're actively sending & receiving payloads successfully + if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { + _connected = true; + _num_pings_missed = 0; + _last_ping = now; + + } else { + if (hrt_elapsed_time(&_last_ping) > 1_s) { + // Check payload tx rate + if (_last_payload_tx_rate == 0) { + _num_tx_rate_zero++; + } + + // Check payload rx rate + if (_last_payload_rx_rate == 0) { + _num_rx_rate_zero++; + } + + // Check ping + _last_ping = now; + + if (_had_ping_reply) { + _num_pings_missed = 0; + + } else { + ++_num_pings_missed; + } + + int timeout_ms = 1'000; // 1 second + uint8_t attempts = 1; + uxr_ping_agent_session(session, timeout_ms, attempts); + + _had_ping_reply = false; + } + + if (_num_pings_missed >= 3) { + PX4_ERR("No ping response, disconnecting"); + _connected = false; + } + + int32_t tx_timeout = _param_uxrce_dds_tx_to.get(); + int32_t rx_timeout = _param_uxrce_dds_rx_to.get(); + + if (tx_timeout > 0 && _num_tx_rate_zero >= tx_timeout) { + PX4_ERR("Payload TX rate zero for too long, disconnecting"); + _connected = false; + } + + if (rx_timeout > 0 && _num_rx_rate_zero >= rx_timeout) { + PX4_ERR("Payload RX rate zero for too long, disconnecting"); + _connected = false; + } + } +} + +void UxrceddsClient::resetConnectivityCounters() +{ + _last_status_update = hrt_absolute_time(); + _last_ping = hrt_absolute_time(); + _had_ping_reply = false; + _num_pings_missed = 0; + _last_num_payload_sent = 0; + _last_num_payload_received = 0; + _num_tx_rate_zero = 0; + _num_rx_rate_zero = 0; +} + void UxrceddsClient::syncSystemClock(uxrSession *session) { struct timespec ts = {}; @@ -535,8 +630,8 @@ void UxrceddsClient::run() continue; } - if (!setup_session(&session)) { - delete_session(&session); + if (!setupSession(&session)) { + deleteSession(&session); px4_usleep(1'000'000); PX4_ERR("session setup failed, will retry now"); continue; @@ -552,13 +647,8 @@ void UxrceddsClient::run() } hrt_abstime last_sync_session = 0; - hrt_abstime last_status_update = hrt_absolute_time(); - hrt_abstime last_ping = hrt_absolute_time(); - int num_pings_missed = 0; - bool had_ping_reply = false; - uint32_t last_num_payload_sent{}; - uint32_t last_num_payload_received{}; int poll_error_counter = 0; + resetConnectivityCounters(); _subs->init(); _subs_initialized = true; @@ -629,55 +719,19 @@ void UxrceddsClient::run() // Check for a ping response /* PONG_IN_SESSION_STATUS */ if (session.on_pong_flag == 1) { - had_ping_reply = true; + _had_ping_reply = true; } - const hrt_abstime now = hrt_absolute_time(); + // Calculate the payload tx/rx rate for connectivity monitoring + calculateTxRxRate(); - if (now - last_status_update > 1_s) { - float dt = (now - last_status_update) / 1e6f; - _last_payload_tx_rate = (_subs->num_payload_sent - last_num_payload_sent) / dt; - _last_payload_rx_rate = (_pubs->num_payload_received - last_num_payload_received) / dt; - last_num_payload_sent = _subs->num_payload_sent; - last_num_payload_received = _pubs->num_payload_received; - last_status_update = now; - } - - // Handle ping, unless we're actively sending & receiving payloads successfully - if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { - _connected = true; - num_pings_missed = 0; - last_ping = now; - - } else { - if (hrt_elapsed_time(&last_ping) > 1_s) { - last_ping = now; - - if (had_ping_reply) { - num_pings_missed = 0; - - } else { - ++num_pings_missed; - } - - int timeout_ms = 1'000; // 1 second - uint8_t attempts = 1; - uxr_ping_agent_session(&session, timeout_ms, attempts); - - had_ping_reply = false; - } - - if (num_pings_missed >= 3) { - PX4_INFO("No ping response, disconnecting"); - _connected = false; - } - } + // Check if there is still connectivity with the agent + checkConnectivity(&session); perf_end(_loop_perf); - } - delete_session(&session); + deleteSession(&session); } } diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index 2a75d73e4a..823e242f9d 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -120,13 +120,17 @@ private: bool init(); void deinit(); - bool setup_session(uxrSession *session); - void delete_session(uxrSession *session); + bool setupSession(uxrSession *session); + void deleteSession(uxrSession *session); bool setBaudrate(int fd, unsigned baud); void handleMessageFormatRequest(); + void calculateTxRxRate(); + void checkConnectivity(uxrSession *session); + void resetConnectivityCounters(); + uORB::Publication _message_format_response_pub{ORB_ID(message_format_response)}; uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)}; @@ -179,6 +183,14 @@ private: uxrCommunication *_comm{nullptr}; int _fd{-1}; + hrt_abstime _last_status_update; + hrt_abstime _last_ping; + bool _had_ping_reply{false}; + int _num_pings_missed{0}; + int32_t _num_tx_rate_zero{0}; + int32_t _num_rx_rate_zero{0}; + uint32_t _last_num_payload_sent{0}; + uint32_t _last_num_payload_received{0}; int _last_payload_tx_rate{}; ///< in B/s int _last_payload_rx_rate{}; ///< in B/s @@ -197,6 +209,8 @@ private: (ParamInt) _param_uxrce_key, (ParamInt) _param_uxrce_dds_ptcfg, (ParamInt) _param_uxrce_dds_syncc, - (ParamInt) _param_uxrce_dds_synct + (ParamInt) _param_uxrce_dds_synct, + (ParamInt) _param_uxrce_dds_tx_to, + (ParamInt) _param_uxrce_dds_rx_to ) }; From e3fd50667d1b4cd272544258ec9952550f0b2236 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 11 Feb 2025 11:56:20 +0100 Subject: [PATCH 36/63] Mag cal: automatically disable internal mags if external ones are available --- src/lib/sensor_calibration/Magnetometer.hpp | 1 + src/modules/commander/mag_calibration.cpp | 14 ++++++++++++++ .../VehicleMagnetometer.cpp | 18 ++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 8d328e7a7d..5f9dd72d43 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -85,6 +85,7 @@ public: int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } + void disable() { _priority = 0; } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a72f0fcadb..7aeadeab55 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -903,6 +903,15 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma bool param_save = false; bool failed = true; + bool external_mag_available = false; + + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].external() && worker_data.calibration[cur_mag].enabled()) { + external_mag_available = true; + break; + } + } + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { calibration::Magnetometer ¤t_cal = worker_data.calibration[cur_mag]; @@ -921,6 +930,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma current_cal.set_offdiagonal(offdiag[cur_mag]); } + if (external_mag_available && !current_cal.external()) { + // automatically disable the internal mags as they should not be used for navigation + current_cal.disable(); + } + current_cal.PrintStatus(); if (current_cal.ParametersSave(cur_mag, true)) { diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index df90ffdf90..a0ba14bcfd 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -209,6 +209,19 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) { bool parameters_notify = false; + bool external_mag_available = false; + + for (unsigned mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].external() + && _calibration[mag_index].enabled() + && mag_bias_est.valid[mag_index] + && mag_bias_est.stable[mag_index]) { + + external_mag_available = true; + break; + } + } + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { @@ -228,6 +241,11 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); if (_calibration[mag_index].set_offset(offset)) { + if (external_mag_available && !_calibration[mag_index].external()) { + // automatically disable the internal mags as they should not be used for navigation + _calibration[mag_index].disable(); + } + // save parameters with preferred calibration slot to current sensor index _calibration[mag_index].ParametersSave(mag_index); From fe5c88789568903b9a7cb175421ae203b8cfc32f Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 14 Feb 2025 15:45:39 +0100 Subject: [PATCH 37/63] mag check: do not require mag 0 sys_has_mag defines the required number of mags; if one mag is needed, it can be any instance, not necessarily mag 0 --- .../HealthAndArmingChecks/checks/magnetometerCheck.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 6716a8e06a..ac35a61803 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -47,9 +47,6 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter int num_enabled_and_valid_calibration = 0; for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) { - bool is_mag_fault = false; - const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault); - const bool exists = _sensor_mag_sub[instance].advertised(); bool is_valid = false; bool is_calibration_valid = false; @@ -80,7 +77,9 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter } // Do not raise errors if a mag is not required - if (!is_required) { + bool is_mag_fault = false; + + if (!isMagRequired(instance, is_mag_fault)) { continue; } From 61d595dc64c32da36d29a2c4409dae2d618f8077 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 13 Feb 2025 11:24:29 +0100 Subject: [PATCH 38/63] reset dist_to_ground_lock if dist_to_bottom is not valid. this avoids a reset to a previous dist_to_ground after a switch back to a valid measurement --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 7ea9289cb6..ed7676296f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -129,6 +129,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } else { _position_setpoint(2) = _position(2); + _dist_to_ground_lock = NAN; } } From fd5bb9e69c0d68fb7d030b52dd5bfc62ec7209a8 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 14 Feb 2025 09:49:36 -0700 Subject: [PATCH 39/63] uavcannode rangefinder: only publish reading too far or too close if less than or greater than. Not equal to. --- src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 129bd32c21..2ad053d9a4 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -100,10 +100,10 @@ public: } // reading_type - if (dist.current_distance >= dist.max_distance) { + if (dist.current_distance > dist.max_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; - } else if (dist.current_distance <= dist.min_distance) { + } else if (dist.current_distance < dist.min_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; } else if (dist.signal_quality != 0) { From b0eb63958793b56e8d244d2095b6f2616b474d7e Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Fri, 14 Feb 2025 11:33:38 -0800 Subject: [PATCH 40/63] voxl2_io: Updated to latest version from ModalAI fork - Updated to the latest version of the voxl2_io driver from the ModalAI fork. - Moved to the platform independent Serial driver - Added voxl2_io driver to the SLPI DSP build --- boards/modalai/voxl2-slpi/default.px4board | 1 + src/drivers/voxl2_io/CMakeLists.txt | 3 - src/drivers/voxl2_io/Kconfig | 2 +- src/drivers/voxl2_io/module.yaml | 13 +- src/drivers/voxl2_io/voxl2_io.cpp | 1044 +++++++----------- src/drivers/voxl2_io/voxl2_io.hpp | 85 +- src/drivers/voxl2_io/voxl2_io_packet.c | 26 + src/drivers/voxl2_io/voxl2_io_packet.h | 3 + src/drivers/voxl2_io/voxl2_io_packet_types.h | 1 + src/drivers/voxl2_io/voxl2_io_params.c | 53 +- src/drivers/voxl2_io/voxl2_io_serial.cpp | 191 ---- src/drivers/voxl2_io/voxl2_io_serial.hpp | 69 -- 12 files changed, 510 insertions(+), 981 deletions(-) delete mode 100644 src/drivers/voxl2_io/voxl2_io_serial.cpp delete mode 100644 src/drivers/voxl2_io/voxl2_io_serial.hpp diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index db58d4e28a..1aec34124a 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -13,6 +13,7 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y CONFIG_DRIVERS_RC_CRSF_RC=y +CONFIG_DRIVERS_VOXL2_IO=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_EKF2=y diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt index 57a5f4af21..b6e3d8a3c2 100644 --- a/src/drivers/voxl2_io/CMakeLists.txt +++ b/src/drivers/voxl2_io/CMakeLists.txt @@ -37,15 +37,12 @@ px4_add_module( SRCS voxl2_io_crc16.c voxl2_io_crc16.h - voxl2_io_serial.cpp - voxl2_io_serial.hpp voxl2_io_packet.c voxl2_io_packet.h voxl2_io_packet_types.h voxl2_io.cpp voxl2_io.hpp DEPENDS - rc px4_work_queue mixer_module MODULE_CONFIG diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig index 622cad470d..15964deeeb 100644 --- a/src/drivers/voxl2_io/Kconfig +++ b/src/drivers/voxl2_io/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO bool "voxl2_io" default n ---help--- - Enable support for voxl2_io + Enable support for voxl2_io \ No newline at end of file diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 021305e955..17feaba8c5 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -1,12 +1,13 @@ module_name: VOXL2 IO Output actuator_output: - config_parameters: - - param: 'VOXL2_IO_MIN' - label: 'PWM min value' - - param: 'VOXL2_IO_MAX' - label: 'PWM max value' +# config_parameters: +# - param: 'VOXL2_IO_MIN' +# label: 'PWM min value' +# - param: 'VOXL2_IO_MAX' +# label: 'PWM max value' output_groups: - param_prefix: VOXL2_IO group_label: 'PWMs' channel_label: 'PWM Channel' - num_channels: 4 + num_channels: 8 + diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 352b474906..90adfb8422 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -33,8 +33,6 @@ #include "voxl2_io.hpp" -#include - Voxl2IO::Voxl2IO() : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)), @@ -43,69 +41,81 @@ Voxl2IO::Voxl2IO() : _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) { _mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS); - _uart_port = new Voxl2IoSerial(); - voxl2_io_packet_init(&_sbus_packet); + voxl2_io_packet_init(&_voxl2_io_packet); + + //set low rate scheduling interval to 200hz so that RC can be updated even if all actuators are disabled + //otherwise, the default low rate scheduling interval is 300ms and RC packets are delayed and lost + _mixing_output.setLowrateSchedulingInterval(5_ms); } Voxl2IO::~Voxl2IO() { - /* make sure servos are off */ - stop_all_pwms(); - - if (_uart_port) { - _uart_port->uart_close(); - _uart_port = nullptr; - } + _uart_port.close(); perf_free(_cycle_perf); perf_free(_output_update_perf); } - int Voxl2IO::init() { + PX4_INFO("VOXL2_IO: Driver starting"); + int ret = PX4_OK; - /* Open serial port in this thread */ - if (!_uart_port->is_open()) { - if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) { - /* Send PWM config to M0065... pwm_min and pwm_max */ - PX4_INFO("Opened UART connection to M0065 device on port %s", _device); - - } else { - PX4_ERR("Failed opening device"); - return PX4_ERROR; - } - } - - /* Verify connectivity and protocol version number */ - if (get_version_info() < 0) { - PX4_ERR("Failed to detect voxl2_io protocol version."); - return PX4_ERROR; - - } else { - if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION - && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) { - PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - - } else { - PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - return PX4_ERROR; - } - } - - /* Getting initial parameter values */ + // Getting initial parameter values ret = update_params(); if (ret != OK) { + PX4_ERR("VOXL2_IO: Failed to update params during init"); return ret; } - /* Send PWM MIN/MAX to M0065 */ - update_pwm_config(); + // Print initial param values + print_params(); - ScheduleOnInterval(_current_update_interval); - // ScheduleNow(); + PX4_INFO("VOXL2_IO: "); + + // Open serial port + if (!_uart_port.isOpen()) { + PX4_INFO("VOXL2_IO: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); + + // Configure UART port + if (! _uart_port.setPort(_device)) { + PX4_ERR("Error configuring serial device on port %s", _device); + return -1; + } + + if (! _uart_port.setBaudrate(_parameters.baud_rate)) { + PX4_ERR("Error setting baudrate to %d on %s", (int) _parameters.baud_rate, _device); + return -1; + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart_port.open()) { + PX4_ERR("Error opening serial device %s", _device); + return -1; + } + } + + // Detect M0065 board + ret = get_version_info(); + + if (ret != 0) { + PX4_ERR("VOXL2_IO: Could not detect the board"); + PX4_ERR("VOXL2_IO: Driver initialization failed. Exiting"); + + if (_uart_port.open()) { + PX4_INFO("VOXL2_IO: Closing uart port"); + _uart_port.close(); + } + + return -1; + } + + //ScheduleOnInterval(_current_update_interval); + ScheduleNow(); + + PX4_INFO("VOXL2_IO: Driver initialization succeeded"); return ret; } @@ -117,11 +127,10 @@ int Voxl2IO::update_params() ret = load_params(&_parameters); if (ret == PX4_OK) { - _mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED); + _mixing_output.setAllDisarmedValues(_parameters.pwm_dis); _mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE); - _mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN); - _mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX); - _pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min; + _mixing_output.setAllMinValues(_parameters.pwm_min); + _mixing_output.setAllMaxValues(_parameters.pwm_max); } return ret; @@ -130,196 +139,159 @@ int Voxl2IO::update_params() int Voxl2IO::load_params(voxl2_io_params_t *params) { int ret = PX4_OK; - int32_t max = params->pwm_max; - int32_t min = params->pwm_min; // initialize out for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { params->function_map[i] = (int)OutputFunction::Disabled; } - /* UART config, PWM mode, and RC protocol*/ + // UART config, PWM mode, and RC protocol param_get(param_find("VOXL2_IO_BAUD"), ¶ms->baud_rate); - param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); + //param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); - /* PWM min, max, and failsafe values*/ + // PWM min, max, and failsafe values param_get(param_find("VOXL2_IO_MIN"), ¶ms->pwm_min); param_get(param_find("VOXL2_IO_MAX"), ¶ms->pwm_max); + param_get(param_find("VOXL2_IO_DIS"), ¶ms->pwm_dis); + param_get(param_find("VOXL2_IO_CMIN"), ¶ms->pwm_cal_min); + param_get(param_find("VOXL2_IO_CMAX"), ¶ms->pwm_cal_max); - /* PWM output functions */ + // PWM output functions + //0: disabled, 1: constant min, 2: constant max + //101-112: motors, 201-208: servos, 402: RC Roll, 403: RC Pitch, 404: RC Throttle, + //405: RC Yaw, 406: RC Flaps, 407-412: RC AUX 1-6, 420-422: Gimbal RPY param_get(param_find("VOXL2_IO_FUNC1"), ¶ms->function_map[0]); param_get(param_find("VOXL2_IO_FUNC2"), ¶ms->function_map[1]); param_get(param_find("VOXL2_IO_FUNC3"), ¶ms->function_map[2]); param_get(param_find("VOXL2_IO_FUNC4"), ¶ms->function_map[3]); + param_get(param_find("VOXL2_IO_FUNC5"), ¶ms->function_map[4]); + param_get(param_find("VOXL2_IO_FUNC6"), ¶ms->function_map[5]); + param_get(param_find("VOXL2_IO_FUNC7"), ¶ms->function_map[6]); + param_get(param_find("VOXL2_IO_FUNC8"), ¶ms->function_map[7]); - /* Validate PWM min and max values */ + // Validate PWM min and max values if (params->pwm_min > params->pwm_max) { - PX4_ERR("Invalid parameter VOXL2_IO_MIN. Please verify parameters."); + PX4_ERR("VOXL2_IO: Invalid parameter VOXL2_IO_MIN. Please verify parameters."); params->pwm_min = 0; ret = PX4_ERROR; } - if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) { - PX4_INFO("Updating PWM params load_params"); - update_pwm_config(); - } - return ret; } -void Voxl2IO::update_pwm_config() -{ - Command cmd; - uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast((_parameters.pwm_min & 0xFF00) >> 8), static_cast(_parameters.pwm_min & 0xFF), - static_cast((_parameters.pwm_max & 0xFF00) >> 8), static_cast(_parameters.pwm_max & 0xFF) - }; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE, cmd.buf, - sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send config packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } -} - int Voxl2IO::get_version_info() { - int res = 0 ; - int header = -1 ; - int info_packet = -1; - int read_retries = 3; - int read_succeeded = 0; + PX4_INFO("VOXL2_IO: Detecting M0065 board..."); + voxl2_io_packet_init(&_voxl2_io_packet); + Command cmd; + cmd.len = voxl2_io_create_extended_version_request_packet(0, cmd.buf, sizeof(cmd.buf)); - /* Request protocol version info from M0065 */ - cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE); + int retries_left = _board_detect_retries; + bool got_response = false; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); + while ((got_response == false) && (retries_left > 0)) { + retries_left--; + + //send the version request command to the board + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Could not write version request packet to UART port"); + return -1; + } - } else { _bytes_sent += cmd.len; _packets_sent++; - } - /* Read response */ - px4_usleep(10000); - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + hrt_abstime t_request = hrt_absolute_time(); + hrt_abstime t_timeout = 50000; //50ms timeout for version info response - while (read_retries) { - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) { - info_packet = index; - break; - } - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; + //wait for the response to come back + while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) { + px4_usleep(500); //sleep a bit while waiting for the board to respond + + int nread = _uart_port.read(_read_buf, sizeof(_read_buf)); + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + hrt_abstime response_time = hrt_elapsed_time(&t_request); + //PX4_INFO("got packet of length %i",ret); + _packets_received++; + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(VOXL2_IO_EXTENDED_VERSION_INFO)) { + VOXL2_IO_EXTENDED_VERSION_INFO ver; + memcpy(&ver, _voxl2_io_packet.buffer, packet_size); + + PX4_INFO("VOXL2_IO: \tVOXL2_IO ID: %i", ver.id); + PX4_INFO("VOXL2_IO: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version).c_str()); + + uint8_t *u = &ver.unique_id[0]; + PX4_INFO("VOXL2_IO: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + + PX4_INFO("VOXL2_IO: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("VOXL2_IO: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_INFO("VOXL2_IO: \tReply time : %uus", (uint32_t)response_time); + + //we requested response from ID 0, so it should match + if (ver.id != 0) { + PX4_ERR("VOXL2_IO: Invalid id: %d", ver.id); + } + + //check HW (board version) + else if (ver.hw_version != VOXL2_IO_HW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid HW version : %d (expected %d)", ver.hw_version, VOXL2_IO_HW_VERSION); + return -1; + } + + //check firmware version running on the board + else if (ver.sw_version != VOXL2_IO_SW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid FW version : %d (expected %d)", ver.sw_version, VOXL2_IO_SW_VERSION); + return -1; + + } else { + got_response = true; + memcpy(&_version_info, &ver, sizeof(_version_info)); //store the version info only if it is valid + } + } } } - /* Try again in a bit if packet header not present yet... */ - if (header == -1 || info_packet == -1) { - if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); } - - if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) { - if (_debug) { - PX4_ERR("Error parsing version info packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - break; - - } else { - memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO)); - read_succeeded = 1; + //break out of the loop waiting for a response + if (got_response) { break; } + } - } else { - read_retries--; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } + if (!got_response) { + PX4_ERR("VOXL2_IO: Board version info response timeout (%d retries left)", retries_left); } } - if (! read_succeeded) { - return -EIO; - } - - return 0; + return (got_response == true ? 0 : -1); } bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], unsigned num_outputs, unsigned num_control_groups_updated) { - /* Stop Mixer while ESCs are being calibrated */ + // Stop Mixer while ESCs are being calibrated if (_outputs_disabled) { return 0; } + //PX4_INFO("VOXL2_IO: Mixer output: %u, %u, %u, %u", outputs[0], outputs[1], outputs[2], outputs[3]); + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) //So, if Run() is blocked by a custom command, this function will not be called until Run is running again - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int32_t _fb_idx = -1; + uint32_t output_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) { - PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); + PX4_ERR("VOXL2_IO: Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); return false; } @@ -333,43 +305,20 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _pwm_on = true; } - if (!_pwm_on || stop_motors) { - _rate_req[i] = 0; + //Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right? + if (0) { //(!_pwm_on || stop_motors) { + output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns } else { - _rate_req[i] = outputs[i]; + output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns } - - _pwm_values[i] = _rate_req[i]; } Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); + cmd.len = voxl2_io_create_hires_pwm_packet(output_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_pwm_on && _debug) { - PX4_INFO("Mixer outputs"); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5], - outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11], - outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17] - ); - - // Debug messages for PWM 400Hz values sent to M0065 - uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1); - uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2); - uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3); - uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4); - PX4_INFO(""); - } - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Failed to send packet"); return false; } else { @@ -377,40 +326,39 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _packets_sent++; } + //if (_pwm_on && _debug){ + if (_debug) { + PX4_INFO("VOXL2_IO: Mixer outputs: [%u, %u, %u, %u, %u, %u, %u, %u]", + outputs[0], outputs[1], outputs[2], outputs[3], + outputs[4], outputs[5], outputs[6], outputs[7]); + } + perf_count(_output_update_perf); return true; } -int Voxl2IO::flush_uart_rx() -{ - while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} - - return 0; -} - static bool valid_port(int port) { - if (port == 2 || port == 6 || port == 7) { return true; } + if (port == 2 || port == 6 || port == 7) { + return true; + } return false; } +/* int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) { for (int i = 0; i < len; i++) { - int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet); + int16_t ret = voxl2_io_packet_process_char(buf[i], &_voxl2_io_packet); if (ret > 0) { - uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet); - uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet); + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { return 0; - - } else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) { - return 0; - } else { return -1; } @@ -418,36 +366,31 @@ int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) } else { //parser error switch (ret) { case VOXL2_IO_ERROR_BAD_CHECKSUM: - if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet checksum"); break; case VOXL2_IO_ERROR_BAD_LENGTH: - if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet length"); break; case VOXL2_IO_ERROR_BAD_HEADER: - if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet header"); break; case VOXL2_IO_NO_PACKET: - // if(_pwm_on) PX4_WARN("NO packet"); + // if(_pwm_on) PX4_WARN("VOXL2_IO: NO packet"); break; default: - if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: Unknown error: %i", ret); break; } - - return ret; } } return 0; } +*/ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -455,6 +398,8 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, unsigned frame_drops, int rssi, input_rc_s &input_rc) { // fill rc_in struct for publishing + memset(&input_rc, 0, sizeof(input_rc)); //zero out the struct first + input_rc.channel_count = raw_rc_count_local; if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { @@ -474,9 +419,9 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _raw_rc_values[i] = UINT16_MAX; } - input_rc.timestamp = now; + input_rc.timestamp = now; input_rc.timestamp_last_signal = input_rc.timestamp; - input_rc.rc_ppm_frame_length = 0; + input_rc.rc_ppm_frame_length = 0; /* fake rssi if no value was provided */ if (rssi == -1) { @@ -494,131 +439,98 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _sbus_frame_drops++; } - input_rc.rc_failsafe = failsafe; - input_rc.rc_lost = input_rc.rc_failsafe; - input_rc.rc_lost_frame_count = _sbus_frame_drops; + input_rc.rssi_dbm = 0.0f; + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = input_rc.rc_failsafe; + input_rc.rc_lost_frame_count = _sbus_frame_drops; input_rc.rc_total_frame_count = ++_sbus_total_frames; } -int Voxl2IO::receive_sbus() + +int Voxl2IO::parse_sbus_packet(uint8_t *raw_data, uint32_t data_len) { - int res = 0; - int header = -1; - int read_retries = 3; - int read_succeeded = 0; - voxl2_io_packet_init(&_sbus_packet); + input_rc_s input_rc; + uint16_t num_values; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); + hrt_abstime t_now = hrt_absolute_time(); - while (read_retries) { - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; - break; - } - } + bool rc_updated = sbus_parse(t_now, raw_data, data_len, _raw_rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - /* Try again in a bit if packet header not present yet... */ - if (header == -1) { - if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); } + if (rc_updated) { + //if (_pwm_on && _debug){ + if (_debug) { + //PX4_INFO("VOXL2_IO: Decoded packet, header pos: %i", header); + PX4_INFO("VOXL2_IO: [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], + _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], + _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], + _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], + _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], + _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] + ); - read_retries--; - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) { - if (_pwm_on && _debug) { - PX4_ERR("Error parsing QC RAW SBUS packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - break; - } - - input_rc_s input_rc; - uint16_t num_values; - bool sbus_failsafe = false; - bool sbus_frame_drop = false; - uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); - hrt_abstime now = hrt_absolute_time(); - bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values, - &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - - if (rc_updated) { - if (_pwm_on && _debug) { - PX4_INFO("Decoded packet, header pos: %i", header); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], - _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], - _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], - _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], - _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], - _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] - ); - } - - input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; - fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); - - if (!input_rc.rc_lost && !input_rc.rc_failsafe) { - _rc_last_valid = input_rc.timestamp; - } - - input_rc.timestamp_last_signal = _rc_last_valid; - _rc_pub.publish(input_rc); - - _bytes_received += res; - _packets_received++; - read_succeeded = 1; - break; - - } else if (_pwm_on && _debug) { - PX4_ERR("Failed to decode SBUS packet, header pos: %i", header); - - if (sbus_frame_drop) { - PX4_WARN("SBUS frame dropped"); - } - - PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); + if (sbus_frame_drop) { + PX4_WARN("VOXL2_IO: SBUS frame dropped"); } } - read_retries--; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + fill_rc_in(num_values, _raw_rc_values, t_now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); + + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + _rc_last_valid_time = input_rc.timestamp; + } + + input_rc.timestamp_last_signal = _rc_last_valid_time; + _rc_pub.publish(input_rc); + + if (_rc_mode == RC_MODE::SCAN) { + PX4_INFO("VOXL2_IO: Detected VOXL2 IO SBUS RC"); + _rc_mode = RC_MODE::SBUS; + } } - if (! read_succeeded) { - _new_packet = false; - return -EIO; + return 0; +} + + +int Voxl2IO::receive_uart_packets() +{ + int nread = _uart_port.read(_read_buf, READ_BUF_SIZE); + + if (nread > 0) { + if (_debug) { + PX4_INFO("VOXL2_IO: receive_uart_packets read %d bytes", nread); + } + + _bytes_received += nread; + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + _packets_received++; + + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { + + //parse SBUS packet only if configured to do so + if ((_rc_mode == RC_MODE::SCAN) || (_rc_mode == RC_MODE::SBUS)) { + parse_sbus_packet(&_voxl2_io_packet.buffer[SBUS_PAYLOAD], SBUS_FRAME_SIZE); + } + } + + //parse other packets (future use) + } + } } - _new_packet = true; return 0; } @@ -635,26 +547,14 @@ void Voxl2IO::Run() perf_begin(_cycle_perf); - /* Handle RC */ - if (_rc_mode == RC_MODE::SCAN) { - if (receive_sbus() == PX4_OK) { - PX4_INFO("Found M0065 SBUS RC."); - _rc_mode = RC_MODE::SBUS; - } // Add more cases here for other protocols in the future.. - - } else if (_rc_mode == RC_MODE::SBUS) { - receive_sbus(); - } - - /* Only update outputs if we have new values from RC */ - if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) { - _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module - _new_packet = false; - } + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update PWM status if armed or if disarmed PWM values are set */ _pwm_on = _mixing_output.armed().armed; + //receive packets from voxl_io board + receive_uart_packets(); + /* check for parameter updates */ if (!_pwm_on && _parameter_update_sub.updated()) { /* clear update */ @@ -665,48 +565,6 @@ void Voxl2IO::Run() update_params(); } - /* Don't process commands if pwm on */ - if (!_pwm_on) { - if (_current_cmd.valid()) { - PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us); - flush_uart_rx(); - - do { - PX4_INFO("CMDs left %d", _current_cmd.repeats); - - if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { - if (_current_cmd.repeats == 0) { - _current_cmd.clear(); - } - - - } else { - _bytes_sent += _current_cmd.len; - _packets_sent++; - - if (_current_cmd.retries == 0) { - _current_cmd.clear(); - PX4_ERR("Failed to send command, errno: %i", errno); - - } else { - _current_cmd.retries--; - PX4_ERR("Failed to send command, errno: %i", errno); - } - } - - px4_usleep(_current_cmd.repeat_delay_us); - } while (_current_cmd.repeats-- > 0); - - } else { - Command *new_cmd = _pending_cmd.load(); - - if (new_cmd) { - _current_cmd = *new_cmd; - _pending_cmd.store(nullptr); - } - } - } - /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ _mixing_output.updateSubscriptions(true); perf_end(_cycle_perf); @@ -728,17 +586,17 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'v': - PX4_INFO("Verbose mode enabled"); + PX4_INFO("VOXL2_IO: Verbose mode enabled"); get_instance()->_debug = true; break; case 'd': - PX4_INFO("M0065 PWM outputs disabled"); + PX4_INFO("VOXL2_IO: M0065 PWM outputs disabled"); get_instance()->_outputs_disabled = true; break; case 'e': - PX4_INFO("M0065 using external RC"); + PX4_INFO("VOXL2_IO: M0065 using external RC"); get_instance()->_rc_mode = RC_MODE::EXTERNAL; break; @@ -747,7 +605,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) snprintf(get_instance()->_device, 2, "%s", myoptarg); } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + PX4_ERR("VOXL2_IO: Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); _object.store(nullptr); _task_id = -1; return PX4_ERROR; @@ -756,7 +614,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) break; default: - print_usage("Unknown command, parsing flags"); + print_usage("VOXL2_IO: Unknown command, parsing flags"); break; } } @@ -766,7 +624,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } } else { - PX4_ERR("alloc failed"); + PX4_ERR("VOXL2_IO: alloc failed"); } _object.store(nullptr); @@ -776,280 +634,131 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } -bool Voxl2IO::stop_all_pwms() -{ - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _fb_idx = 0; - - Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); - return false; - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } - - return true; -} - -int Voxl2IO::send_cmd_thread_safe(Command *cmd) -{ - cmd->id = _cmd_id++; - _pending_cmd.store(cmd); - - /* wait until main thread processed it */ - while (_pending_cmd.load()) { - px4_usleep(1000); - } - - return 0; -} - int Voxl2IO::calibrate_escs() { - /* Disable outputs so Mixer isn't being a PITA while we calibrate */ + // Disable outputs so Mixer isn't being a PITA while we calibrate _outputs_disabled = true; Command cmd; - int32_t fb_idx = -1; - uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0}; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf, - sizeof(cmd.buf)); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet"); - _outputs_disabled = false; - return -1; + PX4_INFO("VOXL2_IO: PWM ESC calibration is starting!"); + + // Give user 10 seconds to plug in PWM cable for ESCs + PX4_INFO("VOXL2_IO: MIN PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: MAX PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_max); + PX4_INFO("VOXL2_IO:"); + PX4_INFO("VOXL2_IO: Connect your ESCs! (Calibration will start in ~10 seconds)"); + + px4_usleep(10000000); + + uint32_t max_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint32_t min_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + + for (int idx = 0; idx < VOXL2_IO_OUTPUT_CHANNELS; idx++) { + //only send out calibration pulse if the actuator is a motor + if ((_parameters.function_map[idx] >= 101) && (_parameters.function_map[idx] <= 112)) { + max_pwm_cmds[idx] = _parameters.pwm_cal_max * MIXER_OUTPUT_TO_CMD_SCALE; + min_pwm_cmds[idx] = _parameters.pwm_cal_min * MIXER_OUTPUT_TO_CMD_SCALE; + } } - /* Give user 10 seconds to plug in PWM cable for ESCs */ - PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)"); - hrt_abstime start_cal = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_cal) < 10000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled max pwms: %u %u %u %u %u %u %u %u", + max_pwm_cmds[0], max_pwm_cmds[1], max_pwm_cmds[2], max_pwm_cmds[3], + max_pwm_cmds[4], max_pwm_cmds[5], max_pwm_cmds[6], max_pwm_cmds[7]); } - /* PWM MAX 3 seconds */ - PX4_INFO("Writing PWM MAX for 3 seconds!"); - int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX}; + hrt_abstime start; - if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); } + // Send PWM max every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MAX (%dus) for 5 seconds!", _parameters.pwm_cal_max); + cmd.len = voxl2_io_create_hires_pwm_packet(max_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - int16_t led_cmd[4] {0, 0, 0, 0}; - cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MAX packet"); + _outputs_disabled = false; + return -1; + } - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet"); - _outputs_disabled = false; - return -1; - - } else { - cmd.clear(); + px4_usleep(2500); } - hrt_abstime start_pwm_max = hrt_absolute_time(); + // Send PWM min every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MIN (%dus) for 5 seconds!", _parameters.pwm_cal_min); - while (hrt_elapsed_time(&start_pwm_max) < 3000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled min pwms: %u %u %u %u %u %u %u %u", + min_pwm_cmds[0], min_pwm_cmds[1], min_pwm_cmds[2], min_pwm_cmds[3], + min_pwm_cmds[4], min_pwm_cmds[5], min_pwm_cmds[6], min_pwm_cmds[7]); } - /* PWM MIN 4 seconds */ - PX4_INFO("Writing PWM MIN for 4 seconds!"); - int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN}; + cmd.len = voxl2_io_create_hires_pwm_packet(min_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); } - cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet"); - _outputs_disabled = false; - return -1; + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MIN packet"); + _outputs_disabled = false; + return -1; + } + + px4_usleep(2500); } - hrt_abstime start_pwm_min = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_pwm_min) < 4000000) { - continue; - } - - PX4_INFO("ESC Calibration complete"); + PX4_INFO("VOXL2_IO: Waiting 5 seconds to finish the calibration (no PWM output)"); + px4_usleep(5000000); + PX4_INFO("VOXL2_IO: ESC Calibration complete"); _outputs_disabled = false; return 0; } int Voxl2IO::custom_command(int argc, char *argv[]) { - int myoptind = 0; - int ch; - const char *myoptarg = nullptr; - - Command cmd; - uint8_t output_channel = 0xF; - int16_t rate = 0; - - uint32_t repeat_count = 100; - uint32_t repeat_delay_us = 10000; - const char *verb = argv[argc - 1]; - if ((strcmp(verb, "pwm")) == 0 && argc < 3) { - return print_usage("pwm command: missing args"); - - } else if (argc < 1) { + if (argc < 1) { return print_usage("unknown command: missing args"); } - PX4_INFO("Executing the following command: %s", verb); + PX4_INFO("VOXL2_IO: Executing the following command: %s", verb); - /* start the FMU if not running */ + /* start if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { return Voxl2IO::task_spawn(argc, argv); } - } - if (!strcmp(verb, "status")) { - if (!is_running()) { - PX4_INFO("Not running"); - return -1; - } - - return get_instance()->print_status(); + PX4_INFO("VOXL2_IO: Already running"); + return 0; } if (!is_running()) { - PX4_INFO("Not running"); + PX4_INFO("VOXL2_IO: Not running"); return -1; } + if (!strcmp(verb, "status")) { + return get_instance()->print_status(); + } + + if (!strcmp(verb, "calibrate_escs")) { if (get_instance()->_outputs_disabled) { - PX4_WARN("Can't calibrate ESCs while outputs are disabled."); + PX4_WARN("VOXL2_IO: Can't calibrate ESCs while outputs are disabled."); return -1; } return get_instance()->calibrate_escs(); } - while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'c': - output_channel = atoi(myoptarg); - - if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) { - char reason[50]; - sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1); - print_usage(reason); - return 0; - } - - break; - - case 'n': - repeat_count = atoi(myoptarg); - - if (repeat_count < 1) { - print_usage("bad repeat_count"); - return 0; - } - - break; - - case 't': - repeat_delay_us = atoi(myoptarg); - - if (repeat_delay_us < 1) { - print_usage("bad repeat delay"); - return 0; - } - - break; - - case 'r': - rate = atoi(myoptarg); - break; - - case 'p': - if (valid_port(atoi(myoptarg))) { - snprintf(get_instance()->_device, 2, "%s", myoptarg); - - } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); - return 0; - } - - break; - - default: - print_usage("Unknown command, parsing flags"); - return 0; - } - } - - if (!strcmp(verb, "pwm")) { - PX4_INFO("Output channel: %i", output_channel); - PX4_INFO("Repeat count: %i", repeat_count); - PX4_INFO("Repeat delay (us): %i", repeat_delay_us); - PX4_INFO("Rate: %i", rate); - - if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) { - PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate); - int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t id_fb = 0; - - if (output_channel == - 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'. - rate_req[0] = rate; - rate_req[1] = rate; - rate_req[2] = rate; - rate_req[3] = rate; - - } else { - rate_req[output_channel] = rate; - id_fb = output_channel; - } - - cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3], - 0, 0, 0, 0, - id_fb, cmd.buf, sizeof(cmd.buf)); - - cmd.response = false; - cmd.repeats = repeat_count; - cmd.resp_delay_us = 1000; - cmd.repeat_delay_us = repeat_delay_us; - cmd.print_feedback = false; - - PX4_INFO("feedback id debug: %i", id_fb); - PX4_INFO("Sending UART M0065 power command %i", rate); - - if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet: stop PWMs"); - return -1; - - } else { - get_instance()->_bytes_sent += cmd.len; - get_instance()->_packets_sent++; - } - - } else { - print_usage("Invalid Output Channel, use 0-3"); - return 0; - } + if (!strcmp(verb, "enable_debug")) { + get_instance()->_debug = true; } return print_usage("unknown custom command"); @@ -1057,22 +766,20 @@ int Voxl2IO::custom_command(int argc, char *argv[]) int Voxl2IO::print_status() { - PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); - PX4_INFO("PWM Rate: 400 Hz"); // Only support 400 Hz for now - PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no"); - PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version); - PX4_INFO("RC Type: SBUS"); // Only support SBUS through M0065 for now - PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes"); - PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames); - PX4_INFO("UART port: %s", _device); - PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); - PX4_INFO("Packets sent: %" PRIu32, _packets_sent); - PX4_INFO(""); - PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]); - PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]); - PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]); - PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + //PX4_INFO("VOXL2_IO: Max update rate: %u Hz", 1000000/_current_update_interval); + //PX4_INFO("VOXL2_IO: PWM Rate: 400 Hz"); // Only support 400 Hz for now + PX4_INFO("VOXL2_IO: Outputs on : %s", _pwm_on ? "yes" : "no"); + PX4_INFO("VOXL2_IO: SW version : %u", _version_info.sw_version); + PX4_INFO("VOXL2_IO: HW version : %u: %s", _version_info.hw_version, + board_id_to_name(_version_info.hw_version).c_str()); + PX4_INFO("VOXL2_IO: RC Type : SBUS"); // Only support SBUS through M0065 for now + PX4_INFO("VOXL2_IO: RC Connected : %s", hrt_absolute_time() - _rc_last_valid_time > 500000 ? "no" : "yes"); + PX4_INFO("VOXL2_IO: RC Packets Rxd : %" PRIu16, _sbus_total_frames); + PX4_INFO("VOXL2_IO: UART port : %s", _device); + PX4_INFO("VOXL2_IO: UART open : %s", _uart_port.open() ? "yes" : "no"); + PX4_INFO("VOXL2_IO: Packets sent : %" PRIu32, _packets_sent); + PX4_INFO("VOXL2_IO: "); + print_params(); perf_print_counter(_cycle_perf); perf_print_counter(_output_update_perf); @@ -1103,17 +810,48 @@ px4io driver is used for main ones. PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); - PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false); - PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); - PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); - PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } +std::string Voxl2IO::board_id_to_name(int board_id) +{ + switch(board_id){ + case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)"); + case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"); + case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"); + case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)"); + case 35: return std::string("ModalAi I/O Expander (M0065)"); + case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)"); + case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)"); + case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)"); + case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)"); + case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)"); + case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)"); + case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)"); + default: return std::string("Unknown Board"); + } +} + +void Voxl2IO::print_params() +{ + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_BAUD : %" PRId32, _parameters.baud_rate); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC1 : %" PRId32, _parameters.function_map[0]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC2 : %" PRId32, _parameters.function_map[1]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC3 : %" PRId32, _parameters.function_map[2]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC4 : %" PRId32, _parameters.function_map[3]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC5 : %" PRId32, _parameters.function_map[4]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC6 : %" PRId32, _parameters.function_map[5]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC7 : %" PRId32, _parameters.function_map[6]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC8 : %" PRId32, _parameters.function_map[7]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_DIS : %" PRId32, _parameters.pwm_dis); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MIN : %" PRId32, _parameters.pwm_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MAX : %" PRId32, _parameters.pwm_max); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMIN : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMAX : %" PRId32, _parameters.pwm_cal_max); +} + extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]); int voxl2_io_main(int argc, char *argv[]) diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp index 6ac46251bc..b3458456a0 100644 --- a/src/drivers/voxl2_io/voxl2_io.hpp +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -49,16 +50,17 @@ #include #include #include +#include #include #include #include #include -#include "voxl2_io_serial.hpp" - #include "voxl2_io_packet.h" #include "voxl2_io_packet_types.h" +using namespace device; + using namespace time_literals; class Voxl2IO final : public ModuleBase, public OutputModuleInterface @@ -85,8 +87,8 @@ public: virtual int init(); - void update_pwm_config(); int get_version_info(); + void print_params(); struct Command { uint16_t id = 0; @@ -105,8 +107,8 @@ public: void clear() { len = 0; } }; - int send_cmd_thread_safe(Command *cmd); - int receive_sbus(); + int receive_uart_packets(); + int parse_sbus_packet(uint8_t *raw_data, uint32_t data_len); void fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -114,53 +116,42 @@ public: unsigned frame_drops, int rssi, input_rc_s &input_rc); private: void Run() override; - bool stop_all_pwms(); /* PWM Parameters */ - static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off - static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes - static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1; - static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; - static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4; - static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0; - - static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200; - static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000; - - static constexpr uint16_t DISARMED_VALUE = 0; - - static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0; - static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800; - static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0; - static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0; - - static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000; - static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000; - static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900; - static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks + static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; + static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 8; + static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = + UINT16_MAX; //this will tell mixer to use the default failsafe depending on the function /* SBUS */ static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30; static constexpr uint16_t SBUS_PAYLOAD = 3; + //packet packing function accepts pulse width in nanoseconds + //assuming the mixer outputs in microseconds, need to convert + static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 1000; //standard pwm + //static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 125; //oneshot125 + /* M0065 version info */ - static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6; - static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1; - static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35; - VOXL2_IO_VERSION_INFO _version_info; + static constexpr uint16_t VOXL2_IO_SW_VERSION = 2; + static constexpr uint16_t VOXL2_IO_HW_VERSION = 35; //this is the version of the HW + int _board_detect_retries{3}; + VOXL2_IO_EXTENDED_VERSION_INFO _version_info; /* Module update interval */ static constexpr unsigned _current_update_interval{4000}; // 250 Hz typedef struct { - int32_t config{VOXL2_IO_CONFIG}; int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD}; - int32_t pwm_min{VOXL2_IO_DEFAULT_MIN}; - int32_t pwm_max{VOXL2_IO_DEFAULT_MAX}; - int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; - int32_t param_rc_input_proto{0}; + int32_t pwm_min{0}; + int32_t pwm_max{0}; + int32_t pwm_dis{0}; + int32_t pwm_cal_min{0}; + int32_t pwm_cal_max{0}; + //int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; + //int32_t param_rc_input_proto{0}; int32_t param_rc_rssi_pwm_chan{0}; - int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0, 0, 0, 0, 0}; int32_t verbose_logging{0}; } voxl2_io_params_t; voxl2_io_params_t _parameters; @@ -179,20 +170,18 @@ private: SCAN } _rc_mode{RC_MODE::SCAN}; - /* QUP7, VOXL2 J19, /dev/slpi-uart-7*/ char _device[10] {VOXL2_IO_DEFAULT_PORT}; - Voxl2IoSerial *_uart_port; + Serial _uart_port{}; /* Mixer output */ MixingOutput _mixing_output; /* RC input */ - VOXL2_IOPacket _sbus_packet; - uint64_t _rc_last_valid; + VOXL2_IOPacket _voxl2_io_packet; + uint64_t _rc_last_valid_time; uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX}; - unsigned _sbus_frame_drops{0}; - uint16_t _sbus_total_frames{0}; - bool _new_packet{false}; + unsigned int _sbus_frame_drops{0}; + uint32_t _sbus_total_frames{0}; /* Publications */ uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; @@ -201,17 +190,12 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; bool _pwm_on{false}; - int32_t _pwm_fullscale{0}; - int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; bool _outputs_disabled{false}; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; bool _debug{false}; - uint16_t _cmd_id{0}; - Command _current_cmd; - px4::atomic _pending_cmd{nullptr}; static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; @@ -220,9 +204,8 @@ private: uint32_t _packets_sent{0}; uint32_t _packets_received{0}; - int parse_response(uint8_t *buf, uint8_t len); int load_params(voxl2_io_params_t *params); int update_params(); - int flush_uart_rx(); int calibrate_escs(); + std::string board_id_to_name(int board_id); }; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c index 3de26acbfe..99340fa974 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.c +++ b/src/drivers/voxl2_io/voxl2_io_packet.c @@ -150,6 +150,32 @@ int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); } +typedef struct { + uint8_t command_type; + //uint8_t channel_offset; + uint16_t vals[8]; //could be 1,2,4,6,8 +} __attribute__((__packed__)) pwm_hires_cmd_t; + +pwm_hires_cmd_t hires_cmd; + +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size) +{ + if (cmd_cnt > 8) { + return -1; + } + + hires_cmd.command_type = 0; + //hires_cmd.channel_offset = 0; + + //resolution of commands in the packet is 0.05us = 50ns + for (uint32_t idx = 0; idx < cmd_cnt; idx++) { + hires_cmd.vals[idx] = pwm_val_ns[idx] / 50; + } + + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_cmd, (cmd_cnt * 2 + 1), out, + out_size); +} + int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) { uint16_t packet_size = size + 5; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h index 7e6237017f..1c4d242814 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.h +++ b/src/drivers/voxl2_io/voxl2_io_packet.h @@ -213,6 +213,9 @@ int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, in uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, uint8_t *out, uint16_t out_size); +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size); + + // Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) // Return value is the length of generated packet (if positive), otherwise error code int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h index 150982eca4..12766fb0ef 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet_types.h +++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h @@ -46,6 +46,7 @@ #define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3 #define VOXL2_IO_PACKET_TYPE_STEP_CMD 4 #define VOXL2_IO_PACKET_TYPE_LED_CMD 5 +#define VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD 6 #define VOXL2_IO_PACKET_TYPE_RESET_CMD 10 #define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11 #define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12 diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c index 704cca989e..ac99723b8d 100644 --- a/src/drivers/voxl2_io/voxl2_io_params.c +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -32,34 +32,73 @@ ****************************************************************************/ /** - * UART M0065 baud rate + * VOXL2_IO UART baud rate * - * Default rate is 921600, which is used for communicating with M0065. + * Default rate is 921600, which is used for communicating with VOXL2_IO board * * @group VOXL2 IO * @unit bit/s */ PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600); + /** - * M0065 PWM Min + * VOXL2_IO Disabled PWM * - * Minimum duration (microseconds) for M0065 PWM + * Pulse duration in disabled state (microseconds) for VOXL2_IO board * * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ -PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); +PARAM_DEFINE_INT32(VOXL2_IO_DIS, 1000); /** - * M0065 PWM Max + * VOXL2_IO Min PWM * - * Maximum duration (microseconds) for M0065 PWM + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1100); + +/** + * VOXL2_IO Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); + + +/** + * VOXL2_IO Calibration Min PWM + * + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_CMIN, 1050); + +/** + * VOXL2_IO Calibration Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_CMAX, 2000); diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp deleted file mode 100644 index cee0923ece..0000000000 --- a/src/drivers/voxl2_io/voxl2_io_serial.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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 "string.h" -#include "voxl2_io_serial.hpp" - -Voxl2IoSerial::Voxl2IoSerial() -{ -} - -Voxl2IoSerial::~Voxl2IoSerial() -{ - if (_uart_fd >= 0) { - uart_close(); - } -} - -int Voxl2IoSerial::uart_open(const char *dev, speed_t speed) -{ - if (_uart_fd >= 0) { - PX4_ERR("Port in use: %s (%i)", dev, errno); - return -1; - } - - /* Open UART */ -#ifdef __PX4_QURT - _uart_fd = qurt_uart_open(dev, speed); -#else - _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); -#endif - - if (_uart_fd < 0) { - PX4_ERR("Error opening port: %s (%i)", dev, errno); - return -1; - } - -#ifndef __PX4_QURT - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { - PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); - uart_close(); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &_cfg); - - /* Disable output post-processing */ - _cfg.c_oflag &= ~OPOST; - - _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ - _cfg.c_cflag &= ~CSIZE; - _cfg.c_cflag |= CS8; /* 8-bit characters */ - _cfg.c_cflag &= ~PARENB; /* no parity bit */ - _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ - _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ - - /* setup for non-canonical mode */ - _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); - _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); - - if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { - PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); - uart_close(); - return -1; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { - PX4_ERR("Error configuring port: %s (tcsetattr)", dev); - uart_close(); - return -1; - } - -#endif - - _speed = speed; - - return 0; -} - -int Voxl2IoSerial::uart_set_baud(speed_t speed) -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - return -1; - } - - if (cfsetispeed(&_cfg, speed) < 0) { - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { - return -1; - } - - _speed = speed; - - return 0; -#endif - - return -1; -} - -int Voxl2IoSerial::uart_close() -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - PX4_ERR("invalid state for closing"); - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { - PX4_ERR("failed restoring uart to original state"); - } - - if (close(_uart_fd)) { - PX4_ERR("error closing uart"); - } - -#endif - - _uart_fd = -1; - - return 0; -} - -int Voxl2IoSerial::uart_write(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for writing or buffer"); - return -1; - } - -#ifdef __PX4_QURT - return qurt_uart_write(_uart_fd, (const char *) buf, len); -#else - return write(_uart_fd, buf, len); -#endif -} - -int Voxl2IoSerial::uart_read(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for reading or buffer"); - return -1; - } - -#ifdef __PX4_QURT -#define ASYNC_UART_READ_WAIT_US 2000 - // The UART read on SLPI is via an asynchronous service so specify a timeout - // for the return. The driver will poll periodically until the read comes in - // so this may block for a while. However, it will timeout if no read comes in. - return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); -#else - return read(_uart_fd, buf, len); -#endif -} diff --git a/src/drivers/voxl2_io/voxl2_io_serial.hpp b/src/drivers/voxl2_io/voxl2_io_serial.hpp deleted file mode 100644 index 638bdef288..0000000000 --- a/src/drivers/voxl2_io/voxl2_io_serial.hpp +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#ifdef __PX4_QURT -#include -#define FAR -#endif - -class Voxl2IoSerial -{ -public: - Voxl2IoSerial(); - virtual ~Voxl2IoSerial(); - - int uart_open(const char *dev, speed_t speed); - int uart_set_baud(speed_t speed); - int uart_close(); - int uart_write(FAR void *buf, size_t len); - int uart_read(FAR void *buf, size_t len); - bool is_open() { return _uart_fd >= 0; }; - int uart_get_baud() {return _speed; } - -private: - int _uart_fd = -1; - -#if ! defined(__PX4_QURT) - struct termios _orig_cfg; - struct termios _cfg; -#endif - - int _speed = -1; -}; From 5ffa69ff54f338d09b1304930ae00883859c0560 Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Fri, 14 Feb 2025 15:16:17 -0500 Subject: [PATCH 41/63] fix newline in module.yaml Signed-off-by: dirksavage88 --- src/drivers/voxl2_io/module.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 17feaba8c5..86dc48ca58 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -10,4 +10,3 @@ actuator_output: group_label: 'PWMs' channel_label: 'PWM Channel' num_channels: 8 - From 01549a58326c47cf562dc190f7fa6eaf966516cc Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Fri, 14 Feb 2025 15:24:34 -0500 Subject: [PATCH 42/63] fix to kconfig newline Signed-off-by: dirksavage88 --- src/drivers/voxl2_io/Kconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig index 15964deeeb..622cad470d 100644 --- a/src/drivers/voxl2_io/Kconfig +++ b/src/drivers/voxl2_io/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO bool "voxl2_io" default n ---help--- - Enable support for voxl2_io \ No newline at end of file + Enable support for voxl2_io From 024dd701fb17089b9a2d2ccce1abbc6e3aa5b23b Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 17 Feb 2025 11:11:37 -0800 Subject: [PATCH 43/63] readme: update maintainers information (#24305) * creates new maintainers file per oss best practices * adds Pedro as Space maintainer --- MAINTAINERS.md | 42 ++++++++++++++++++++++++++++++++++++++++++ README.md | 25 ++----------------------- 2 files changed, 44 insertions(+), 23 deletions(-) create mode 100644 MAINTAINERS.md diff --git a/MAINTAINERS.md b/MAINTAINERS.md new file mode 100644 index 0000000000..426852e7b2 --- /dev/null +++ b/MAINTAINERS.md @@ -0,0 +1,42 @@ +Maintainers +=========== + +See [the documentation on Maintainers](https://docs.px4.io/main/en/contribute/maintainers.html) to learn about the role of the maintainers and the process to become one. + +**Active Maintainers** + +| Name | Sector | GitHub | Chat | email +|-------------------------|--------|--------|------|---------------- +| Lorenz Meier | Founder | [LorenzMeier][LorenzMeier] | | +| Daniel Agar | Architecture | [dagar][dagar] | daniel_agar | +| Beat Küng | Architecture | [bkueng][bkueng] | beatkueng | +| Ramón Roche | CI / Testing | [mrpollo][mrpollo] | rroche | +| Mathieu Bresciani | State Estimation | [bresch][bresch] | mbresch | +| Paul Riseborough | State Estimation | [priseborough][priseborough] | | +| David Sidrane | RTOS / NuttX | [davids5][davids5] | david_s5 | +| Jayoung Lim | Simulation | [Jaeyoung-Lim][Jaeyoung-Lim] | jaeyounglim. | +| Beniamino Pozzan | ROS 2 | [beniaminopozzan][beniaminopozzan] | beniaminopozzan | +| Matthias Grob | Multirotor | [MaEtUgR][MaEtUgR] | maetugr | +| Silvan Fuhrer | Fixed-Wing / VTOL | [sfuhrer][sfuhrer] | sfuhrer | +| Christian Friedrich | Rover | [chfriedrich98][chfriedrich98] | christian982564 | +| Pedro Roque | Spacecraft | [Pedro-Roque][Pedro-Roque] | .pedroroque | + + +**Documentation Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Hamish Willee | [hamishwillee][hamishwillee] | hamishwillee | + +**Release Managers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Ramón Roche | [mrpollo][mrpollo] | rroche | +| Daniel Agar | [dagar][dagar] | daniel_agar | + +**Retired Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| | | | diff --git a/README.md b/README.md index aef9e8f8a5..dac25f0ea1 100644 --- a/README.md +++ b/README.md @@ -44,30 +44,9 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con ## Maintenance Team -Note: This is the source of truth for the active maintainers of PX4 ecosystem. +See the latest list of maintainers on [MAINTAINERS](MAINTAINERS.md) file at the root of the project. -| Sector | Maintainer | -|---|---| -| Founder | [Lorenz Meier](https://github.com/LorenzMeier) | -| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)| -| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) | -| OS/NuttX | [David Sidrane](https://github.com/davids5) | -| Drivers | [Daniel Agar](https://github.com/dagar) | -| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) | -| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) | -| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) | -| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) | - -| Vehicle Type | Maintainer | -|---|---| -| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) | -| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) | -| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) | -| Rover | [Christian Friedrich](https://github.com/chfriedrich98) | -| Boat | x | - - -See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date. +For the latest stats on contributors please see the latest stats for the Dronecode ecosystem in our project dashboard under [LFX Insights](https://insights.lfx.linuxfoundation.org/foundation/dronecode). For information on how to update your profile and affiliations please see the following support link on how to [Complete Your LFX Profile](https://docs.linuxfoundation.org/lfx/my-profile/complete-your-lfx-profile). Dronecode publishes a yearly snapshot of contributions and achievements on its [website under the Reports section](https://dronecode.org). ## Supported Hardware From e12c3c00a4e658a0206735eb7402afbbb0242b20 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Feb 2025 14:35:15 -0500 Subject: [PATCH 44/63] control_allocator: param update avoid temporary - this is a harmless workaround for a GCC warning (-Wdangling-pointer) false positive --- .../ActuatorEffectivenessControlSurfaces.cpp | 6 +----- .../ActuatorEffectivenessControlSurfaces.hpp | 2 +- .../ActuatorEffectivenessHelicopter.cpp | 7 +++---- .../ActuatorEffectivenessHelicopter.hpp | 2 +- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 7 +++---- .../ActuatorEffectivenessHelicopterCoaxial.hpp | 2 +- 6 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 23dee70563..46fea5f33e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -69,15 +69,11 @@ void ActuatorEffectivenessControlSurfaces::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_count_handle, &count) != 0) { + if (param_get(_count_handle, &_count) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _count = count; - for (int i = 0; i < _count; i++) { param_get(_param_handles[i].type, (int32_t *)&_params[i].type); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 7047363cc8..5e64e5a738 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -107,7 +107,7 @@ private: param_t _count_handle; Params _params[MAX_COUNT] {}; - int _count{0}; + int32_t _count{0}; SlewRate _flaps_setpoint_with_slewrate; SlewRate _spoilers_setpoint_with_slewrate; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 643b218c47..beacc7007b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -73,14 +73,13 @@ void ActuatorEffectivenessHelicopter::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)3, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 93083fa066..669bb725e6 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -58,7 +58,7 @@ public: struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; - int num_swash_plate_servos{0}; + int32_t num_swash_plate_servos{0}; float throttle_curve[NUM_CURVE_POINTS]; float pitch_curve[NUM_CURVE_POINTS]; float yaw_collective_pitch_scale; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 0c06f5963f..40e956df99 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -60,14 +60,13 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)2, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index a507aee2dd..e97c31c0da 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -55,7 +55,7 @@ public: struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; - int num_swash_plate_servos{0}; + int32_t num_swash_plate_servos{0}; float spoolup_time; }; From 46609d5e6cc1af75c015a9cba09d196beda010cb Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 17 Feb 2025 18:55:24 -0900 Subject: [PATCH 45/63] voxl: document enable_debug command (#24353) --- src/drivers/voxl2_io/voxl2_io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 90adfb8422..3461c99a4b 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -810,6 +810,7 @@ px4io driver is used for main ones. PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); + PRINT_MODULE_USAGE_COMMAND_DESCR("enable_debug", "Enables driver debugging"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; From 38de04a53a66441a65664bfbdbf4b6ce850e4882 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Feb 2025 23:06:24 -0500 Subject: [PATCH 46/63] Tools/setup/ubuntu.sh: add curl for NXP mr-canhubk3 build (#24346) --- Tools/setup/ubuntu.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 434b2a81cd..e641a82da3 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -116,6 +116,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then binutils-dev \ bison \ build-essential \ + curl \ flex \ g++-multilib \ gcc-arm-none-eabi \ From 90b229070050af72352b3e8b7add2ecc5ac45d04 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 18 Feb 2025 00:26:06 -0500 Subject: [PATCH 47/63] Tools: docker_run.sh fix entrypoint as user - modern docker can specify the user at runtime (--user) --- Tools/docker_run.sh | 2 +- Tools/setup/docker-entrypoint.sh | 16 +--------------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index c78b9cc7e1..46888cd066 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -47,6 +47,7 @@ CCACHE_DIR=${HOME}/.ccache mkdir -p "${CCACHE_DIR}" docker run -it --rm -w "${SRC_DIR}" \ + --user="$(id -u):$(id -g)" \ --env=AWS_ACCESS_KEY_ID \ --env=AWS_SECRET_ACCESS_KEY \ --env=BRANCH_NAME \ @@ -54,7 +55,6 @@ docker run -it --rm -w "${SRC_DIR}" \ --env=CI \ --env=CODECOV_TOKEN \ --env=COVERALLS_REPO_TOKEN \ - --env=LOCAL_USER_ID="$(id -u)" \ --env=PX4_ASAN \ --env=PX4_MSAN \ --env=PX4_TSAN \ diff --git a/Tools/setup/docker-entrypoint.sh b/Tools/setup/docker-entrypoint.sh index 1094587c1c..0412558902 100755 --- a/Tools/setup/docker-entrypoint.sh +++ b/Tools/setup/docker-entrypoint.sh @@ -1,6 +1,5 @@ #!/bin/bash -echo "[docker-entrypoint.sh] Starting entrypoint" # Start virtual X server in the background # - DISPLAY default is :99, set in dockerfile # - Users can override with `-e DISPLAY=` in `docker run` command to avoid @@ -17,17 +16,4 @@ if [ -n "${ROS_DISTRO}" ]; then source "/opt/ros/$ROS_DISTRO/setup.bash" fi -echo "[docker-entrypoint.sh] Working Directory: ${pwd}" - -# Use the LOCAL_USER_ID if passed in at runtime -if [ -n "${LOCAL_USER_ID}" ]; then - echo "[docker-entrypoint.sh] Starting with: $LOCAL_USER_ID:user" - # modify existing user's id - usermod -u $LOCAL_USER_ID user - - # run as user - # exec gosu user "$@" - exec "$@" -else - exec "$@" -fi +exec "$@" From d2cbe10243551b860ba06e745bf6dd9fb1d22dd7 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Tue, 18 Feb 2025 13:23:10 +0100 Subject: [PATCH 48/63] Clean up temperature msg fields (#24272) * remove temp field from airspeed.msg, adjust temp selection * temp-sensor hirarchy: airspeed, ext. baro, default value * directly use diff-press or baro temp in true-airspeed calc * improve clarity * add enum for temperature source in VehicleAirData.msg --- msg/Airspeed.msg | 2 - msg/VehicleAirData.msg | 4 +- src/drivers/telemetry/hott/messages.cpp | 2 +- src/drivers/uavcan/sensors/airspeed.cpp | 1 - .../airspeed_selector_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/streams/HIGHRES_IMU.hpp | 2 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 13 +++++- src/modules/sensors/sensors.cpp | 24 +---------- src/modules/sensors/sensors.hpp | 6 +-- .../vehicle_air_data/VehicleAirData.cpp | 40 +++++++++++-------- .../vehicle_air_data/VehicleAirData.hpp | 10 +++-- src/modules/simulation/simulator_sih/sih.cpp | 1 - 13 files changed, 50 insertions(+), 58 deletions(-) diff --git a/msg/Airspeed.msg b/msg/Airspeed.msg index aaed7b72ca..bc673cf0aa 100644 --- a/msg/Airspeed.msg +++ b/msg/Airspeed.msg @@ -5,6 +5,4 @@ float32 indicated_airspeed_m_s # indicated airspeed in m/s float32 true_airspeed_m_s # true filtered airspeed in m/s -float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown - float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/VehicleAirData.msg b/msg/VehicleAirData.msg index 59ca5e5c8d..41e5358ae8 100644 --- a/msg/VehicleAirData.msg +++ b/msg/VehicleAirData.msg @@ -6,10 +6,10 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 baro_device_id # unique device ID for the selected barometer float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. -float32 baro_temp_celcius # Temperature in degrees Celsius float32 baro_pressure_pa # Absolute pressure in Pascals +float32 ambient_temperature # Abient temperature in degrees Celsius +uint8 temperature_source # Source of temperature data: 0: Default Temperature (15°C), 1: External Baro, 2: Airspeed float32 rho # air density -float32 eas2tas # equivalent airspeed to true airspeed conversion factor uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 3be05d87a3..364d5005a7 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -150,7 +150,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(airdata.ambient_temperature + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index e62b29d5ea..2127817fd0 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -104,7 +104,6 @@ UavcanAirspeedBridge::ias_sub_cb(const report.timestamp = hrt_absolute_time(); report.indicated_airspeed_m_s = msg.indicated_airspeed; report.true_airspeed_m_s = _last_tas_m_s; - report.air_temperature_celsius = _last_outside_air_temp_k + atmosphere::kAbsoluteNullCelsius; publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index f8012529e1..03b9d48574 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -388,7 +388,7 @@ AirspeedModule::Run() input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; input_data.airspeed_timestamp = airspeed_raw.timestamp; - input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + input_data.air_temperature_celsius = _vehicle_air_data.ambient_temperature; if (_in_takeoff_situation) { // set flag to false if either speed is above stall speed, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aa8b50d01a..00c0b9ec93 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2669,7 +2669,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - airspeed.air_temperature_celsius = 15.f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf..7a3d96cb38 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -193,7 +193,7 @@ private: msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_pa; msg.pressure_alt = air_data.baro_alt_meter; - msg.temperature = air_data.baro_temp_celcius; + msg.temperature = air_data.ambient_temperature; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 6aca9f7d98..8623950e02 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -521,6 +522,7 @@ private: update_gps(); update_vehicle_status(); update_wind(); + update_vehicle_air_data(); } void update_airspeed() @@ -529,7 +531,6 @@ private: if (_airspeed_sub.update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); - _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); } } @@ -610,6 +611,15 @@ private: } } + void update_vehicle_air_data() + { + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _temperature.add_value(air_data.ambient_temperature, _update_rate_filtered); + } + } + void set_default_values(mavlink_high_latency2_t &msg) const { msg.airspeed = 0; @@ -659,6 +669,7 @@ private: uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _health_report_sub{ORB_ID(health_report)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..97ceb6db21 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -278,32 +278,15 @@ void Sensors::diff_pres_poll() vehicle_air_data_s air_data{}; _vehicle_air_data_sub.copy(&air_data); - - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } + const float temperature = air_data.ambient_temperature; // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; + float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f }; _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? // accumulate average for publication _diff_pres_timestamp_sum += diff_pres.timestamp_sample; _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _diff_pres_temperature_sum += air_temperature_celsius; _baro_pressure_sum += air_data.baro_pressure_pa; _diff_pres_count++; @@ -313,12 +296,10 @@ void Sensors::diff_pres_poll() const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; // reset _diff_pres_timestamp_sum = 0; _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; _baro_pressure_sum = 0; _diff_pres_count = 0; @@ -354,7 +335,6 @@ void Sensors::diff_pres_poll() airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 911005249c..3a8bcd50bc 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -90,11 +90,7 @@ using namespace sensors; using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f + class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 263a56090f..a12634055f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -45,6 +45,9 @@ using namespace matrix; using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; +static constexpr float DEFAULT_TEMPERATURE_CELSIUS = 15.f; +static constexpr float TEMPERATURE_MIN_CELSIUS = -60.f; +static constexpr float TEMPERATURE_MAX_CELSIUS = 60.f; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), @@ -77,21 +80,23 @@ void VehicleAirData::Stop() } } -void VehicleAirData::AirTemperatureUpdate() +float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, + const hrt_abstime time_now_us) { + // use the temperature from the differential pressure sensor if available + // otherwise use the temperature from the external barometer + // Temperature measurements from internal baros are not used as typically not representative for ambient temperature + float temperature = source == TemperatureSource::EXTERNAL_BARO ? temperature_baro : DEFAULT_TEMPERATURE_CELSIUS; differential_pressure_s differential_pressure; - static constexpr float temperature_min_celsius = -20.f; - static constexpr float temperature_max_celsius = 35.f; - - // update air temperature if data from differential pressure sensor is finite and not exactly 0 - // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight - if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) - && fabsf(differential_pressure.temperature) > FLT_EPSILON) { - - _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, - temperature_max_celsius); + if (_differential_pressure_sub.copy(&differential_pressure) + && time_now_us - differential_pressure.timestamp_sample < 1_s + && PX4_ISFINITE(differential_pressure.temperature)) { + temperature = differential_pressure.temperature; + source = TemperatureSource::AIRSPEED; } + + return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS); } bool VehicleAirData::ParametersUpdate(bool force) @@ -139,8 +144,6 @@ void VehicleAirData::Run() const bool parameter_update = ParametersUpdate(); - AirTemperatureUpdate(); - estimator_status_flags_s estimator_status_flags; const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); @@ -272,23 +275,26 @@ void VehicleAirData::Run() if (publish) { const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; - const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; + const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance]; + TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO : + TemperatureSource::DEFAULT_TEMP; + const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us); const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; out.timestamp_sample = timestamp_sample; out.baro_device_id = _calibration[instance].device_id(); out.baro_alt_meter = altitude; - out.baro_temp_celcius = temperature; + out.ambient_temperature = ambient_temperature; + out.temperature_source = static_cast(temperature_source); out.baro_pressure_pa = pressure_pa; out.rho = air_density; - out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON)); out.calibration_count = _calibration[instance].calibration_count(); out.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index a0bdb4007d..1e6555fcdd 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -73,9 +73,15 @@ public: void PrintStatus(); private: + enum TemperatureSource { + DEFAULT_TEMP = 0, + EXTERNAL_BARO = 1, + AIRSPEED = 2, + }; + void Run() override; - void AirTemperatureUpdate(); + float AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, const hrt_abstime time_now_us); void CheckFailover(const hrt_abstime &time_now_us); bool ParametersUpdate(bool force = false); void UpdateStatus(); @@ -124,8 +130,6 @@ private: int8_t _selected_sensor_sub_index{-1}; - float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature - bool _last_status_baro_fault{false}; DEFINE_PARAMETERS( diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 07c24fe642..2ebf936823 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -556,7 +556,6 @@ void Sih::send_airspeed(const hrt_abstime &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; airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); From 1f2dba68d28b6a819656568d4f34f949f09f385f Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:13:51 +0100 Subject: [PATCH 49/63] remove avoidance library and logic Signed-off-by: Silvan --- Makefile | 2 +- .../1014_gazebo-classic_iris_obs_avoid | 2 - .../init.d-posix/airframes/4006_gz_px4vision | 1 - .../init.d/airframes/4016_holybro_px4vision | 1 - .../airframes/4020_holybro_px4vision_v1_5 | 1 - msg/versioned/VehicleStatus.msg | 3 - src/lib/CMakeLists.txt | 1 - src/lib/avoidance/CMakeLists.txt | 39 --- src/lib/avoidance/ObstacleAvoidance.cpp | 271 ------------------ src/lib/avoidance/ObstacleAvoidance.hpp | 152 ---------- src/lib/avoidance/ObstacleAvoidanceTest.cpp | 242 ---------------- src/lib/avoidance/ObstacleAvoidance_dummy.hpp | 93 ------ src/modules/commander/Commander.cpp | 26 -- src/modules/commander/Commander.hpp | 3 - .../checks/systemCheck.cpp | 21 -- src/modules/commander/commander_params.c | 8 - .../tasks/Auto/CMakeLists.txt | 2 +- .../tasks/Auto/FlightTaskAuto.cpp | 17 -- .../tasks/Auto/FlightTaskAuto.hpp | 10 - .../FlightTaskManualPosition.hpp | 2 +- src/modules/logger/params.c | 2 +- 21 files changed, 4 insertions(+), 895 deletions(-) delete mode 100644 src/lib/avoidance/CMakeLists.txt delete mode 100644 src/lib/avoidance/ObstacleAvoidance.cpp delete mode 100644 src/lib/avoidance/ObstacleAvoidance.hpp delete mode 100644 src/lib/avoidance/ObstacleAvoidanceTest.cpp delete mode 100644 src/lib/avoidance/ObstacleAvoidance_dummy.hpp diff --git a/Makefile b/Makefile index f549e4c8ce..c65989ffe7 100644 --- a/Makefile +++ b/Makefile @@ -404,7 +404,7 @@ check_newlines: # Testing # -------------------------------------------------------------------- -.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance +.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard .PHONY: rostest python_coverage tests: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid index 8221969873..e3c25ec777 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid @@ -28,5 +28,3 @@ 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 COM_OBS_AVOID 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 65d290e0cc..967051b042 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -19,7 +19,6 @@ param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters -param set-default COM_OBS_AVOID 0 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d60de46e7b..9b86703c31 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index c9c3e5843a..e63a9eef01 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index a347dfce3d..062c6f0765 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -131,9 +131,6 @@ bool open_drone_id_system_healthy bool parachute_system_present bool parachute_system_healthy -bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter -bool avoidance_system_valid # Status of the obstacle avoidance system - bool rc_calibration_in_progress bool calibration_enabled diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1075ad4cd6..5fd36f546f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -34,7 +34,6 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) -add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) diff --git a/src/lib/avoidance/CMakeLists.txt b/src/lib/avoidance/CMakeLists.txt deleted file mode 100644 index c1da2a76dc..0000000000 --- a/src/lib/avoidance/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(avoidance - ObstacleAvoidance.cpp -) -target_link_libraries(avoidance PUBLIC hysteresis bezier) - -px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance) diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp deleted file mode 100644 index f818f068a0..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 ObstacleAvoidance.cpp - */ - -#include "ObstacleAvoidance.hpp" -#include "bezier/BezierN.hpp" - -using namespace matrix; -using namespace time_literals; - -/** Timeout in us for trajectory data to get considered invalid */ -static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; -/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */ -static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; -static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; - -ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : - ModuleParams(parent) -{ - _desired_waypoint = empty_trajectory_waypoint; - _failsafe_position.setNaN(); - _avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE); - _no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US); - -} - -void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) -{ - _sub_vehicle_status.update(); - _sub_vehicle_trajectory_waypoint.update(); - _sub_vehicle_trajectory_bezier.update(); - - const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get(); - const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get(); - - const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US; - const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime( - bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f); - const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout; - - const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; - const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0; - - _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid - && !avoidance_bezier_valid, hrt_absolute_time()); - - const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()); - - if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) { - // if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system - return; - } - - if (avoidance_invalid) { - if (_avoidance_activated) { - // Invalid point received: deactivate - PX4_WARN("Obstacle Avoidance system failed, loitering"); - _publishVehicleCmdDoLoiter(); - _avoidance_activated = false; - } - - if (!_failsafe_position.isAllFinite()) { - // save vehicle position when entering failsafe - _failsafe_position = _position; - } - - pos_sp = _failsafe_position; - vel_sp.setNaN(); - yaw_sp = NAN; - yaw_speed_sp = NAN; - - // Do nothing further - wait until activation - return; - - } else if (!_avoidance_activated) { - // First setpoint has been received: activate - PX4_INFO("Obstacle Avoidance system activated"); - _failsafe_position.setNaN(); - _avoidance_activated = true; - } - - if (avoidance_point_valid && !wp_msg_timeout) { - const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - pos_sp = Vector3f(point0.position); - vel_sp = Vector3f(point0.velocity); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = point0.yaw; - yaw_speed_sp = point0.yaw_speed; - } - - } else if (avoidance_bezier_valid && !bezier_msg_timeout) { - float yaw = NAN, yaw_speed = NAN; - _generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = yaw; - yaw_speed_sp = yaw_speed; - } - } -} - -void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, - float &yaw, float &yaw_velocity) -{ - const auto &msg = _sub_vehicle_trajectory_bezier.get(); - int bezier_order = msg.bezier_order; - matrix::Vector3f bezier_points[bezier_order]; - float bezier_yaws[bezier_order]; - - for (int i = 0; i < bezier_order; i++) { - bezier_points[i] = Vector3f(msg.control_points[i].position); - bezier_yaws[i] = msg.control_points[i].yaw; - } - - const float duration_s = msg.control_points[bezier_order - 1].delta; - const hrt_abstime now = hrt_absolute_time(); - const hrt_abstime start = msg.timestamp; - const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f); - - float T = NAN; - - if (bezier::calculateT(start, end, now, T) && - bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) && - bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity) - ) { - // translate bezier velocities T [0;1] into real velocities m/s - yaw_velocity /= duration_s; - velocity /= duration_s; - - } else { - PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); - } -} - - -void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, - const bool ext_yaw_active, const int wp_type) -{ - _desired_waypoint.timestamp = hrt_absolute_time(); - _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - _curr_wp = curr_wp; - _ext_yaw_active = ext_yaw_active; - - curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type; - - next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true; -} - -void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type) -{ - pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); - vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); - - _desired_waypoint = empty_trajectory_waypoint; -} - -void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp, - float target_acceptance_radius, const Vector2f &closest_pt) -{ - _position = pos; - position_controller_status_s pos_control_status = {}; - pos_control_status.timestamp = hrt_absolute_time(); - - // vector from previous triplet to current target - Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp); - // vector from previous triplet to the vehicle projected position on the line previous-target triplet - Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp); - // fraction of the previous-tagerget line that has been flown - const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length(); - - Vector2f pos_to_target = Vector2f(_curr_wp - _position); - - if (prev_curr_travelled > 1.0f) { - // if the vehicle projected position on the line previous-target is past the target waypoint, - // increase the target acceptance radius such that navigator will update the triplets - pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f; - } - - const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2)); - - bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z); - _no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time()); - - if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get() - && _no_progress_z_hysteresis.get_state()) { - // vehicle above or below the target waypoint - pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; - } - - _prev_pos_to_target_z = pos_to_target_z; - - // do not check for waypoints yaw acceptance in navigator - pos_control_status.yaw_acceptance = NAN; - - _pub_pos_control_status.publish(pos_control_status); -} - -void ObstacleAvoidance::_publishVehicleCmdDoLoiter() -{ - vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode - command.target_system = 1; - command.target_component = 1; - command.source_system = 1; - command.source_component = 1; - command.confirmation = false; - command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command - _pub_vehicle_command.publish(command); -} diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp deleted file mode 100644 index 8a27ffbbf1..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.hpp +++ /dev/null @@ -1,152 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 ObstacleAvoidance.hpp - * This class is used to inject the setpoints of an obstacle avoidance system - * into the FlightTasks - * - * @author Martina Rivizzigno - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, - { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} - } -}; - -class ObstacleAvoidance : public ModuleParams -{ -public: - ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance() = default; - - /** - * Inject setpoints from obstacle avoidance system into FlightTasks. - * @param pos_sp, position setpoint - * @param vel_sp, velocity setpoint - * @param yaw_sp, yaw setpoint - * @param yaw_speed_sp, yaw speed setpoint - */ - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp); - - /** - * Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight. - * @param curr_wp, current position triplet - * @param curr_yaw, current yaw triplet - * @param curr_yawspeed, current yaw speed triplet - * @param next_wp, next position triplet - * @param next_yaw, next yaw triplet - * @param next_yawspeed, next yaw speed triplet - * @param wp_type, current triplet type - */ - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type); - /** - * Updates the desired setpoints to send to the obstacle avoidance system. - * @param pos_sp, desired position setpoint computed by the active FlightTask - * @param vel_sp, desired velocity setpoint computed by the active FlightTask - * @param type, current triplet type - */ - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type); - - /** - * Checks the vehicle progress between previous and current position waypoint of the triplet. - * @param pos, vehicle position - * @param prev_wp, previous position triplet - * @param target_acceptance_radius, current position triplet xy acceptance radius - * @param closest_pt, closest point to the vehicle on the line previous-current position triplet - */ - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt); - -protected: - - uORB::SubscriptionData _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - - vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ - - uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ - uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ - uORB::Publication _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ - - matrix::Vector3f _curr_wp = {}; /**< current position triplet */ - matrix::Vector3f _position = {}; /**< current vehicle position */ - matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ - - bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */ - - systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */ - systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */ - - float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */ - - bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ - - /** - * Publishes vehicle command. - */ - void _publishVehicleCmdDoLoiter(); - void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity); - - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); - -}; diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp deleted file mode 100644 index 008a8758a9..0000000000 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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 - - -using namespace matrix; -// to run: make tests TESTFILTER=ObstacleAvoidance - -class ObstacleAvoidanceTest : public ::testing::Test -{ -public: - Vector3f pos_sp; - Vector3f vel_sp; - float yaw_sp = 0.123f; - float yaw_speed_sp = NAN; - void SetUp() override - - { - param_control_autosave(false); - param_reset_all(); - pos_sp = Vector3f(1.f, 1.2f, 0.1f); - vel_sp = Vector3f(0.3f, 0.4f, 0.1f); - } -}; - -class TestObstacleAvoidance : public ::ObstacleAvoidance -{ -public: - TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {} - void paramsChanged() {ObstacleAvoidance::updateParamsImpl();} - void test_setPosition(Vector3f &pos) {_position = pos;} -}; - -TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); } - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - message.timestamp = hrt_absolute_time(); - message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_bezier_s message {}; - message.timestamp = hrt_absolute_time(); - message.bezier_order = 2; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - vehicle_trajectory_bezier_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(2.6f, pos_sp(0)); - EXPECT_FLOAT_EQ(2.4f, pos_sp(1)); - EXPECT_LT(2.7f, pos_sp(2)); - EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests - EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0); - EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f); - EXPECT_FLOAT_EQ(0.23, yaw_sp); - EXPECT_FLOAT_EQ(yaw_speed_sp, 0); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - Vector3f pos(3.1f, 4.7f, 5.2f); - oa.test_setPosition(pos); - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints shouldn't be injected - EXPECT_FLOAT_EQ(pos(0), pos_sp(0)); - EXPECT_FLOAT_EQ(pos(1), pos_sp(1)); - EXPECT_FLOAT_EQ(pos(2), pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FALSE(PX4_ISFINITE(yaw_sp)); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_desired) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and the waypoints from FLightTaskAuto - TestObstacleAvoidance oa; - - pos_sp = Vector3f(1.f, 1.2f, NAN); - vel_sp = Vector3f(NAN, NAN, 0.7f); - int type = 4; - Vector3f curr_wp(1.f, 1.2f, 5.0f); - float curr_yaw = 1.02f; - float curr_yawspeed = NAN; - Vector3f next_wp(11.f, 1.2f, 5.0f); - float next_yaw = 0.82f; - float next_yawspeed = NAN; - bool ext_yaw_active = false; - - // WHEN: we inject the setpoints and waypoints in the interface - oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active, - type); - oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type); - - // WHEN: we subscribe to the uORB message out of the interface - uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 - EXPECT_FLOAT_EQ(pos_sp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]); - EXPECT_FLOAT_EQ(pos_sp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1])); - EXPECT_FLOAT_EQ(vel_sp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid); - - EXPECT_FLOAT_EQ(curr_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]); - EXPECT_FLOAT_EQ(curr_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]); - EXPECT_FLOAT_EQ(curr_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]); - EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed)); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid); - - EXPECT_FLOAT_EQ(next_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]); - EXPECT_FLOAT_EQ(next_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]); - EXPECT_FLOAT_EQ(next_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]); - EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed)); - EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid); - -} diff --git a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp b/src/lib/avoidance/ObstacleAvoidance_dummy.hpp deleted file mode 100644 index 7831c919fd..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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 ObstacleAvoidance_dummy.hpp - * This is a dummy class to reduce flash space for when obstacle avoidance is not required - * - * @author Julian Kent - */ - -#pragma once - -#include -#include - - -#include - - -class ObstacleAvoidance -{ -public: - ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor - - - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, - const int type) - { - notify_dummy(); - } - - - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt) - { - notify_dummy(); - }; - -protected: - - bool _logged_error = false; - void notify_dummy() - { - if (!_logged_error) { - PX4_ERR("Dummy avoidance library called!"); - _logged_error = true; - } - } -}; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a5e1583175..f0537c7c6e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1717,8 +1717,6 @@ void Commander::updateParameters() _vehicle_status.system_type = value_int32; } - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status) @@ -1799,9 +1797,6 @@ void Commander::run() _status_changed = true; } - /* Update OA parameter */ - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - handlePowerButtonState(); systemPowerUpdate(); @@ -2814,16 +2809,6 @@ void Commander::dataLinkCheck() _vehicle_status.open_drone_id_system_present = true; _vehicle_status.open_drone_id_system_healthy = healthy; } - - if (telemetry.heartbeat_component_obstacle_avoidance) { - if (_avoidance_system_lost) { - _avoidance_system_lost = false; - _status_changed = true; - } - - _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; - _vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy; - } } } @@ -2875,17 +2860,6 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = true; _status_changed = true; } - - // AVOIDANCE SYSTEM state check (only if it is enabled) - if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { - // if heartbeats stop - if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) - && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { - - _avoidance_system_lost = true; - _vehicle_status.avoidance_system_valid = false; - } - } } void Commander::battery_status_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 04c1a0d05a..395e7954cb 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -241,7 +241,6 @@ private: Hysteresis _auto_disarm_killed{false}; hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; @@ -268,7 +267,6 @@ private: bool _failsafe_user_override_request{false}; ///< override request due to stick movements bool _open_drone_id_system_lost{true}; - bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; bool _parachute_system_lost{true}; @@ -342,7 +340,6 @@ private: (ParamBool) _param_com_force_safety, (ParamFloat) _param_com_kill_disarm, (ParamBool) _param_com_mot_test_en, - (ParamBool) _param_com_obs_avoid, (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 32beaba9af..1f2592c9ed 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -121,27 +121,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // avoidance system - if (context.status().avoidance_system_required) { - if (context.status().avoidance_system_valid) { - reporter.setIsPresent(health_component_t::avoidance); - - } else { - /* EVENT - * @description - * - * This check can be configured via COM_OBS_AVOID parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"), - events::Log::Error, "Avoidance system not ready"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready"); - } - } - } - // VTOL in transition if (context.status().is_vtol && !context.isArmed()) { if (context.status().in_transition_mode) { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 37dcbdc9a9..44c2f07987 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -640,14 +640,6 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); */ PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0); -/** - * Flag to enable obstacle avoidance. - * - * @boolean - * @group Commander - */ -PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); - /** * Expect and require a healthy MAVLink parachute system * diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 2fadf2754d..905e08e80b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane) +target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility WeatherVane) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 8e19ffd5a3..614bcc6118 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -156,12 +156,6 @@ bool FlightTaskAuto::update() break; } - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; @@ -477,17 +471,6 @@ bool FlightTaskAuto::_evaluateTriplets() _updateInternalWaypoints(); } - if (_param_com_obs_avoid.get() - && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - (float)NAN, - _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); - _obstacle_avoidance.checkAvoidanceProgress( - _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); - } - // set heading _weathervane.update(); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23..6c49dd8726 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -53,13 +53,6 @@ #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" -// TODO: make this switchable in the board config, like a module -#if CONSTRAINED_FLASH -#include -#else -#include -#endif - /** * This enum has to agree with position_setpoint_s type definition * The only reason for not using the struct position_setpoint is because @@ -147,8 +140,6 @@ protected: AlphaFilter _yawspeed_filter; bool _yaw_sp_aligned{false}; - ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */ - PositionSmoothing _position_smoothing; Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; @@ -165,7 +156,6 @@ protected: (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, - (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index fb7f5dd4e5..b1f05092d3 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -68,5 +68,5 @@ private: uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention{this}; /**< collision prevention setpoint amendment */ }; diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 3d7e873854..e1bbd5145f 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators) * 5 : Debugging topics (debug_*.msg topics, for custom code) * 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data) - * 7 : Topics for computer vision and collision avoidance + * 7 : Topics for computer vision and collision prevention * 8 : Raw FIFO high-rate IMU (Gyro) * 9 : Raw FIFO high-rate IMU (Accel) * 10: Logging of mavlink tunnel message (useful for payload communication debugging) From b34a5eb6f7c590e5acccba356fc0c477daa8e59e Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:44:30 +0100 Subject: [PATCH 50/63] PositionControllerStatus: remove unused fields Remove yaw_acceptance and altitude_acceptance_radius fields as they were only filled by now removed avoidance controller. Signed-off-by: Silvan --- msg/PositionControllerStatus.msg | 2 -- .../fw_pos_control/FixedwingPositionControl.cpp | 2 -- src/modules/navigator/navigator_main.cpp | 14 -------------- .../rover_pos_control/RoverPositionControl.cpp | 1 - 4 files changed, 19 deletions(-) diff --git a/msg/PositionControllerStatus.msg b/msg/PositionControllerStatus.msg index 7237351fd0..44ec5412ba 100644 --- a/msg/PositionControllerStatus.msg +++ b/msg/PositionControllerStatus.msg @@ -7,6 +7,4 @@ float32 target_bearing # Bearing angle from aircraft to current target [rad] float32 xtrack_error # Signed track error [m] float32 wp_dist # Distance to active (next) waypoint [m] float32 acceptance_radius # Current horizontal acceptance radius [m] -float32 yaw_acceptance # Yaw acceptance error[rad] -float32 altitude_acceptance # Current vertical acceptance error [m] uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8baddd55e6..5a55be1cc8 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -487,8 +487,6 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - pos_ctrl_status.yaw_acceptance = NAN; - pos_ctrl_status.timestamp = hrt_absolute_time(); pos_ctrl_status.type = _position_sp_type; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 201eeb6123..34b4d2c00e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1142,17 +1142,11 @@ float Navigator::get_altitude_acceptance_radius() } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); - - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { alt_acceptance_radius = curr_sp.alt_acceptance_radius; - } else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { - alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } return alt_acceptance_radius; @@ -1214,14 +1208,6 @@ bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw) { float yaw = mission_item_yaw; - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that - // the waypoint can be reached from any direction - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { - yaw = pos_ctrl_status.yaw_acceptance; - } - return PX4_ISFINITE(yaw); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 7466077a93..119c8c18e8 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -441,7 +441,6 @@ RoverPositionControl::Run() _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); pos_ctrl_status.acceptance_radius = turn_distance; - pos_ctrl_status.yaw_acceptance = NAN; pos_ctrl_status.timestamp = hrt_absolute_time(); From b7b6d45e183175815b08d278aab6f3c0249d45dc Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 11:35:35 +0100 Subject: [PATCH 51/63] lib: remove bezier Signed-off-by: Silvan --- platforms/posix/cmake/sitl_tests.cmake | 1 - src/lib/CMakeLists.txt | 1 - src/lib/bezier/BezierN.cpp | 163 --------- src/lib/bezier/BezierN.hpp | 76 ---- src/lib/bezier/BezierNTest.cpp | 325 ------------------ src/lib/bezier/BezierQuad.cpp | 223 ------------ src/lib/bezier/BezierQuad.hpp | 210 ----------- src/lib/bezier/CMakeLists.txt | 39 --- .../tasks/Utility/CMakeLists.txt | 2 +- 9 files changed, 1 insertion(+), 1039 deletions(-) delete mode 100644 src/lib/bezier/BezierN.cpp delete mode 100644 src/lib/bezier/BezierN.hpp delete mode 100644 src/lib/bezier/BezierNTest.cpp delete mode 100644 src/lib/bezier/BezierQuad.cpp delete mode 100644 src/lib/bezier/BezierQuad.hpp delete mode 100644 src/lib/bezier/CMakeLists.txt diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index bf69b6a7ce..fdd8a5300e 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -5,7 +5,6 @@ # tests command arguments set(tests atomic_bitset - bezier bitset bson dataman diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 5fd36f546f..a9396caa1f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -35,7 +35,6 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) -add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) add_subdirectory(cdev EXCLUDE_FROM_ALL) add_subdirectory(cdrstream EXCLUDE_FROM_ALL) diff --git a/src/lib/bezier/BezierN.cpp b/src/lib/bezier/BezierN.cpp deleted file mode 100644 index 0d30aebd88..0000000000 --- a/src/lib/bezier/BezierN.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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 BezierN.cpp - * Bezier function - * - * @author Julian Kent - */ - -#include -#include - -namespace -{ - -/* - * Generic in-place bezier implementation. Leaves result in first element. - * - */ -template -void calculateBezier(matrix::Vector *positions, int N, Scalar t, Scalar one_minus_t) -{ - for (int bezier_order = 1; bezier_order < N; bezier_order++) { - for (int i = 0; i < N - bezier_order; i++) { - positions[i] = positions[i] * one_minus_t + positions[i + 1] * t; - } - } -} -} - -namespace bezier -{ - -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using Vector3Df = matrix::Vector3; - - Vector3Df intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = positions[i](j); - } - } - - Df dual_t(t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(intermediates[0]); - velocity = matrix::collectDerivatives(intermediates[0]); - - return true; -} - -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using DDf = matrix::Dual; - using Vector3DDf = matrix::Vector3; - - Vector3DDf intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = Df(positions[i](j)); - } - } - - DDf dual_t(Df(t, 0), 0); // 1st and 2nd derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(matrix::collectReals(intermediates[0])); - velocity = matrix::collectReals(matrix::collectDerivatives(intermediates[0])); - acceleration = matrix::collectDerivatives(matrix::collectDerivatives(intermediates[0])); - - return true; -} - -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint) -{ - if (setpoints == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - - using Df = matrix::Dual; - using Vector1Df = matrix::Vector; - - Vector1Df intermediates[N]; - - // all yaw setpoints are wrapped relative to the starting yaw - const float offset = setpoints[0]; - - for (int i = 0; i < N; i++) { - intermediates[i](0) = matrix::wrap_pi(setpoints[i] - offset); - } - - Df dual_t (t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - Df result = intermediates[0](0); - yaw_setpoint = matrix::wrap_pi(result.value + offset); - yaw_vel_setpoint = result.derivative(0); - - return true; -} - -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T) -{ - if (now < start_time || end_time < now) { - return false; - } - - int64_t total_duration = end_time - start_time; - int64_t elapsed_duration = now - start_time; - - T = (float) elapsed_duration / (float) total_duration; - - return true; -} - -} diff --git a/src/lib/bezier/BezierN.hpp b/src/lib/bezier/BezierN.hpp deleted file mode 100644 index 7f635e94b5..0000000000 --- a/src/lib/bezier/BezierN.hpp +++ /dev/null @@ -1,76 +0,0 @@ - - -/**************************************************************************** - * - * Copyright (C) 2020 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 BezierN.hpp - * - * @author Julian Kent - * - * N-order Bezier library designed for time-aware trajectory tracking - */ - -#pragma once -#include - -namespace bezier -{ - -/* - * Calculates the location and velocity with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity); - -/* - * Calculates the position, velocity and acceleration with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration); - -/* - * Calculates the position and velocity of yaw with respect to t on a bezier curve. - * All yaw setpoints are wrapped relative to the starting yaw. - * - */ -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint); - -/* - * Calculates the fraction between the begin and end time which can be used for fast bezier curve lookups - */ -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T); - -} diff --git a/src/lib/bezier/BezierNTest.cpp b/src/lib/bezier/BezierNTest.cpp deleted file mode 100644 index b08a40fd62..0000000000 --- a/src/lib/bezier/BezierNTest.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 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 code for the Velocity Smoothing library - * Run this test only using make tests TESTFILTER=BezierN - * - * @author Julian Kent - */ - -#include -#include - -#include "BezierN.hpp" - -TEST(BezierN_calculateBezier, checks_validity) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b; - EXPECT_FALSE(bezier::calculateBezierPosVel(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezier, checks_validity_accel) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b, c; - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(nullptr, 10, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 0, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, -0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, 1.5f, a, b, c)); -} - -TEST(BezierN_calculateBezier, work_1_point) -{ - // GIVEN: a single point bezier curve - matrix::Vector3f points[2] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 1, 0.5f, pos, vel)); - - // THEN: it should be the same as the point, and the velocity should be 0 - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_EQ(vel.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_2_points) -{ - // GIVEN: a 2-point bezier curve - matrix::Vector3f points[3] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.5f, pos, vel)); - - // THEN: the position should be the mid-point between the start and end, and velocity should be the length - EXPECT_EQ((pos - matrix::Vector3f(3, 1, 2)).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the beginning point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the end point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 1.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_zero_accel) -{ - // GIVEN: 3 points bezier, evenly spaced in a straight line - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(9, -2, -1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - - // THEN: it should be the middle point, with velocity of 1st to last - EXPECT_FLOAT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - - matrix::Vector3f pos2, vel2, accel2; - - // WHEN: we use the accel interface - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel2, accel2)); - - // THEN: it should give same position, velocity as the non-accel interface, and zero accel (since this curve is 0 accel) - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel2, accel2)); - - // THEN: it should be the starting point and same velocity - EXPECT_FLOAT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel2, accel2)); - - // THEN: it should be the ending point and same velocity - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_accel) -{ - // GIVEN: 3 points bezier, in a curve - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(19, -8, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - matrix::Vector3f pos2; - pos2 *= NAN; - - matrix::Vector3f accel_start, accel_mid, accel_end; - matrix::Vector3f vel_start, vel_mid, vel_end; - - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel_start, accel_start)); - - // THEN: it should give same position, velocity as the non-accel interface, and non-zero accel - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_start - vel).norm(), 0.f); - EXPECT_GT(accel_start.norm(), 0.f); - - // WHEN: we use the accel interface to get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel_mid, accel_mid)); - - // THEN: the values should matche between accel and non-accel version - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_mid - vel).norm(), 0.f); - - // AND: the accel should be the same as the start - EXPECT_FLOAT_EQ((accel_mid - accel_start).norm(), 0.f); - - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel_end, accel_end)); - - // THEN: it should be the ending point, and accel should match - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_end - vel).norm(), 0.f); - EXPECT_FLOAT_EQ((accel_end - accel_start).norm(), 0.f); - - // FINALLY: mid point velocity should be average of start and end velocity - EXPECT_FLOAT_EQ((vel_mid - 0.5f * (vel_start + vel_end)).norm(), 0.f); -} - -TEST(BezierN_calculateBezierYaw, checks_validity) -{ - float points[10]; - float a, b; - EXPECT_FALSE(bezier::calculateBezierYaw(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezierYaw, work_1_point) -{ - // GIVEN: a single yaw point - float points[2] = {M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we use it as a 1-point bezier curve - EXPECT_TRUE(bezier::calculateBezierYaw(points, 1, 0.5f, yaw, yaw_speed)); - - // THEN: it should have that same value, and the velocity should be 0 - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, 0); -} - -TEST(BezierN_calculateBezierYaw, work_2_points) -{ - // GIVEN: a single yaw point - float points[3] = {0, M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, 0); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 4); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); -} - -TEST(BezierN_calculateBezierYaw, work_2_points_wrap) -{ - // GIVEN: 2 yaw points on either side of the +- PI wrap line - float points[3] = {-M_PI + 0.1, M_PI - 0.1, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, -M_PI + 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the wrapped middle value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(matrix::wrap_pi(yaw - float(M_PI)), 0); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the end value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, M_PI - 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); -} - - -TEST(BezierN_calculateT, rejects_bad_timestamps) -{ - float f = NAN; - EXPECT_FALSE(bezier::calculateT(100, 1000, 99, f)); - EXPECT_FALSE(bezier::calculateT(100, 1000, 1001, f)); - EXPECT_FALSE(bezier::calculateT(1001, 1000, 1001, f)); -} - - -TEST(BezierN_calculateT, begin_middle_end) -{ - float f = NAN; - EXPECT_TRUE(bezier::calculateT(100, 1000, 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} - -TEST(BezierN_calculateT, giant_offset) -{ - int64_t offset = 0xFFFFFFFFFFFF; // 48 bit max - float f = NAN; - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp deleted file mode 100644 index 61309cc496..0000000000 --- a/src/lib/bezier/BezierQuad.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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 BezierQuad.cpp - * Bezier function - * - * @author Dennis Mannhart - */ - -// The header includes this implementation, so in order to have this library shared with other -// .cpp files and avoid duplication of constants, we need to prevent including it twice -#ifndef BEZIER_QUAD_CPP -#define BEZIER_QUAD_CPP - - -#include "BezierQuad.hpp" - -namespace bezier -{ -static constexpr double GOLDEN_RATIO = 1.6180339887; //(sqrt(5)+1)/2 -static constexpr double RESOLUTION = 0.0001; //End criterion for golden section search - -template -void BezierQuad::setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration) -{ - _pt0 = pt0; - _ctrl = ctrl; - _pt1 = pt1; - _duration = duration; - _cached_resolution = (Tp)(-1); -} - -template -void BezierQuad::getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1) -{ - pt0 = _pt0; - ctrl = _ctrl; - pt1 = _pt1; -} - -template -matrix::Vector BezierQuad::getPoint(const Tp t) -{ - return (_pt0 * ((Tp)1 - t / _duration) * ((Tp)1 - t / _duration) + _ctrl * (Tp)2 * (( - Tp)1 - t / _duration) * t / _duration + _pt1 * - t / _duration * t / _duration); -} - -template -matrix::Vector BezierQuad::getVelocity(const Tp t) -{ - return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration)); -} - -template -matrix::Vector BezierQuad::getAcceleration() -{ - return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration)); -} - -template -void BezierQuad::getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp time) -{ - point = getPoint(time); - vel = getVelocity(time); - acc = getAcceleration(); -} - -template -void BezierQuad::getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get states corresponding to t - getStates(point, vel, acc, t); - -} - -template -void BezierQuad::setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration) -{ - // update bezier points - _ctrl = ctrl; - _duration = duration; - _pt0 = _ctrl - vel0 * _duration / (Tp)2; - _pt1 = _ctrl + vel1 * _duration / (Tp)2; - _cached_resolution = (Tp)(-1); -} - -template -Tp BezierQuad::getArcLength(const Tp resolution) -{ - // we don't need to recompute arc length if: - // 1. _cached_resolution is up to date; 2. _cached_resolution is smaller than desired resolution (= more accurate) - if ((_cached_resolution > (Tp)0) && (_cached_resolution <= resolution)) { - return _cached_arc_length; - } - - // get number of elements - int n = (int)(roundf(_duration / resolution)); - - // check if n is even - if (n % 2 == 1) { - n += 1; - } - - // step size - Tp h = (_duration) / n; - - // get integration - _cached_arc_length = (Tp)0; - Vector3_t y; - - for (int i = 1; i < n; i++) { - - y = getVelocity(h * i); - - if (i % 2 == 1) { - _cached_arc_length += (Tp)4 * y.length(); - - } else { - _cached_arc_length += (Tp)2 * y.length(); - } - } - - // velocity length at start and end points - Tp y0 = getVelocity((Tp)0).length(); - Tp yn = getVelocity(_duration).length(); - - // 1/3 simpsons rule - _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length); - - // update cache - _cached_resolution = resolution; - - return _cached_arc_length; -} - -template -Tp BezierQuad::getDistToClosestPoint(const Vector3_t &pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get closest point - Vector3_t point = getPoint(t); - return (pose - point).length(); -} - -template -Tp BezierQuad::_goldenSectionSearch(const Vector3_t &pose) -{ - Tp a, b, c, d; - a = (Tp)0; // represents most left point - b = _duration * (Tp)1; // represents most right point - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - - while (fabsf(c - d) > RESOLUTION) { - if (_getDistanceSquared(c, pose) < _getDistanceSquared(d, pose)) { - b = d; - - } else { - a = c; - } - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - } - - return (b + a) / (Tp)2; -} - -template -Tp BezierQuad::_getDistanceSquared(const Tp t, const Vector3_t &pose) -{ - // get point on bezier - Vector3_t vec = getPoint(t); - - // get vector from point to pose - vec = vec - pose; - - // norm squared - return (vec * vec); -} -} - -#endif diff --git a/src/lib/bezier/BezierQuad.hpp b/src/lib/bezier/BezierQuad.hpp deleted file mode 100644 index 5aebe94b7b..0000000000 --- a/src/lib/bezier/BezierQuad.hpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018 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 BezierQuad.hpp - * - * Quadratic bezier lib - * - * A quadratic bezier function/spline is completely defined by three 3D points in space and a time scaling factor. - * pt0 and pt1 define the start and end points of the spline. ctrl point is a point in space that effects the curvature - * of the spline. The time scaling factor (= duration) defines the time it takes to travel along the spline from pt0 to - * pt1. - * A bezier spline is a continuous function from which position, velocity and acceleration can be extracted. For a given spline, - * acceleration stays constant. - */ - - -#pragma once - -#include - -namespace bezier -{ -template -class BezierQuad -{ -public: - - using Vector3_t = matrix::Vector; - - /** - * Empty constructor - */ - BezierQuad() : - _pt0(Vector3_t()), _ctrl(Vector3_t()), _pt1(Vector3_t()), _duration(1.0f) {} - - /** - * Constructor from array - */ - BezierQuad(const Tp pt0[3], const Tp ctrl[3], const Tp pt1[3], Tp duration = 1.0f) : - _pt0(Vector3_t(pt0)), _ctrl(Vector3_t(ctrl)), _pt1(Vector3_t(pt1)), _duration(duration) {} - - /** - * Constructor from vector - */ - BezierQuad(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = 1.0f): - _pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {} - - - /* - * Get bezier points - */ - void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1); - - /* - * Return pt0 - */ - Vector3_t getPt0() {return _pt0;} - - /* - * Return ctrl - */ - Vector3_t getCtrl() {return _ctrl;} - - /* - * Return pt1 - */ - Vector3_t getPt1() {return _pt1;} - - /** - * Set new bezier points and duration - */ - void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = (Tp)1); - - /* - * Set duration - * - * @param time is the total time it takes to travel along the bezier spline. - */ - void setDuration(const Tp time) {_duration = time;} - - /** - * Return point on bezier point corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return a point on bezier - */ - Vector3_t getPoint(const Tp t); - - /* - * Distance to closest point given a position - * - * @param pose is a position in 3D space from which distance to bezier is computed. - * @return distance to closest point on bezier - */ - Tp getDistToClosestPoint(const Vector3_t &pose); - - /* - * Return velocity on bezier corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return velocity vector at time t - */ - Vector3_t getVelocity(const Tp t); - - /* - * Return acceleration on bezier corresponding to time t - * - * @return constant acceleration of bezier - */ - Vector3_t getAcceleration(); - - /* - * Get all states on bezier corresponding to time t - */ - void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t); - - /* - * Get states on bezier which are closest to pose in space - * - * @param point is a posiiton on the spline that is closest to a given pose - * @param vel is the velocity at that given point - * @param acc is the acceleration for that spline - * @param pose represent a position in space from which closest point is computed - */ - void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose); - - /* - * Compute bezier from velocity at bezier end points and ctrl point - * - * The bezier end points are fully defined by a given control point ctrl, the duration and - * the desired velocity vectors at the end points. - */ - void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration = (Tp)1); - - /* - * Return the arc length of a bezier spline - * - * The arc length is computed with simpsons integration. - * @param resolution in meters. - */ - Tp getArcLength(const Tp resolution); - -private: - - Vector3_t _pt0; /**< Bezier starting point */ - Vector3_t _ctrl; /**< Bezier control point */ - Vector3_t _pt1; /**< bezier end point */ - Tp _duration = (Tp)1; /**< Total time to travle along spline */ - - Tp _cached_arc_length = (Tp)0; /**< The saved arc length of the spline */ - Tp _cached_resolution = (Tp)(-1); /**< The resolution used to compute the arc length. - Negative number means that cache is not up to date. */ - - /* - * Golden section search - */ - Tp _goldenSectionSearch(const Vector3_t &pose); - - /* - * Get squared distance from 3D pose in space and a point on bezier. - * - * @param t is the time in between [0, duration] that defines a point on the bezier. - * @param pose is a 3D pose in space. - */ - Tp _getDistanceSquared(const Tp t, const Vector3_t &pose); - - -}; - -using BezierQuad_f = BezierQuad; -using BezierQuad_d = BezierQuad; -} - -// include implementation -#include "BezierQuad.cpp" diff --git a/src/lib/bezier/CMakeLists.txt b/src/lib/bezier/CMakeLists.txt deleted file mode 100644 index f069fff591..0000000000 --- a/src/lib/bezier/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(bezier - BezierQuad.cpp - BezierN.cpp -) - -px4_add_unit_gtest(SRC BezierNTest.cpp LINKLIBS bezier) diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 911da11d26..6c32116237 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -38,7 +38,7 @@ px4_add_library(FlightTaskUtility StickYaw.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility) From b916a96e005227458c4f3ae1e02fcd62a873ecfe Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:11:23 +0100 Subject: [PATCH 52/63] Remove uorb topics exclusively used for avoidance - TrajectoryBezier.msg - TrajectoryWaypoint.msg - VehicleTrajectoryBezier.msg - VehicleTrajectoryWaypoint.msg Additionally remove TRAJECTORY_REPRESENTATION_WAYPOINTS mavlink stream. Signed-off-by: Silvan --- msg/CMakeLists.txt | 4 - msg/TrajectoryBezier.msg | 8 -- msg/TrajectoryWaypoint.msg | 13 -- msg/VehicleTrajectoryBezier.msg | 18 --- msg/VehicleTrajectoryWaypoint.msg | 21 --- src/modules/logger/logged_topics.cpp | 2 - src/modules/mavlink/mavlink_main.cpp | 3 - src/modules/mavlink/mavlink_messages.cpp | 4 - src/modules/mavlink/mavlink_receiver.cpp | 66 ---------- src/modules/mavlink/mavlink_receiver.h | 6 - .../TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp | 124 ------------------ src/modules/uxrce_dds_client/dds_topics.yaml | 9 -- src/modules/zenoh/Kconfig | 3 - src/modules/zenoh/Kconfig.topics | 15 --- 14 files changed, 296 deletions(-) delete mode 100644 msg/TrajectoryBezier.msg delete mode 100644 msg/TrajectoryWaypoint.msg delete mode 100644 msg/VehicleTrajectoryBezier.msg delete mode 100644 msg/VehicleTrajectoryWaypoint.msg delete mode 100644 src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 51c9325413..af7a98cf54 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -214,8 +214,6 @@ set(msg_files TelemetryStatus.msg TiltrotorExtraControls.msg TimesyncStatus.msg - TrajectoryBezier.msg - TrajectoryWaypoint.msg TransponderReport.msg TuneControl.msg UavcanParameterRequest.msg @@ -235,8 +233,6 @@ set(msg_files VehicleRoi.msg VehicleThrustSetpoint.msg VehicleTorqueSetpoint.msg - VehicleTrajectoryBezier.msg - VehicleTrajectoryWaypoint.msg VelocityLimits.msg WheelEncoders.msg Wind.msg diff --git a/msg/TrajectoryBezier.msg b/msg/TrajectoryBezier.msg deleted file mode 100644 index e3d9d4e0ff..0000000000 --- a/msg/TrajectoryBezier.msg +++ /dev/null @@ -1,8 +0,0 @@ -# Bezier Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier - -uint64 timestamp # time since system start (microseconds) - -float32[3] position # local position x,y,z (metres) -float32 yaw # yaw angle (rad) -float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds) diff --git a/msg/TrajectoryWaypoint.msg b/msg/TrajectoryWaypoint.msg deleted file mode 100644 index 6ea9bae4d3..0000000000 --- a/msg/TrajectoryWaypoint.msg +++ /dev/null @@ -1,13 +0,0 @@ -# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint - -uint64 timestamp # time since system start (microseconds) - -float32[3] position -float32[3] velocity -float32[3] acceleration -float32 yaw -float32 yaw_speed - -bool point_valid -uint8 type diff --git a/msg/VehicleTrajectoryBezier.msg b/msg/VehicleTrajectoryBezier.msg deleted file mode 100644 index d4bf99b469..0000000000 --- a/msg/VehicleTrajectoryBezier.msg +++ /dev/null @@ -1,18 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the -# companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryBezier[5] control_points -uint8 bezier_order - -# TOPICS vehicle_trajectory_bezier diff --git a/msg/VehicleTrajectoryWaypoint.msg b/msg/VehicleTrajectoryWaypoint.msg deleted file mode 100644 index 6bff1cec84..0000000000 --- a/msg/VehicleTrajectoryWaypoint.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. -# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 - -uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum. - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryWaypoint[5] waypoints - -# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 57b2a429de..3114d83bb1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -335,8 +335,6 @@ void LoggedTopics::add_vision_and_avoidance_topics() add_topic("obstacle_distance_fused"); add_topic("obstacle_distance"); add_topic("vehicle_mocap_odometry", 30); - add_topic("vehicle_trajectory_waypoint", 200); - add_topic("vehicle_trajectory_waypoint_desired", 200); add_topic("vehicle_visual_odometry", 30); } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6e81849600..724391cf86 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1525,7 +1525,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1592,7 +1591,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RC_CHANNELS", 5.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1784,7 +1782,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 72b5b49123..7d5e23bb6e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -116,7 +116,6 @@ #include "streams/SYSTEM_TIME.hpp" #include "streams/TIME_ESTIMATE_TO_TARGET.hpp" #include "streams/TIMESYNC.hpp" -#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" #include "streams/VFR_HUD.hpp" #include "streams/VIBRATION.hpp" #include "streams/WIND_COV.hpp" @@ -382,9 +381,6 @@ static const StreamListItem streams_list[] = { #if defined(MANUAL_CONTROL_HPP) create_stream_list_item(), #endif // MANUAL_CONTROL_HPP -#if defined(TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP) - create_stream_list_item(), -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP #if defined(OPTICAL_FLOW_RAD_HPP) create_stream_list_item(), #endif // OPTICAL_FLOW_RAD_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 00c0b9ec93..6afab95313 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -256,14 +256,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_tunnel(msg); break; - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER: - handle_message_trajectory_representation_bezier(msg); - break; - - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS: - handle_message_trajectory_representation_waypoints(msg); - break; - case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS: handle_message_onboard_computer_status(msg); break; @@ -1988,64 +1980,6 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg) } -void -MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_bezier_t trajectory; - mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory); - - vehicle_trajectory_bezier_s trajectory_bezier{}; - - trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); - - for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) { - trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i]; - trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i]; - trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i]; - - trajectory_bezier.control_points[i].delta = trajectory.delta[i]; - trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i]; - } - - trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS); - _trajectory_bezier_pub.publish(trajectory_bezier); -} - -void -MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_waypoints_t trajectory; - mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); - - vehicle_trajectory_waypoint_s trajectory_waypoint{}; - - const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS); - - for (int i = 0; i < number_valid_points; ++i) { - trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i]; - trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i]; - trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i]; - - trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i]; - trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i]; - trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i]; - - trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i]; - trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i]; - trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i]; - - trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; - trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; - - trajectory_waypoint.waypoints[i].point_valid = true; - - trajectory_waypoint.waypoints[i].type = UINT8_MAX; - } - - trajectory_waypoint.timestamp = hrt_absolute_time(); - _trajectory_waypoint_pub.publish(trajectory_waypoint); -} - void MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 326d0ee19e..84ad2e7c62 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,8 +110,6 @@ #include #include #include -#include -#include #include #if !defined(CONSTRAINED_FLASH) @@ -201,8 +199,6 @@ private: void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); void handle_message_tunnel(mavlink_message_t *msg); - void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); - void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); #if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used void handle_message_set_velocity_limits(mavlink_message_t *msg); @@ -329,8 +325,6 @@ private: uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; diff --git a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp deleted file mode 100644 index 174c633fd2..0000000000 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ - -#ifndef TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP -#define TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP - -#include - -class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream -{ -public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTrajectoryRepresentationWaypoints(mavlink); } - - const char *get_name() const override { return get_name_static(); } - uint16_t get_id() override { return get_id_static(); } - - static constexpr const char *get_name_static() { return "TRAJECTORY_REPRESENTATION_WAYPOINTS"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; } - - unsigned get_size() override - { - if (_traj_wp_avoidance_sub.advertised()) { - return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - - return 0; - } - -private: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} - - uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - bool send() override - { - vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; - - if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) { - mavlink_trajectory_representation_waypoints_t msg{}; - - msg.time_usec = traj_wp_avoidance_desired.timestamp; - int number_valid_points = 0; - - for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { - msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0]; - msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1]; - msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2]; - - msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0]; - msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1]; - msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2]; - - msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0]; - msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1]; - msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2]; - - msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw; - msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed; - - switch (traj_wp_avoidance_desired.waypoints[i].type) { - case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; - break; - - case position_setpoint_s::SETPOINT_TYPE_LOITER: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; - break; - - case position_setpoint_s::SETPOINT_TYPE_LAND: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - break; - - default: - msg.command[i] = UINT16_MAX; - } - - if (traj_wp_avoidance_desired.waypoints[i].point_valid) { - number_valid_points++; - } - - } - - msg.valid_points = number_valid_points; - - mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg); - - return true; - } - - return false; - } -}; - -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 211e75fd05..bbd0d8c631 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -71,9 +71,6 @@ publications: - topic: /fmu/out/vehicle_status type: px4_msgs::msg::VehicleStatus - - topic: /fmu/out/vehicle_trajectory_waypoint_desired - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/out/airspeed_validated type: px4_msgs::msg::AirspeedValidated @@ -145,12 +142,6 @@ subscriptions: - topic: /fmu/in/vehicle_command_mode_executor type: px4_msgs::msg::VehicleCommand - - topic: /fmu/in/vehicle_trajectory_bezier - type: px4_msgs::msg::VehicleTrajectoryBezier - - - topic: /fmu/in/vehicle_trajectory_waypoint - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/in/vehicle_thrust_setpoint type: px4_msgs::msg::VehicleThrustSetpoint diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig index c3c042df13..3cce22f026 100644 --- a/src/modules/zenoh/Kconfig +++ b/src/modules/zenoh/Kconfig @@ -46,9 +46,6 @@ if MODULES_ZENOH select ZENOH_PUBSUB_VEHICLE_ODOMETRY select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT select ZENOH_PUBSUB_VEHICLE_COMMAND - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - config ZENOH_PUBSUB_ALL bool "All" diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 585831cd2f..ac96b8586f 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -741,10 +741,6 @@ menu "Zenoh publishers/subscribers" bool "timesync_status" default n - config ZENOH_PUBSUB_TRAJECTORY_BEZIER - bool "trajectory_bezier" - default n - config ZENOH_PUBSUB_TRAJECTORY_SETPOINT bool "trajectory_setpoint" default n @@ -881,14 +877,6 @@ menu "Zenoh publishers/subscribers" bool "vehicle_torque_setpoint" default n - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - bool "vehicle_trajectory_bezier" - default n - - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - bool "vehicle_trajectory_waypoint" - default n - config ZENOH_PUBSUB_VELOCITY_LIMITS bool "velocity_limits" default n @@ -1099,7 +1087,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_TELEMETRY_STATUS select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS select ZENOH_PUBSUB_TIMESYNC_STATUS - select ZENOH_PUBSUB_TRAJECTORY_BEZIER select ZENOH_PUBSUB_TRAJECTORY_SETPOINT select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_TRANSPONDER_REPORT @@ -1134,8 +1121,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_VEHICLE_STATUS select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_VELOCITY_LIMITS select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS select ZENOH_PUBSUB_WHEEL_ENCODERS From a1ff1d83725e483e0cb98dc0765e15bcef2b34e7 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:21:26 +0100 Subject: [PATCH 53/63] tests: remove test_bezier Signed-off-by: Silvan --- Tools/HIL/run_tests.py | 3 - src/systemcmds/tests/CMakeLists.txt | 1 - src/systemcmds/tests/test_bezierQuad.cpp | 278 ----------------------- src/systemcmds/tests/tests_main.c | 1 - src/systemcmds/tests/tests_main.h | 1 - 5 files changed, 284 deletions(-) delete mode 100644 src/systemcmds/tests/test_bezierQuad.cpp diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py index 3e79c70227..0d9c5f5d1b 100755 --- a/Tools/HIL/run_tests.py +++ b/Tools/HIL/run_tests.py @@ -136,9 +136,6 @@ class TestHardwareMethods(unittest.TestCase): def test_atomic_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "atomic_bitset")) - def test_bezier(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bezier")) - def test_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bitset")) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 72d99c6708..2beab87943 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -33,7 +33,6 @@ set(srcs test_atomic_bitset.cpp - test_bezierQuad.cpp test_bitset.cpp test_bson.cpp test_dataman.cpp diff --git a/src/systemcmds/tests/test_bezierQuad.cpp b/src/systemcmds/tests/test_bezierQuad.cpp deleted file mode 100644 index 9fc5f7f987..0000000000 --- a/src/systemcmds/tests/test_bezierQuad.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-2019 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 test_bezierQuad.cpp - * Test for Bezier curve computation. - */ - -#include -#include -#include -#include - -#include "../../lib/bezier/BezierQuad.hpp" - -class BezierQuadTest : public UnitTest -{ -public: - virtual bool run_tests(); - -private: - - bool _get_states_from_time(); - bool _get_arc_length(); - bool _set_bez_from_vel(); - - float random(float min, float max); - -}; - - -bool BezierQuadTest::run_tests() -{ - ut_run_test(_get_states_from_time); - ut_run_test(_get_arc_length); - ut_run_test(_set_bez_from_vel); - - return (_tests_failed == 0); -} - -bool BezierQuadTest::_get_states_from_time() -{ - // symmetric around 0 - matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f); - matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f); - matrix::Vector3f pt1(0.5f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bezier::BezierQuad_f bz(pt0, ctrl, pt1); - - matrix::Vector3f pos, vel, acc; - float precision = 0.00001; - - // states at time = 0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 1", vel(1), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), -1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.50f); - - // pos must be equal to ctrl(0) and lower than ctrl(1) - ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision); - ut_assert_true(pos(1) < ctrl(1)); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal -2", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // acceleration - pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f); - ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f); - pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bz.setBezier(pt0, ctrl, pt1, 1.0f); - - // states at time = 0.0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 0", vel(0), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1.0 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 2", vel(0), 2.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.5f); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - return true; - -} - -bool BezierQuadTest::_get_arc_length() -{ - // create random numbers - srand(0); // choose a constant to make it deterministic - - float min = -50.f; - float max = 50.f; - float resolution = 0.1f; - - matrix::Vector3f pt0, pt1, ctrl; - float duration, arc_length, triangle_length, straigth_length; - float T = 100.0f; - - // loop trough different control points 100x and check if arc_length is in the expected range - for (int i = 0; i < 100 ; i++) { - // random bezier point - pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - - // use for each test a new duration - duration = random(0.0f, T); - - // create bezier - bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration); - - // compute arc length, triangle length and straigh length - arc_length = bz.getArcLength(resolution); - triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length(); - straigth_length = (pt1 - pt0).length(); - - // we also compute length from going point to point and add segment - float time_increment = duration / T; - float t = 0.0f + time_increment; - matrix::Vector3f p0 = pt0; - float sum_segments = 0.0f; - - for (int s = 0; s < (int)T; s++) { - matrix::Vector3f nextpt = bz.getPoint(t); - sum_segments = (nextpt - p0).length() + sum_segments; - p0 = bz.getPoint(t); - t = t + time_increment; - } - - // test comparisons - ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length) - && (fabsf(arc_length - sum_segments) < 1.f)); - } - - - return true; -} - -bool BezierQuadTest::_set_bez_from_vel() -{ - // create random numbers - srand(100); // choose a constant to make it deterministic - - float low = -50.0f; - float max = 50.0f; - float precision = 0.001f; - - for (int i = 0; i < 20; i++) { - - // set velocity - matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max)); - float duration = random(0.0f, 100.0f); - - bezier::BezierQuad_f bz;; - bz.setBezFromVel(ctrl, vel0, vel1, duration); - - // get velocity back - matrix::Vector3f v0 = bz.getVelocity(0.0f); - matrix::Vector3f v1 = bz.getVelocity(duration); - ut_compare_float("", vel0(0), v0(0), precision); - ut_compare_float("", vel1(0), v1(0), precision); - - ut_compare_float("", vel0(1), v0(1), precision); - ut_compare_float("", vel1(1), v1(1), precision); - - ut_compare_float("", vel0(2), v0(2), precision); - ut_compare_float("", vel1(2), v1(2), precision); - } - - return true; -} - -float BezierQuadTest::random(float min, float max) -{ - float s = rand() / (float)RAND_MAX; - return (min + s * (max - min)); - -} - -ut_declare_test_c(test_bezierQuad, BezierQuadTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index af25fd074e..3d04ad31ef 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -78,7 +78,6 @@ const struct { #endif /* __PX4_NUTTX */ {"atomic_bitset", test_atomic_bitset, 0}, - {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 0e5a14743c..c42bce07fc 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -44,7 +44,6 @@ __BEGIN_DECLS extern int test_atomic_bitset(int argc, char *argv[]); -extern int test_bezierQuad(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]); From 04cd247c906bc34a4869a7cc2b1be45a85018f47 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:34:48 +0100 Subject: [PATCH 54/63] FlightTaskAuto: remove isTargetModified() As it is no longer needed w/o avoidance. Signed-off-by: Silvan --- .../tasks/Auto/FlightTaskAuto.cpp | 14 -------------- .../tasks/Auto/FlightTaskAuto.hpp | 1 - 2 files changed, 15 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 614bcc6118..419f25827a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -159,11 +159,6 @@ bool FlightTaskAuto::update() _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; - if (isTargetModified()) { - // In case object avoidance has injected a new setpoint, we take this as the next waypoints - waypoints[2] = _position_setpoint; - } - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; @@ -764,15 +759,6 @@ bool FlightTaskAuto::_generateHeadingAlongTraj() return res; } -bool FlightTaskAuto::isTargetModified() const -{ - const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); - const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); - const bool z_modified = z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; - - return xy_modified || z_modified; -} - void FlightTaskAuto::_updateTrajConstraints() { // update params of the position smoothing diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 6c49dd8726..5b8d3e9b5b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -108,7 +108,6 @@ protected: void _checkEmergencyBraking(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ - bool isTargetModified() const; void _updateTrajConstraints(); void rcHelpModifyYaw(float &yaw_sp); From 0b370ab5d3ec1bd5b81ec9d54af4cd5593a3ff58 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jan 2025 19:26:16 +0100 Subject: [PATCH 55/63] Remove obstacle avoidance MAVLink Heartbeat check --- msg/TelemetryStatus.msg | 2 -- src/modules/mavlink/mavlink_receiver.cpp | 6 ------ src/modules/mavlink/mavlink_receiver.h | 1 - 3 files changed, 9 deletions(-) diff --git a/msg/TelemetryStatus.msg b/msg/TelemetryStatus.msg index 48d85f934d..b2b3920a36 100644 --- a/msg/TelemetryStatus.msg +++ b/msg/TelemetryStatus.msg @@ -51,13 +51,11 @@ bool heartbeat_type_open_drone_id # MAV_TYPE_ODID bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO bool heartbeat_component_log # MAV_COMP_ID_LOG bool heartbeat_component_osd # MAV_COMP_ID_OSD -bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE # Misc component health -bool avoidance_system_healthy bool open_drone_id_system_healthy bool parachute_system_healthy diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6afab95313..0b06a24b76 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2193,11 +2193,6 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) _heartbeat_component_osd = now; break; - case MAV_COMP_ID_OBSTACLE_AVOIDANCE: - _heartbeat_component_obstacle_avoidance = now; - _mavlink.telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); - break; - case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: _heartbeat_component_visual_inertial_odometry = now; break; @@ -2909,7 +2904,6 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd); - tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance); tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry); tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager); tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 84ad2e7c62..3d617a7731 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -394,7 +394,6 @@ private: hrt_abstime _heartbeat_component_telemetry_radio{0}; hrt_abstime _heartbeat_component_log{0}; hrt_abstime _heartbeat_component_osd{0}; - hrt_abstime _heartbeat_component_obstacle_avoidance{0}; hrt_abstime _heartbeat_component_visual_inertial_odometry{0}; hrt_abstime _heartbeat_component_pairing_manager{0}; hrt_abstime _heartbeat_component_udp_bridge{0}; From ed9111ec49d1ab4ae424ec2ba13004863d27d907 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jan 2025 19:30:42 +0100 Subject: [PATCH 56/63] Remove obstacle avoidance test with Gazebo classic and ROS 1 --- Makefile | 4 - .../1014_gazebo-classic_iris_obs_avoid | 30 ------- .../1014_gazebo-classic_iris_obs_avoid.post | 2 - .../init.d-posix/airframes/CMakeLists.txt | 2 - .../px4_it/mavros/missions/avoidance.plan | 88 ------------------- .../sitl_targets_gazebo-classic.cmake | 1 - test/mavros_posix_test_avoidance.test | 52 ----------- test/rostest_avoidance_run.sh | 20 ----- 8 files changed, 199 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid delete mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post delete mode 100644 integrationtests/python_src/px4_it/mavros/missions/avoidance.plan delete mode 100644 test/mavros_posix_test_avoidance.test delete mode 100755 test/rostest_avoidance_run.sh diff --git a/Makefile b/Makefile index c65989ffe7..cfd87b6e23 100644 --- a/Makefile +++ b/Makefile @@ -457,10 +457,6 @@ tests_offboard: rostest @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_rpyrt_ctl.test -tests_avoidance: rostest - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_safe_landing.test - python_coverage: @mkdir -p "$(SRC_DIR)"/build/python_coverage @cd "$(SRC_DIR)"/build/python_coverage && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=px4_sitl_default -DPYTHON_COVERAGE=ON diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid deleted file mode 100644 index e3c25ec777..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/sh -# -# @name 3DR Iris Quadrotor SITL (Obstacle Avoidance) -# -# @type Quadrotor Wide -# - -. ${R}etc/init.d/rc.mc_defaults - - -param set-default CA_AIRFRAME 0 - -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 -param set-default CA_ROTOR3_KM -0.05 - -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post deleted file mode 100644 index 64b8ce8b19..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post +++ /dev/null @@ -1,2 +0,0 @@ -# shellcheck disable=SC2154 -mavlink start -x -u 14558 -r 4000000 -m onboard -o 14541 -p # add mavlink stream for SDK diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 94f2f4d39a..4ae56f51e9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -39,8 +39,6 @@ px4_add_romfs_files( 1012_gazebo-classic_iris_rplidar 1013_gazebo-classic_iris_vision 1013_gazebo-classic_iris_vision.post - 1014_gazebo-classic_iris_obs_avoid - 1014_gazebo-classic_iris_obs_avoid.post 1015_gazebo-classic_iris_depth_camera 1016_gazebo-classic_iris_downward_depth_camera 1017_gazebo-classic_iris_opt_flow_mockup diff --git a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan deleted file mode 100644 index 1698c6b2bc..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan +++ /dev/null @@ -1,88 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5456085, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458765, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": -1, - "AltitudeMode": 0, - "autoContinue": true, - "command": 21, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458812, - -1 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.3977419, - 8.5458854, - 487.923 - ], - "vehicleType": 2, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index b67fe546cc..258e18d8b2 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -80,7 +80,6 @@ if(gazebo_FOUND) iris_dual_gps iris_foggy_lidar iris_irlock - iris_obs_avoid iris_depth_camera iris_downward_depth_camera iris_opt_flow diff --git a/test/mavros_posix_test_avoidance.test b/test/mavros_posix_test_avoidance.test deleted file mode 100644 index 5c5adeaa4d..0000000000 --- a/test/mavros_posix_test_avoidance.test +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - $(arg pointcloud_topics) - - - - diff --git a/test/rostest_avoidance_run.sh b/test/rostest_avoidance_run.sh deleted file mode 100755 index 2fc2be7eb6..0000000000 --- a/test/rostest_avoidance_run.sh +++ /dev/null @@ -1,20 +0,0 @@ -#! /bin/bash - -DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -PX4_SRC_DIR=${DIR}/.. - -source /opt/ros/${ROS_DISTRO:-kinetic}/setup.bash -mkdir -p ${PX4_SRC_DIR}/catkin_ws/src -cd ${PX4_SRC_DIR}/catkin_ws/ -git clone -b 0.3.1 --single-branch --depth 1 https://github.com/PX4/avoidance.git src/avoidance - -catkin init -catkin build local_planner safe_landing_planner --cmake-args -DCMAKE_BUILD_TYPE=Release - -source ${PX4_SRC_DIR}/catkin_ws/devel/setup.bash -source /usr/share/gazebo/setup.sh - -export CATKIN_SETUP_UTIL_ARGS=--extend -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${PX4_SRC_DIR}/catkin_ws/src/avoidance/avoidance/sim/models - -source $DIR/rostest_px4_run.sh "$@" From 3119510f2598b774c75594c762134ba802ad103e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 14 Jan 2025 11:13:07 +0100 Subject: [PATCH 57/63] Remove health_component::avoidance Signed-off-by: Silvan Fuhrer --- msg/versioned/ArmingCheckReply.msg | 1 - .../commander/HealthAndArmingChecks/checks/externalChecks.hpp | 3 --- src/modules/mavlink/streams/SYS_STATUS.hpp | 1 - 3 files changed, 5 deletions(-) diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index 1a1aaa5183..4c1616d849 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -6,7 +6,6 @@ uint8 request_id uint8 registration_id uint8 HEALTH_COMPONENT_INDEX_NONE = 0 -uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 uint8 health_component_index # HEALTH_COMPONENT_INDEX_* bool health_component_is_present diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index bbba418000..2efa6896c3 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -40,9 +40,6 @@ #include #include -static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) - health_component_t::avoidance, "enum definition missmatch"); - class ExternalChecks : public HealthAndArmingCheckBase { public: diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index c875eaceaa..7a89501793 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -144,7 +144,6 @@ private: fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_BATTERY, health_component_t::battery, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, health_component_t::motors_escs, msg); fillOutComponent(health_report, MAV_SYS_STATUS_RECOVERY_SYSTEM, health_component_t::parachute, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, health_component_t::avoidance, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_ACCEL, health_component_t::accel, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_GYRO, health_component_t::gyro, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); From 662c66e5463efc6131bb499d4bcd8a730198f1ae Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 15 Jan 2025 10:19:53 +0100 Subject: [PATCH 58/63] VehicleStatus: bump VERSION to 1 after removal of avoidance_system_required Signed-off-by: Silvan Fuhrer --- msg/versioned/VehicleStatus.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 062c6f0765..b9a3edd91f 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,6 +1,6 @@ # Encodes the system state of the vehicle published by commander -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) From 3be8b680f66be0ba286fe1e2986e46f0ca744c74 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 14 Feb 2025 10:35:27 +0100 Subject: [PATCH 59/63] msg_translation: Add vehicle_status_v1 translation Signed-off-by: Silvan Fuhrer --- msg/px4_msgs_old/msg/VehicleStatusV0.msg | 140 ++++++++++++++++++ .../translations/all_translations.h | 2 + .../translation_vehicle_status_v1.h | 110 ++++++++++++++ 3 files changed, 252 insertions(+) create mode 100644 msg/px4_msgs_old/msg/VehicleStatusV0.msg create mode 100644 msg/translation_node/translations/translation_vehicle_status_v1.h diff --git a/msg/px4_msgs_old/msg/VehicleStatusV0.msg b/msg/px4_msgs_old/msg/VehicleStatusV0.msg new file mode 100644 index 0000000000..a347dfce3d --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV0.msg @@ -0,0 +1,140 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_FREE4 = 8 +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_AIRSHIP = 4 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 2b3c2030b4..4369a3c58a 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -9,3 +9,5 @@ //#include "example_translation_direct_v1.h" //#include "example_translation_multi_v2.h" //#include "example_translation_service_v1.h" + +#include "translation_vehicle_status_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_status_v1.h b/msg/translation_node/translations/translation_vehicle_status_v1.h new file mode 100644 index 0000000000..2041b013d8 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v1.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v0 <--> v1 +#include +#include + +class VehicleStatusV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.armed_time = msg_older.armed_time; + msg_newer.takeoff_time = msg_older.takeoff_time; + msg_newer.arming_state = msg_older.arming_state; + msg_newer.latest_arming_reason = msg_older.latest_arming_reason; + msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason; + msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp; + msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention; + msg_newer.nav_state = msg_older.nav_state; + msg_newer.executor_in_charge = msg_older.executor_in_charge; + msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask; + msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask; + msg_newer.failure_detector_status = msg_older.failure_detector_status; + msg_newer.hil_state = msg_older.hil_state; + msg_newer.vehicle_type = msg_older.vehicle_type; + msg_newer.failsafe = msg_older.failsafe; + msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over; + msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state; + msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost; + msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter; + msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost; + msg_newer.is_vtol = msg_older.is_vtol; + msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter; + msg_newer.in_transition_mode = msg_older.in_transition_mode; + msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw; + msg_newer.system_type = msg_older.system_type; + msg_newer.system_id = msg_older.system_id; + msg_newer.component_id = msg_older.component_id; + msg_newer.safety_button_available = msg_older.safety_button_available; + msg_newer.safety_off = msg_older.safety_off; + msg_newer.power_input_valid = msg_older.power_input_valid; + msg_newer.usb_connected = msg_older.usb_connected; + msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present; + msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; + msg_newer.parachute_system_present = msg_older.parachute_system_present; + msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; + msg_newer.calibration_enabled = msg_older.calibration_enabled; + msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.armed_time = msg_newer.armed_time; + msg_older.takeoff_time = msg_newer.takeoff_time; + msg_older.arming_state = msg_newer.arming_state; + msg_older.latest_arming_reason = msg_newer.latest_arming_reason; + msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason; + msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp; + msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; + msg_older.nav_state = msg_newer.nav_state; + msg_older.executor_in_charge = msg_newer.executor_in_charge; + msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; + msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; + msg_older.failure_detector_status = msg_newer.failure_detector_status; + msg_older.hil_state = msg_newer.hil_state; + msg_older.vehicle_type = msg_newer.vehicle_type; + msg_older.failsafe = msg_newer.failsafe; + msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over; + msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state; + msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost; + msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter; + msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost; + msg_older.is_vtol = msg_newer.is_vtol; + msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter; + msg_older.in_transition_mode = msg_newer.in_transition_mode; + msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw; + msg_older.system_type = msg_newer.system_type; + msg_older.system_id = msg_newer.system_id; + msg_older.component_id = msg_newer.component_id; + msg_older.safety_button_available = msg_newer.safety_button_available; + msg_older.safety_off = msg_newer.safety_off; + msg_older.power_input_valid = msg_newer.power_input_valid; + msg_older.usb_connected = msg_newer.usb_connected; + msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present; + msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; + msg_older.parachute_system_present = msg_newer.parachute_system_present; + msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + msg_older.avoidance_system_required = false; + msg_older.avoidance_system_valid = false; + msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; + msg_older.calibration_enabled = msg_newer.calibration_enabled; + msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV1Translation); From 5e06ab1430eca9e55d63fb00e83aaf14f42d965f Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 12 Feb 2025 10:53:39 +0100 Subject: [PATCH 60/63] ekf-agp: do not reset to AGP if GNSS fusion is active --- .../aux_global_position.cpp | 29 +++++++++++++++---- src/modules/ekf2/EKF/estimator_interface.cpp | 24 +++++++++++++-- src/modules/ekf2/EKF/estimator_interface.h | 5 +++- 3 files changed, 49 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 3307dd56b9..675bffdd3c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -104,9 +104,20 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed _state = State::starting; if (ekf.global_origin_valid()) { - ekf.enableControlStatusAuxGpos(); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; - _state = State::active; + const bool fused = ekf.fuseHorizontalPosition(aid_src); + bool reset = false; + + if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) { + ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv)); + ekf.resetAidSourceStatusZeroInnovation(aid_src); + reset = true; + } + + if (fused || reset) { + ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _state = State::active; + } } else { // Try to initialize using measurement @@ -128,12 +139,18 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) { - ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); - ekf.resetAidSourceStatusZeroInnovation(aid_src); + ekf.resetAidSourceStatusZeroInnovation(aid_src); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + } } } else { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 84ced94c80..22ccf958f0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -600,12 +600,32 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f } int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +{ + return getNumberOfActiveHorizontalPositionAidingSources() + getNumberOfActiveHorizontalVelocityAidingSources(); +} + +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalPositionAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalPositionAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalPositionAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const { return int(_control_status.flags.gps) - + int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_pos) + + int(_control_status.flags.aux_gpos); +} + +int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const +{ + return int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_vel) - + int(_control_status.flags.aux_gpos) // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning // and so is treated as a single aiding source. + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1212300278..93632bc109 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -207,8 +207,8 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) { _air_density = air_density; } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; + bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const; /* * Check if there are any other active source of horizontal aiding @@ -221,6 +221,7 @@ public: * @return true if an other source than aiding_flag is active */ bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const; // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos @@ -228,6 +229,8 @@ public: bool isVerticalAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; + int getNumberOfActiveHorizontalPositionAidingSources() const; + int getNumberOfActiveHorizontalVelocityAidingSources() const; bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; bool isVerticalPositionAidingActive() const; From 412d4390a6d98f9be670aab905d66b54a0d1dada Mon Sep 17 00:00:00 2001 From: Perre <60040461+Perrrewi@users.noreply.github.com> Date: Tue, 18 Feb 2025 14:38:11 +0100 Subject: [PATCH 61/63] MC Slowmode: Yawstick for gimbal control, vehicle yaw follows gimbal (#24242) * Yawsticks on gimbal, vehicle follows gimbal in slowmode, once vehicle has taken off * Increase queue length to avoid automatically unadvertise queued publications with queue length 1 * Improve readability --------- Co-authored-by: Pernilla --- msg/GimbalManagerSetAttitude.msg | 2 + .../FlightTaskManualAccelerationSlow.cpp | 33 ++++- .../FlightTaskManualAccelerationSlow.hpp | 10 +- .../flight_task_acceleration_slow_params.c | 14 ++ .../tasks/Utility/CMakeLists.txt | 1 + .../tasks/Utility/Gimbal.cpp | 126 ++++++++++++++++++ .../tasks/Utility/Gimbal.hpp | 90 +++++++++++++ src/modules/gimbal/input_mavlink.cpp | 28 ++-- 8 files changed, 288 insertions(+), 16 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp create mode 100644 src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp diff --git a/msg/GimbalManagerSetAttitude.msg b/msg/GimbalManagerSetAttitude.msg index d88acca8b6..e1a174fd0b 100644 --- a/msg/GimbalManagerSetAttitude.msg +++ b/msg/GimbalManagerSetAttitude.msg @@ -20,3 +20,5 @@ float32[4] q float32 angular_velocity_x float32 angular_velocity_y float32 angular_velocity_z + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 7b1c24f1dd..dd2ca1da59 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -39,6 +39,7 @@ #include using namespace time_literals; +using namespace matrix; bool FlightTaskManualAccelerationSlow::update() { @@ -128,7 +129,29 @@ bool FlightTaskManualAccelerationSlow::update() FlightTaskManualAltitude::_velocity_constraint_down = velocity_down; FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate); - return FlightTaskManualAcceleration::update(); + bool ret = FlightTaskManualAcceleration::update(); + + // Optimize input-to-video latency gimbal control + if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) { + _gimbal.acquireGimbalControlIfNeeded(); + + // the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward + const float pitchrate_setpoint = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate; + _yawspeed_setpoint = _sticks.getYaw() * yaw_rate; + + _gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, pitchrate_setpoint, _yawspeed_setpoint)); + + if (_gimbal.allAxesLockedConfirmed()) { + // but the vehicle makes sure it stays alligned with the gimbal absolute yaw + _yaw_setpoint = _gimbal.getTelemetryYaw(); + } + + } else { + _gimbal.releaseGimbalControlIfNeeded(); + } + + return ret; } float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value) @@ -136,3 +159,11 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i const int sanitized_index = math::constrain(parameter_value - 1, 0, 5); return _sticks.getAux()(sanitized_index); } + +bool FlightTaskManualAccelerationSlow::haveTakenOff() +{ + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + + return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT; +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index b54d9b765d..66b2de4a8f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -42,6 +42,7 @@ #include "FlightTaskManualAcceleration.hpp" #include "StickAccelerationXY.hpp" +#include "Gimbal.hpp" #include #include @@ -67,9 +68,15 @@ private: bool _velocity_limits_received_before{false}; + // Gimbal control + Gimbal _gimbal{this}; + uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)}; velocity_limits_s _velocity_limits{}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + bool haveTakenOff(); + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, (ParamInt) _param_mc_slow_map_hvel, (ParamInt) _param_mc_slow_map_vvel, @@ -83,6 +90,7 @@ private: (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_man_y_max + (ParamFloat) _param_mpc_man_y_max, + (ParamInt) _param_mc_slow_map_pitch ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c index 4c643d87d8..32558d98ea 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -156,3 +156,17 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f); * @group Multicopter Position Slow Mode */ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f); + +/** + * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode + * + * @value 0 No pitch rate input + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0); diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 6c32116237..44a5e1dba1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp + Gimbal.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp new file mode 100644 index 0000000000..21e97f5fc3 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 "Gimbal.hpp" +#include + +using namespace time_literals; +using namespace matrix; + +Gimbal::Gimbal(ModuleParams *parent) : + ModuleParams(parent) +{} + +Gimbal::~Gimbal() +{ + releaseGimbalControlIfNeeded(); +} + +bool Gimbal::checkForTelemetry(const hrt_abstime now) +{ + if (_gimbal_device_attitude_status_sub.updated()) { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status)) { + _telemtry_timestamp = gimbal_device_attitude_status.timestamp; + _telemetry_flags = gimbal_device_attitude_status.device_flags; + _telemetry_yaw = Eulerf(Quatf(gimbal_device_attitude_status.q)).psi(); + } + } + + return now < _telemtry_timestamp + 2_s; +} + +void Gimbal::acquireGimbalControlIfNeeded() +{ + if (!_have_gimbal_control) { + _have_gimbal_control = true; + + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _param_mav_sys_id.get(); + vehicle_command.param2 = _param_mav_comp_id.get(); + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_sys_id.get(); + vehicle_command.from_external = false; + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::releaseGimbalControlIfNeeded() +{ + if (_have_gimbal_control) { + _have_gimbal_control = false; + + // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior + publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, + Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, 0.f, 0.f)); + + // Release gimbal + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = -3.0f; // Remove control if it had it. + vehicle_command.param2 = -3.0f; // Remove control if it had it. + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.from_external = false; + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_comp_id.get(); + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates) +{ + gimbal_manager_set_attitude_s gimbal_setpoint{}; + gimbal_setpoint.origin_sysid = _param_mav_sys_id.get(); + gimbal_setpoint.origin_compid = _param_mav_comp_id.get(); + gimbal_setpoint.flags = gimbal_flags; + q_gimbal_setpoint.copyTo(gimbal_setpoint.q); + gimbal_setpoint.angular_velocity_x = gimbal_rates(0); + gimbal_setpoint.angular_velocity_y = gimbal_rates(1); + gimbal_setpoint.angular_velocity_z = gimbal_rates(2); + gimbal_setpoint.timestamp = hrt_absolute_time(); + _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp new file mode 100644 index 0000000000..938ef32aa2 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file Gimbal.hpp + * @brief Gimbal interface with flight tasks + * @author Pernilla Wikström + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Sticks.hpp" + + +class Gimbal : public ModuleParams +{ +public: + Gimbal(ModuleParams *parent); + ~Gimbal(); + + bool checkForTelemetry(const hrt_abstime now); + void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates); + void acquireGimbalControlIfNeeded(); + void releaseGimbalControlIfNeeded(); + float getTelemetryYaw() { return _telemetry_yaw; } + bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } + + static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK; + static constexpr uint16_t FLAGS_ROLL_PITCH_LOCKED = + gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + +private: + bool _have_gimbal_control{false}; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + hrt_abstime _telemtry_timestamp{}; + uint16_t _telemetry_flags{}; + float _telemetry_yaw{}; + + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id + ) +}; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index e20bf6c552..aef348ca53 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -43,6 +43,8 @@ #include #include +using namespace matrix; + namespace gimbal { @@ -936,21 +938,19 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con if (set_manual_control.origin_sysid == control_data.sysid_primary_control && set_manual_control.origin_compid == control_data.compid_primary_control) { - const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) - ? - matrix::Quatf( - matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) - : - matrix::Quatf(NAN, NAN, NAN, NAN); + Quatf q(NAN, NAN, NAN, NAN); - const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && - PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); + if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) { + q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)); + } + + Vector3f angular_velocity(NAN, NAN, NAN); + + if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) { + angular_velocity = Vector3f(0.0f, + set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch), + set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw)); + } _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, angular_velocity, set_manual_control.timestamp); From 3c129aefa15edc9fcdea3fdeaa5cbce9c5abdab3 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 18 Feb 2025 17:15:32 +0100 Subject: [PATCH 62/63] Add IC engine control module (#24055) Internal combustion engine control module. New actuator functions and RPM based start/restart logic. Not enabled by default. --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Pernilla --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 + ROMFS/px4fmu_common/init.d/rcS | 5 + msg/CMakeLists.txt | 1 + msg/InternalCombustionEngineControl.msg | 8 + src/lib/mixer_module/CMakeLists.txt | 1 + .../functions/FunctionICEngineControl.hpp | 83 ++++ src/lib/mixer_module/mixer_module.cpp | 1 + src/lib/mixer_module/mixer_module.hpp | 1 + src/lib/mixer_module/output_functions.yaml | 5 + .../CMakeLists.txt | 46 ++ .../InternalCombustionEngineControl.cpp | 397 ++++++++++++++++++ .../InternalCombustionEngineControl.hpp | 159 +++++++ .../Kconfig | 5 + .../module.yaml | 120 ++++++ src/modules/logger/logged_topics.cpp | 1 + 15 files changed, 838 insertions(+) create mode 100644 msg/InternalCombustionEngineControl.msg create mode 100644 src/lib/mixer_module/functions/FunctionICEngineControl.hpp create mode 100644 src/modules/internal_combustion_engine_control/CMakeLists.txt create mode 100644 src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp create mode 100644 src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp create mode 100644 src/modules/internal_combustion_engine_control/Kconfig create mode 100644 src/modules/internal_combustion_engine_control/module.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index ef2907deb7..c53dbbd46a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -337,6 +337,11 @@ then payload_deliverer start fi +if param compare -s ICE_EN 1 +then + internal_combustion_engine_control start +fi + #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 807a26ed03..b850985fd8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -555,6 +555,11 @@ else payload_deliverer start fi + if param compare -s ICE_EN 1 + then + internal_combustion_engine_control start + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index af7a98cf54..5844caafec 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -114,6 +114,7 @@ set(msg_files HeaterStatus.msg HoverThrustEstimate.msg InputRc.msg + InternalCombustionEngineControl.msg InternalCombustionEngineStatus.msg IridiumsbdStatus.msg IrlockReport.msg diff --git a/msg/InternalCombustionEngineControl.msg b/msg/InternalCombustionEngineControl.msg new file mode 100644 index 0000000000..08b4198e01 --- /dev/null +++ b/msg/InternalCombustionEngineControl.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +bool ignition_on # activate/deactivate ignition (Spark Plug) +float32 throttle_control # [0,1] - Motor should idle with 0. Includes slew rate if enabled. +float32 choke_control # [0,1] - 1 fully closes the air inlet. +float32 starter_engine_control # [0,1] - control value for electric starter motor. + +uint8 user_request # user intent for the ICE being on/off diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3dde..3a1581118b 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(mixer_module functions/FunctionConstantMax.hpp functions/FunctionConstantMin.hpp functions/FunctionGimbal.hpp + functions/FunctionICEngineControl.hpp functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp diff --git a/src/lib/mixer_module/functions/FunctionICEngineControl.hpp b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp new file mode 100644 index 0000000000..0609f5cc94 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "FunctionProviderBase.hpp" + +#include + +/** + * Functions: ICE... + */ +class FunctionICEControl : public FunctionProviderBase +{ +public: + FunctionICEControl() + { + resetAllToDisarmedValue(); + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionICEControl(); } + + void update() override + { + internal_combustion_engine_control_s internal_combustion_engine_control; + + // map [0, 1] to [-1, 1] which is the interface for non-motor PWM channels + if (_internal_combustion_engine_control_sub.update(&internal_combustion_engine_control)) { + _data[0] = internal_combustion_engine_control.ignition_on * 2.f - 1.f; + _data[1] = internal_combustion_engine_control.throttle_control * 2.f - 1.f; + _data[2] = internal_combustion_engine_control.choke_control * 2.f - 1.f; + _data[3] = internal_combustion_engine_control.starter_engine_control * 2.f - 1.f; + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::IC_Engine_Ignition]; } + +private: + static constexpr int num_data_points = 4; + + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + + static_assert(num_data_points == (int)OutputFunction::IC_Engine_Starter - (int)OutputFunction::IC_Engine_Ignition + 1, + "number of functions mismatch"); + + uORB::Subscription _internal_combustion_engine_control_sub{ORB_ID(internal_combustion_engine_control)}; + float _data[num_data_points] {}; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab540..56a2d61356 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -64,6 +64,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Gripper, &FunctionGripper::allocate}, {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, {OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate}, + {OutputFunction::IC_Engine_Ignition, OutputFunction::IC_Engine_Starter, &FunctionICEControl::allocate}, }; MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index ff400ca6d5..c55b72da4e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,6 +40,7 @@ #include "functions/FunctionConstantMin.hpp" #include "functions/FunctionGimbal.hpp" #include "functions/FunctionGripper.hpp" +#include "functions/FunctionICEngineControl.hpp" #include "functions/FunctionLandingGear.hpp" #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index a9965a5454..52543add97 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -38,6 +38,11 @@ functions: Landing_Gear_Wheel: 440 + IC_Engine_Ignition: 450 + IC_Engine_Throttle: 451 + IC_Engine_Choke: 452 + IC_Engine_Starter: 453 + # Add your own here: #MyCustomFunction: 10000 diff --git a/src/modules/internal_combustion_engine_control/CMakeLists.txt b/src/modules/internal_combustion_engine_control/CMakeLists.txt new file mode 100644 index 0000000000..15df6ca368 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE modules__internal_combustion_engine_control + MAIN internal_combustion_engine_control + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + InternalCombustionEngineControl.cpp + InternalCombustionEngineControl.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + SlewRate +) diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp new file mode 100644 index 0000000000..f4ab6a05aa --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -0,0 +1,397 @@ +/**************************************************************************** + * + * 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 "InternalCombustionEngineControl.hpp" + +#include + +using namespace time_literals; + +namespace internal_combustion_engine_control +{ + +InternalCombustionEngineControl::InternalCombustionEngineControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + _internal_combustion_engine_control_pub.advertise(); + _internal_combustion_engine_status_pub.advertise(); +} + +InternalCombustionEngineControl::~InternalCombustionEngineControl() +{ + +} + +int InternalCombustionEngineControl::task_spawn(int argc, char *argv[]) +{ + InternalCombustionEngineControl *obj = new InternalCombustionEngineControl(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void InternalCombustionEngineControl::start() +{ + ScheduleOnInterval(20_ms); // 50 Hz +} + +void InternalCombustionEngineControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + _throttle_control_slew_rate.setSlewRate(_param_ice_thr_slew.get()); + } + + + manual_control_setpoint_s manual_control_setpoint; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + + actuator_motors_s actuator_motors; + _actuator_motors.copy(&actuator_motors); + + const float throttle_in = actuator_motors.control[0]; + + const hrt_abstime now = hrt_absolute_time(); + + UserOnOffRequest user_request = UserOnOffRequest::Off; + + switch (static_cast(_param_ice_on_source.get())) { + case ICESource::ArmingState: { + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux1: { + if (manual_control_setpoint.aux1 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux2: { + if (manual_control_setpoint.aux2 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + } + + switch (_state) { + case State::Stopped: { + controlEngineStop(); + + if (user_request == UserOnOffRequest::On && !maximumAttemptsReached()) { + + _state = State::Starting; + _state_start_time = now; + PX4_INFO("ICE: Starting"); + } + } + break; + + case State::Starting: { + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + + switch (_starting_sub_state) { + case SubState::Rest: { + if (isStartingPermitted(now)) { + _state_start_time = now; + _starting_sub_state = SubState::Run; + } + } + break; + + case SubState::Run: + default: { + controlEngineStartup(now); + + if (isEngineRunning(now)) { + _state = State::Running; + PX4_INFO("ICE: Starting finished"); + + } else { + + if (maximumAttemptsReached()) { + _state = State::Fault; + PX4_WARN("ICE: Fault"); + + } else if (!isStartingPermitted(now)) { + controlEngineStop(); + _starting_sub_state = SubState::Rest; + } + } + + break; + } + + } + } + + } + break; + + case State::Running: { + controlEngineRunning(throttle_in); + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else if (!isEngineRunning(now) && _param_ice_running_fault_detection.get()) { + // without RPM feedback we assume the engine is running after the + // starting procedure but only switch state if fault detection is enabled + _state = State::Starting; + _state_start_time = now; + _starting_retry_cycle = 0; + PX4_WARN("ICE: Running Fault detected"); + events::send(events::ID("internal_combustion_engine_control_fault"), events::Log::Critical, + "IC engine fault detected"); + } + } + + break; + + case State::Fault: { + + // do nothing + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + controlEngineFault(); + } + } + + + break; + } + + const float control_interval = math::constrain(static_cast((now - _last_time_run) * 1e-6f), 0.01f, 0.1f); + + _last_time_run = now; + + // slew rate limit throttle control if it's finite, otherwise just pass it through (0 throttle = NAN = disarmed) + if (PX4_ISFINITE(_throttle_control)) { + _throttle_control = _throttle_control_slew_rate.update(_throttle_control, control_interval); + + } else { + _throttle_control_slew_rate.setForcedValue(0.f); + } + + publishControl(now, user_request); +} + +void InternalCombustionEngineControl::publishControl(const hrt_abstime now, const UserOnOffRequest user_request) +{ + internal_combustion_engine_control_s ice_control{}; + ice_control.timestamp = now; + ice_control.choke_control = _choke_control; + ice_control.ignition_on = _ignition_on; + ice_control.starter_engine_control = _starter_engine_control; + ice_control.throttle_control = _throttle_control; + ice_control.user_request = static_cast(user_request); + _internal_combustion_engine_control_pub.publish(ice_control); + + internal_combustion_engine_status_s ice_status; + ice_status.state = static_cast(_state); + ice_status.timestamp = now; + _internal_combustion_engine_status_pub.publish(ice_status); +} + +bool InternalCombustionEngineControl::isEngineRunning(const hrt_abstime now) +{ + rpm_s rpm; + + if (_rpm_sub.copy(&rpm)) { + const hrt_abstime rpm_timestamp = rpm.timestamp; + + return (_param_ice_min_run_rpm.get() > FLT_EPSILON && (now < rpm_timestamp + 2_s) + && rpm.rpm_estimate > _param_ice_min_run_rpm.get()); + } + + return false; +} + +void InternalCombustionEngineControl::controlEngineRunning(float throttle_in) +{ + _ignition_on = true; + _choke_control = 0.f; + _starter_engine_control = 0.f; + _throttle_control = throttle_in; + +} + +void InternalCombustionEngineControl::controlEngineStop() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineFault() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineStartup(const hrt_abstime now) +{ + float ignition_delay = 0.f; + float choke_duration = 0.f; + const float starter_duration = _param_ice_strt_dur.get(); + + if (_starting_retry_cycle == 0) { + ignition_delay = math::max(_param_ice_ign_delay.get(), 0.f); + + if (_param_ice_choke_st_dur.get() > FLT_EPSILON) { + choke_duration = _param_ice_choke_st_dur.get(); + } + } + + _ignition_on = true; + _throttle_control = _param_ice_strt_thr.get(); + _choke_control = now < _state_start_time + (choke_duration + ignition_delay) * 1_s ? 1.f : 0.f; + _starter_engine_control = now > _state_start_time + (ignition_delay * 1_s) ? 1.f : 0.f; + const hrt_abstime cycle_timeout_duration = (ignition_delay + choke_duration + starter_duration) * 1_s; + + if (now > _state_start_time + cycle_timeout_duration) { + // start resting timer if engine is not running + _starting_rest_time = now; + _starting_retry_cycle++; + PX4_INFO("ICE: starting attempt %i finished", _starting_retry_cycle); + } +} + +bool InternalCombustionEngineControl::isStartingPermitted(const hrt_abstime now) +{ + return now > _starting_rest_time + DELAY_BEFORE_RESTARTING * 1_s; +} + +bool InternalCombustionEngineControl::maximumAttemptsReached() +{ + // First and only attempt + if (_param_ice_strt_attempts.get() == 0) { + return _starting_retry_cycle > 0; + } + + return _starting_retry_cycle >= _param_ice_strt_attempts.get(); +} + +int InternalCombustionEngineControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The module controls internal combustion engine (ICE) features including: +ignition (on/off),throttle and choke level, starter engine delay, and user request. +The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). +The architecture is as shown below.: +![Architecture](../../assets/diagrams/ice_control_diagram.png) + +### Enabling +This feature is not enabled by default needs to be configured in the +build target for your board together with the rpm capture driver: +CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y +CONFIG_DRIVERS_RPM_CAPTURE=y + +Additionally, to enable the module: +- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN) +to true and adjust the other module parameters ICE_ according to your needs. +- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true. + +### Implementation +The ICE is implemented with a (4) state machine: +![Architecture](../../assets/diagrams/ice_control_state_machine.png) +The state machine: +- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running +- allows for user inputs from +- AUX{N} +- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md) + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("internal_combustion_engine_control", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int internal_combustion_engine_control_main(int argc, char *argv[]) +{ + return InternalCombustionEngineControl::main(argc, argv); +} + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp new file mode 100644 index 0000000000..01884e4967 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace internal_combustion_engine_control +{ + +class InternalCombustionEngineControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + InternalCombustionEngineControl(); + ~InternalCombustionEngineControl() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void start(); + +private: + void Run() override; + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + uORB::Subscription _actuator_motors{ORB_ID(actuator_motors)}; + + uORB::Publication _internal_combustion_engine_control_pub{ORB_ID(internal_combustion_engine_control)}; + uORB::Publication _internal_combustion_engine_status_pub{ORB_ID(internal_combustion_engine_status)}; + + + // has to mirror internal_combustion_engine_status_s::State + enum class State { + Stopped, + Starting, + Running, + Fault + } _state{State::Stopped}; + + enum class SubState { + Run, + Rest + }; + + enum class UserOnOffRequest { + Off, + On + }; + + enum class ICESource { + ArmingState, + Aux1, + Aux2, + }; + + hrt_abstime _state_start_time{0}; + hrt_abstime _last_time_run{0}; + + bool _ignition_on{false}; + float _choke_control{1.f}; + float _starter_engine_control{0.f}; + float _throttle_control{0.f}; + + SlewRate _throttle_control_slew_rate; + + bool isEngineRunning(const hrt_abstime now); + void controlEngineRunning(float throttle_in); + void controlEngineStop(); + void controlEngineStartup(const hrt_abstime now); + void controlEngineFault(); + bool maximumAttemptsReached(); + void publishControl(const hrt_abstime now, const UserOnOffRequest user_request); + + // Starting state specifics + static constexpr float DELAY_BEFORE_RESTARTING{1.f}; + int _starting_retry_cycle{0}; + hrt_abstime _starting_rest_time{0}; + SubState _starting_sub_state{SubState::Run}; + + /** + * @brief Currently the ICE starting state is permitted after resting + * DELAY_BEFORE_RESTARTING s to reduce wear on the starter motor. + * @param now current time + * @return if true, otherwise false + */ + bool isStartingPermitted(const hrt_abstime now); + + DEFINE_PARAMETERS( + (ParamInt) _param_ice_on_source, + (ParamFloat) _param_ice_choke_st_dur, + (ParamFloat) _param_ice_strt_dur, + (ParamFloat) _param_ice_min_run_rpm, + (ParamInt) _param_ice_strt_attempts, + (ParamInt) _param_ice_running_fault_detection, + (ParamFloat) _param_ice_strt_thr, + (ParamInt) _param_ice_stop_choke, + (ParamFloat) _param_ice_thr_slew, + (ParamFloat) _param_ice_ign_delay + ) +}; + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/Kconfig b/src/modules/internal_combustion_engine_control/Kconfig new file mode 100644 index 0000000000..c0d0e9049f --- /dev/null +++ b/src/modules/internal_combustion_engine_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL + bool "internal_combustion_engine_control" + default n + ---help--- + Enable support for internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml new file mode 100644 index 0000000000..517fc89fee --- /dev/null +++ b/src/modules/internal_combustion_engine_control/module.yaml @@ -0,0 +1,120 @@ +module_name: Internal Combustion Engine Control + +parameters: + - group: ICE + definitions: + + ICE_EN: + description: + short: Enable internal combustion engine + type: boolean + default: true + + ICE_ON_SOURCE: + description: + short: Engine start/stop input source + type: enum + default: 0 + values: + 0: On arming - disarming + 1: Aux1 + 2: Aux2 + + ICE_CHOKE_ST_DUR: + description: + short: Duration of choking during startup + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_STRT_DUR: + description: + short: Duration of single starting attempt (excl. choking) + long: | + Maximum expected time for startup before declaring timeout. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_MIN_RUN_RPM: + description: + short: Minimum RPM for engine to be declared running + type: float + unit: rpm + min: 0 + max: 10000 + decimal: 0 + increment: 1 + default: 2000 + + ICE_STRT_ATTEMPT: + description: + short: Number attempts for starting the engine + long: | + Number of accepted attempts for starting the engine before declaring a fault. + type: int32 + min: 0 + max: 10 + default: 3 + + ICE_RUN_FAULT_D: + description: + short: Fault detection if it stops in running state + long: | + Enables restart if a fault is detected during the running state. Otherwise + commands continues in running state until given an user request off. + type: boolean + default: true + + ICE_STRT_THR: + description: + short: Throttle value for starting engine + long: | + During the choking and the starting phase, the throttle value is set to this value. + type: float + unit: norm + min: 0 + max: 1 + decimal: 0 + increment: 0.01 + default: 0.1 + + ICE_STOP_CHOKE: + description: + short: Apply choke when stopping engine + type: boolean + default: true + + ICE_THR_SLEW: + description: + short: Throttle slew rate + long: | + Maximum rate of change of throttle value per second. + type: float + unit: 1/s + min: 0 + max: 1 + decimal: 2 + increment: 0.01 + default: 0.5 + + ICE_IGN_DELAY: + description: + short: Cold-start delay after ignition before engaging starter + long: | + In case that the ignition takes a moment to be up and running, this parameter can be set to account for that. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3114d83bb1..4882061d1d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -79,6 +79,7 @@ void LoggedTopics::add_default_topics() add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); + add_optional_topic("internal_combustion_engine_control", 10); add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("iridiumsbd_status", 1000); add_optional_topic("irlock_report", 1000); From 04a3c4af20df9e851e281776bfa51d02759ddee6 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Tue, 18 Feb 2025 18:26:38 +0100 Subject: [PATCH 63/63] Differential Rover: Refactor and clean up, align with Ackermann rover(#24318) * differential: refactor code architecture * Offboard fix * fix accel/decel slew rate --- .../init.d-posix/airframes/4009_gz_r1_rover | 44 +- .../airframes/50001_aion_robotics_r1_rover | 43 +- msg/CMakeLists.txt | 3 - msg/RoverDifferentialGuidanceStatus.msg | 7 - msg/RoverDifferentialSetpoint.msg | 9 - msg/RoverDifferentialStatus.msg | 14 - src/lib/rover_control/RoverControl.cpp | 89 +++- src/lib/rover_control/RoverControl.hpp | 19 + src/lib/rover_control/module.yaml | 18 +- src/modules/logger/logged_topics.cpp | 3 - src/modules/rover_ackermann/Kconfig | 2 +- src/modules/rover_differential/CMakeLists.txt | 14 +- .../CMakeLists.txt | 10 +- .../DifferentialAttControl.cpp | 208 +++++++++ .../DifferentialAttControl.hpp | 143 ++++++ .../CMakeLists.txt | 9 +- .../DifferentialPosVelControl.cpp | 406 ++++++++++++++++++ .../DifferentialPosVelControl.hpp | 243 +++++++++++ .../DifferentialRateControl/CMakeLists.txt | 39 ++ .../DifferentialRateControl.cpp | 190 ++++++++ .../DifferentialRateControl.hpp | 144 +++++++ src/modules/rover_differential/Kconfig | 3 +- .../rover_differential/RoverDifferential.cpp | 283 ++++-------- .../rover_differential/RoverDifferential.hpp | 119 ++--- .../RoverDifferentialControl.cpp | 277 ------------ .../RoverDifferentialControl.hpp | 169 -------- .../RoverDifferentialGuidance.cpp | 234 ---------- .../RoverDifferentialGuidance.hpp | 151 ------- src/modules/rover_differential/module.yaml | 191 +------- 29 files changed, 1722 insertions(+), 1362 deletions(-) delete mode 100644 msg/RoverDifferentialGuidanceStatus.msg delete mode 100644 msg/RoverDifferentialSetpoint.msg delete mode 100644 msg/RoverDifferentialStatus.msg rename src/modules/rover_differential/{RoverDifferentialControl => DifferentialAttControl}/CMakeLists.txt (85%) create mode 100644 src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp create mode 100644 src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp rename src/modules/rover_differential/{RoverDifferentialGuidance => DifferentialPosVelControl}/CMakeLists.txt (86%) create mode 100644 src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp create mode 100644 src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp create mode 100644 src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp create mode 100644 src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 3f96dad5f8..f05bbdcf45 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -11,29 +11,39 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 5 -param set-default RD_MAX_DECEL 10 -param set-default RD_MAX_JERK 30 param set-default RD_MAX_THR_YAW_R 1.5 -param set-default RD_YAW_RATE_P 0.25 -param set-default RD_YAW_RATE_I 0.01 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_MAX_SPEED 2 -param set-default RD_MAX_THR_SPD 2.15 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 -param set-default RD_MAX_YAW_RATE 180 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 -param set-default RD_MAX_YAW_ACCEL 1000 -# Pure pursuit parameters -param set-default PP_LOOKAHD_MAX 30 -param set-default PP_LOOKAHD_MIN 2 +# Rover Control Parameters +param set-default RO_ACCEL_LIM 5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_ACCEL_LIM 120 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 # Simulated sensors param set-default SENS_EN_GPSSIM 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 57a3644dae..45b35ffeaa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -21,26 +21,37 @@ param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 param set-default RBCLW_REV 1 # reverse right wheels -# Rover parameters + +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 3 -param set-default RD_MAX_DECEL 4 -param set-default RD_MAX_JERK 5 -param set-default RD_MAX_SPEED 1.6 -param set-default RD_MAX_THR_SPD 1.9 param set-default RD_MAX_THR_YAW_R 0.7 -param set-default RD_MAX_YAW_ACCEL 600 -param set-default RD_MAX_YAW_RATE 250 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 param set-default RD_TRANS_DRV_TRN 0.785398 param set-default RD_TRANS_TRN_DRV 0.139626 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_YAW_RATE_P 0.1 -param set-default RD_YAW_RATE_I 0.01 -# Pure pursuit parameters +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 4 +param set-default RO_JERK_LIM 5 +param set-default RO_MAX_THR_SPEED 1.9 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 250 +param set-default RO_YAW_ACCEL_LIM 600 +param set-default RO_YAW_DECEL_LIM 600 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 1.6 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5844caafec..ac59212f40 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -179,9 +179,6 @@ set(msg_files RoverRateStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg - RoverDifferentialGuidanceStatus.msg - RoverDifferentialSetpoint.msg - RoverDifferentialStatus.msg RoverMecanumGuidanceStatus.msg RoverMecanumSetpoint.msg RoverMecanumStatus.msg diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg deleted file mode 100644 index ce3d375111..0000000000 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] - -# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg deleted file mode 100644 index 100cc6b3cd..0000000000 --- a/msg/RoverDifferentialSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover -float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) -float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels -float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover - -# TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg deleted file mode 100644 index 767474fb5b..0000000000 --- a/msg/RoverDifferentialStatus.msg +++ /dev/null @@ -1,14 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_yaw # [rad] Measured yaw -float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate -float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller - -# TOPICS rover_differential_status diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp index 7b0864a428..0eafcaa9f8 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/rover_control/RoverControl.cpp @@ -48,26 +48,24 @@ float throttleControl(SlewRate &motor_setpoint, const float throttle_setp if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate motor_setpoint.setSlewRate(max_accel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - current_motor_setpoint)) { - motor_setpoint.setForcedValue(current_motor_setpoint); + motor_setpoint.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate motor_setpoint.setSlewRate(max_decel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - current_motor_setpoint)) { - motor_setpoint.setForcedValue(current_motor_setpoint); + motor_setpoint.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else { // Fallthrough if slew rate parameters are not configured motor_setpoint.setForcedValue(throttle_setpoint); } @@ -88,14 +86,13 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + adjusted_yaw_setpoint.update(yaw_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) { - adjusted_yaw_setpoint.setForcedValue(vehicle_yaw); + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } - adjusted_yaw_setpoint.update(yaw_setpoint, dt); - } else { adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } @@ -118,26 +115,24 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const // Apply acceleration and deceleration limit if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_accel); + speed_with_rate_limit.update(speed_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( speed_setpoint - vehicle_speed)) { - speed_with_rate_limit.setForcedValue(vehicle_speed); + speed_with_rate_limit.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_decel); + speed_with_rate_limit.update(speed_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( speed_setpoint - vehicle_speed)) { - speed_with_rate_limit.setForcedValue(vehicle_speed); + speed_with_rate_limit.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else { // Fallthrough if slew rate is not configured speed_with_rate_limit.setForcedValue(speed_setpoint); } @@ -153,12 +148,72 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const } // Feedback control - pid_speed.setSetpoint(speed_with_rate_limit.getState()); - forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) { + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + } else { + pid_speed.resetIntegral(); + } + return math::constrain(forward_speed_normalized, -1.f, 1.f); } +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint, + const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel, + const float wheel_track, const float dt) +{ + // Apply acceleration and deceleration limit + if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else { // Fallthrough if slew rate is not configured + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + // Feedback control + if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) { + pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); + + } else { + pid_yaw_rate.resetIntegral(); + } + + + return math::constrain(speed_diff_normalized, -1.f, 1.f); +} + void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, MapProjection &global_ned_proj_ref) diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp index 7f76710225..1196e04650 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/rover_control/RoverControl.hpp @@ -92,6 +92,25 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, f float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float speed_setpoint, float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt); +/** + * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint + * using a feedforward term and/or a PID controller. + * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels. + * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function). + * @param pid_yaw_rate Yaw rate PID (Updated by this function). + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. + * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2]. + * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2]. + * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m]. + * @param dt Time since last update [s]. + * @return Normalized speed difference setpoint [-1, 1]. + */ +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, + float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel, + float wheel_track, float dt); + /** * Projects positionSetpointTriplet waypoints from global to ned frame. * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml index 8450a03b06..a786a32c4b 100644 --- a/src/lib/rover_control/module.yaml +++ b/src/lib/rover_control/module.yaml @@ -133,7 +133,23 @@ parameters: RO_YAW_ACCEL_LIM: description: short: Yaw acceleration limit - long: Set to -1 to disable. + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can increase. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_DECEL_LIM: + description: + short: Yaw deceleration limit + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can decrease. + Set to -1 to disable. type: float unit: deg/s^2 min: -1 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 4882061d1d..b98d185aff 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -110,9 +110,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100); - add_optional_topic("rover_differential_guidance_status", 100); - add_optional_topic("rover_differential_setpoint", 100); - add_optional_topic("rover_differential_status", 100); add_optional_topic("rover_mecanum_guidance_status", 100); add_optional_topic("rover_mecanum_setpoint", 100); add_optional_topic("rover_mecanum_status", 100); diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index a6acd00251..bfcff4a895 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -2,4 +2,4 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n ---help--- - Enable support for rover_ackermann + Enable support for ackermann rovers diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 8805893160..8fafc30bab 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverDifferentialGuidance) -add_subdirectory(RoverDifferentialControl) +add_subdirectory(DifferentialRateControl) +add_subdirectory(DifferentialAttControl) +add_subdirectory(DifferentialPosVelControl) px4_add_module( MODULE modules__rover_differential @@ -41,10 +42,11 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS - RoverDifferentialGuidance - RoverDifferentialControl + DifferentialRateControl + DifferentialAttControl + DifferentialPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt rename to src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt index 7464de4f5b..456b21489d 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(RoverDifferentialControl - RoverDifferentialControl.cpp +px4_add_library(DifferentialAttControl + DifferentialAttControl.cpp ) -target_link_libraries(RoverDifferentialControl PUBLIC PID) -target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialAttControl PUBLIC PID) +target_include_directories(DifferentialAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp new file mode 100644 index 0000000000..a36ed41ada --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * 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 "DifferentialAttControl.hpp" + +using namespace time_literals; + +DifferentialAttControl::DifferentialAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void DifferentialAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void DifferentialAttControl::updateAttControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void DifferentialAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude && !_offboard_control_mode.position + && !_offboard_control_mode.velocity; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool DifferentialAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp new file mode 100644 index 0000000000..e4198384c0 --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential attitude control. + */ +class DifferentialAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialAttControl. + * @param parent The parent ModuleParams object. + */ + DifferentialAttControl(ModuleParams *parent); + ~DifferentialAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * or trajectorySetpoint (Offboard attitude control). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt rename to src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt index 81f350d484..a4a4795be7 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# 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 @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(RoverDifferentialGuidance - RoverDifferentialGuidance.cpp +px4_add_library(DifferentialPosVelControl + DifferentialPosVelControl.cpp ) -target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialPosVelControl PUBLIC PID) +target_include_directories(DifferentialPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp new file mode 100644 index 0000000000..32532f4c56 --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * 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 "DifferentialPosVelControl.hpp" + +using namespace time_literals; + +DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void DifferentialPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void DifferentialPosVelControl::updatePosVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { + + const float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (fabsf(speed_body_x_setpoint_normalized) > 1.f - + fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + _speed_body_x_setpoint = sign(_speed_body_x_setpoint) * + math::interpolate(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void DifferentialPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void DifferentialPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void DifferentialPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void DifferentialPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void DifferentialPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + // State machine + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + fabsf(_vehicle_speed_body_x)); + const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); + + if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + // Calculate desired speed in body x direction + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get()); + + } break; + + case GuidanceState::SPOT_TURNING: + if (fabsf(_vehicle_speed_body_x) > 0.f) { + yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop + + } + + _speed_body_x_setpoint = 0.f; + break; + + case GuidanceState::STOPPED: + default: + yaw_setpoint = _vehicle_yaw; + _speed_body_x_setpoint = 0.f; + break; + + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); +} + +void DifferentialPosVelControl::updateAutoSubscriptions() +{ + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float trans_drv_trn, const float miss_spd_gain) +{ + float speed_body_x_setpoint = cruising_speed; + + if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) { + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f); + + } else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed); + +} + +bool DifferentialPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp new file mode 100644 index 0000000000..0ae8a24eae --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential position/velocity control. + */ +class DifferentialPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialPosVelControl. + * @param parent The parent ModuleParams object. + */ + DifferentialPosVelControl(ModuleParams *parent); + ~DifferentialPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain); + + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain, + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt new file mode 100644 index 0000000000..0182e89d30 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(DifferentialRateControl + DifferentialRateControl.cpp +) + +target_link_libraries(DifferentialRateControl PUBLIC PID) +target_include_directories(DifferentialRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp new file mode 100644 index 0000000000..fe62328cd9 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * 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 "DifferentialRateControl.hpp" + +using namespace time_literals; + +DifferentialRateControl::DifferentialRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void DifferentialRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void DifferentialRateControl::updateRateControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void DifferentialRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.roll, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.position + && !_offboard_control_mode.velocity && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void DifferentialRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rd_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool DifferentialRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), + _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp new file mode 100644 index 0000000000..5b9bd6d632 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential rate control. + */ +class DifferentialRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialRateControl. + * @param parent The parent ModuleParams object. + */ + DifferentialRateControl(ModuleParams *parent); + ~DifferentialRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) + * or trajectorySetpoint (Offboard rate control). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_thr_yaw_r, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig index 840e2cdbf9..ed62f619b7 100644 --- a/src/modules/rover_differential/Kconfig +++ b/src/modules/rover_differential/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_DIFFERENTIAL bool "rover_differential" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of differential rovers + Enable support for differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index c5fe294cd3..5ad4439597 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * 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 @@ -33,12 +33,15 @@ #include "RoverDifferential.hpp" +using namespace time_literals; + RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_differential_setpoint_pub.advertise(); } bool RoverDifferential::init() @@ -50,219 +53,103 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverDifferential::Run() { - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude, rate and speed setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { - const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; - rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, - -_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); - - } else { - rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; - - } - - rover_differential_setpoint.yaw_rate_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) - if (!_yaw_ctl) { - _stab_desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Course control if the yaw rate input is zero (keep driving on a straight line) - if (!_yaw_ctl) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _yaw_ctl = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( - rover_differential_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, - _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); - rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? - yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); - -} - -void RoverDifferential::updateSubscriptions() -{ - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { // Reset on mode change - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } + _differential_pos_vel_control.updatePosVelControl(); + _differential_att_control.updateAttControl(); + _differential_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - _vehicle_attitude_sub.copy(&vehicle_attitude); - _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + if (_vehicle_control_mode.flag_armed) { + generateActuatorSetpoint(); + } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; +} + +void RoverDifferential::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } +void RoverDifferential::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} + +Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle_body_x -= sign(throttle_body_x) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle_body_x - speed_diff_normalized, + throttle_body_x + speed_diff_normalized); +} + int RoverDifferential::task_spawn(int argc, char *argv[]) { RoverDifferential *instance = new RoverDifferential(); @@ -288,7 +175,7 @@ int RoverDifferential::task_spawn(int argc, char *argv[]) int RoverDifferential::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverDifferential::print_usage(const char *reason) @@ -300,7 +187,7 @@ int RoverDifferential::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Differential controller. +Rover differential module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 3fa8f4891c..9c943627b0 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * 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 @@ -39,40 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" -#include "RoverDifferentialControl/RoverDifferentialControl.hpp" - -using namespace time_literals; - -// Constants -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value -static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "DifferentialRateControl/DifferentialRateControl.hpp" +#include "DifferentialAttControl/DifferentialAttControl.hpp" +#include "DifferentialPosVelControl/DifferentialPosVelControl.hpp" class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverDifferential + */ RoverDifferential(); ~RoverDifferential() override = default; @@ -88,51 +82,66 @@ public: bool init(); protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; - // uORB Publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverDifferentialGuidance _rover_differential_guidance{this}; - RoverDifferentialControl _rover_differential_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + DifferentialRateControl _differential_rate_control{this}; + DifferentialAttControl _differential_att_control{this}; + DifferentialPosVelControl _differential_pos_vel_control{this}; // Variables - Vector2f _curr_pos_ned{}; - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - int _nav_state{0}; - bool _armed{false}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode - float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode - Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode - Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp deleted file mode 100644 index a378de3b94..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverDifferentialControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_differential_status_pub.advertise(); -} - -void RoverDifferentialControl::updateParams() -{ - ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; - - // Update PID - _pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f); - _pid_yaw_rate.setIntegralLimit(1.f); - _pid_yaw_rate.setOutputLimit(1.f); - _pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f); - _pid_throttle.setIntegralLimit(1.f); - _pid_throttle.setOutputLimit(1.f); - _pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f); - _pid_yaw.setIntegralLimit(_max_yaw_rate); - _pid_yaw.setOutputLimit(_max_yaw_rate); - - // Update slew rates - if (_max_yaw_rate > FLT_EPSILON) { - _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); - } - - if (_max_yaw_accel > FLT_EPSILON) { - _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); - } - -} - -void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update differential setpoint - _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); - - // Closed loop yaw control (Overrides yaw rate setpoint) - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { - _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); - _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - _pid_yaw.setSetpoint( - matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - - vehicle_yaw)); // error as setpoint to take care of wrapping - _rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); - _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; - - } else { - _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, false); - - } else { // Use normalized setpoint - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized, - vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, true); - } - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { - - forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), -1.f, 1.f); - - if (fabsf(forward_speed_normalized) > 1.f - - fabsf(speed_diff_normalized)) { // Adjust forward speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - _rover_differential_setpoint.forward_speed_setpoint = sign(_rover_differential_setpoint.forward_speed_setpoint) * - math::interpolate(1.f - fabsf(speed_diff_normalized), -1.f, 1.f, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get()); - } - } - - - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false); - - } else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true); - - } - - // Publish rover differential status (logging) - _rover_differential_status.timestamp = _timestamp; - _rover_differential_status.measured_forward_speed = vehicle_forward_speed; - _rover_differential_status.measured_yaw = vehicle_yaw; - _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral(); - _rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral(); - _rover_differential_status_pub.publish(_rover_differential_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); -} - -float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, - const float max_thr_yaw_r, - const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID &pid_yaw_rate, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; - } - - if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); - yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - - } else { - yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); - } - - // Transform yaw rate into speed difference - float speed_diff_normalized{0.f}; - - if (normalized) { - speed_diff_normalized = yaw_rate_with_accel_limit.getState(); - - } else { - _rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState(); - - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); - } - - pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); - speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); - } - - return math::constrain(speed_diff_normalized, -1.f, 1.f); - -} - -float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, - const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, - PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) { - if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); - forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - - } - - } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); - forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - _rover_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), - -max_thr_spd, max_thr_spd, - -1.f, 1.f); - } - - pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt); - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, - const float speed_diff_normalized) -{ - float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - forward_speed_normalized -= sign(forward_speed_normalized) * excess; - } - - // Calculate the left and right wheel speeds - return Vector2f(forward_speed_normalized - speed_diff_normalized, - forward_speed_normalized + speed_diff_normalized); -} - -void RoverDifferentialControl::resetControllers() -{ - _pid_throttle.resetIntegral(); - _pid_yaw_rate.resetIntegral(); - _pid_yaw.resetIntegral(); -} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp deleted file mode 100644 index e08153356b..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialControl. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialControl(ModuleParams *parent); - ~RoverDifferentialControl() = default; - - /** - * @brief Compute motor commands based on setpoints - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - - /** - * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. - * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. - * @param vehicle_yaw_rate Measured yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. - * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. - * @param wheel_track Wheel track [m]. - * @param dt Time since last update [s]. - * @param yaw_rate_with_accel_limit Yaw rate slew rate. - * @param pid_yaw_rate Yaw rate PID - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); - /** - * @brief Compute normalized forward speed setpoint and apply slew rates. - * to the forward speed setpoint and doing closed loop speed control if enabled. - * @param forward_speed_setpoint Forward speed setpoint [m/s]. - * @param vehicle_forward_speed Actual forward speed [m/s]. - * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. - * @param forward_speed_setpoint_with_accel_limit Speed slew rate. - * @param pid_throttle Throttle PID - * @param max_accel Maximum acceleration [m/s^2] - * @param max_decel Maximum deceleration [m/s^2] - * @param dt Time since last update [s]. - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, - float dt, bool normalized); - - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * @param forward_speed_normalized Normalized forward speed [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. - */ - matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized); - - // uORB subscriptions - uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; - rover_differential_status_s _rover_differential_status{}; - - // Variables - rover_differential_setpoint_s _rover_differential_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - - // Controllers - PID _pid_throttle; // Closed loop speed control - PID _pid_yaw; // Closed loop yaw control - PID _pid_yaw_rate; // Closed loop yaw rate control - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - SlewRate _yaw_rate_with_accel_limit{0.f}; - SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_spd, - (ParamFloat) _param_rd_max_accel, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_yaw_accel, - (ParamFloat) _param_rd_yaw_rate_p, - (ParamFloat) _param_rd_yaw_rate_i, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, - (ParamFloat) _param_rd_p_gain_yaw, - (ParamFloat) _param_rd_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp deleted file mode 100644 index bb45e6b7b9..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ /dev/null @@ -1,234 +0,0 @@ -/**************************************************************************** - * - * 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 "RoverDifferentialGuidance.hpp" - -#include - -using namespace matrix; - -RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_forward_speed = _param_rd_max_speed.get(); - _rover_differential_guidance_status_pub.advertise(); -} - -void RoverDifferentialGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_speed, const int nav_state) -{ - updateSubscriptions(); - - // Catch return to launch - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - math::max(forward_speed, 0.f)); - const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); - - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { - if (_currentState == GuidanceState::STOPPED) { - _currentState = GuidanceState::DRIVING; - } - - if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - _currentState = GuidanceState::SPOT_TURNING; - - } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _currentState = GuidanceState::DRIVING; - } - - } else { // Mission finished or delay command - _currentState = GuidanceState::STOPPED; - } - - // Guidance logic - float desired_forward_speed{0.f}; - - switch (_currentState) { - case GuidanceState::DRIVING: { - // Calculate desired forward speed - desired_forward_speed = _max_forward_speed; - - if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) { - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, 0.0f); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); - } - - } else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON - && _param_rd_miss_spd_gain.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate( - M_PI_F - _waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction)); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, - _max_forward_speed); - } - - } break; - - case GuidanceState::SPOT_TURNING: - if (forward_speed > TURN_MAX_VELOCITY) { - desired_yaw = vehicle_yaw; // Wait for the rover to stop - - } - - break; - - case GuidanceState::STOPPED: - default: - desired_yaw = vehicle_yaw; - break; - - } - - // Publish differential guidance status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - rover_differential_guidance_status_s rover_differential_guidance_status{}; - rover_differential_guidance_status.timestamp = timestamp; - rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - rover_differential_guidance_status.state_machine = (uint8_t) _currentState; - _rover_differential_guidance_status_pub.publish(rover_differential_guidance_status); - - // Publish setpoints - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = desired_yaw; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); -} - -void RoverDifferentialGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } -} - -void RoverDifferentialGuidance::updateWaypoints() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } else { - _curr_wp = Vector2d(0, 0); - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } else { - _prev_wp = _curr_pos; - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } else { - _next_wp = _home_position; - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); - - } else { - _max_forward_speed = _param_rd_max_speed.get(); - } -} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp deleted file mode 100644 index cc1b033635..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - SPOT_TURNING, // The vehicle is currently turning on the spot. - DRIVING, // The vehicle is currently driving. - STOPPED // The vehicle is stopped. -}; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialGuidance. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialGuidance(ModuleParams *parent); - ~RoverDifferentialGuidance() = default; - - /** - * @brief Compute and publish attitude and velocity setpoints based on the mission plan. - * @param vehicle_yaw The yaw of the vehicle [rad]. - * @param forward_speed The forward speed of the vehicle [m/s]. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float vehicle_yaw, float forward_speed, int nav_state); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions. - */ - void updateSubscriptions(); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - - // uORB publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; - uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // Waypoints - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - Vector2d _prev_wp{}; - Vector2f _prev_wp_ned{}; - Vector2d _curr_wp{}; - Vector2f _curr_wp_ned{}; - Vector2d _next_wp{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s] - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - - // Constants - static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rd_max_jerk, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn, - (ParamFloat) _param_rd_miss_spd_gain - ) -}; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 6297f3aafd..eca54e798e 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -3,176 +3,17 @@ module_name: Rover Differential parameters: - group: Rover Differential definitions: - RD_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheel to the center of the left wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RD_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_SPEED_P: - description: - short: Proportional gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_SPEED_I: - description: - short: Integral gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_YAW_RATE_P: - description: - short: Proportional gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_RATE_I: - description: - short: Integral gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_DECEL: - description: - short: Maximum deceleration - long: | - Maximum decelaration is used to limit the deceleration of the rover. - Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: This disables the slow down effect in auto modes. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RD_MAX_SPEED: - description: - short: Maximum speed setpoint - long: | - This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_THR_SPD: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_YAW_RATE: - description: - short: Maximum allowed yaw rate for the rover - long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates - in Acro,Stabilized and Position mode. - type: float - unit: deg/s - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 90 - - RD_MAX_YAW_ACCEL: - description: - short: Maximum allowed yaw acceleration for the rover - long: | - This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints - to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. - This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RD_MAX_THR_YAW_R: description: @@ -181,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -189,11 +30,14 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 + default: 0 RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving + long: | + This threshold is used for the state machine to switch from turning to driving based on the + error between the desired and actual yaw. type: float unit: rad min: 0.001 @@ -227,9 +71,10 @@ parameters: The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP interpolated from [0, 180] -> [0, 1]. Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. type: float - min: 0.05 + min: -1 max: 100 increment: 0.01 decimal: 2 - default: 1 + default: -1