diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index bcfbdc2c6b..f64d29b684 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -17,7 +17,7 @@ on: jobs: build: name: Build and Push Container - runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] steps: - uses: actions/checkout@v4 with: @@ -52,12 +52,7 @@ jobs: ghcr.io/PX4/px4-dev ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }} tags: | - type=semver,pattern={{version}} - type=semver,pattern={{major}}.{{minor}} - type=semver,pattern={{major}} - type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700 - type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600 - type=ref,event=branch,suffix=,priority=500 + type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca268..93e6f0d33f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -12,6 +12,7 @@ param set-default MAV_TYPE 4 param set-default COM_PREARM_MODE 2 param set-default COM_SPOOLUP_TIME 10 +param set-default COM_DISARM_PRFLT 60 # No need for minimum collective pitch (or airmode) to keep torque authority param set-default MPC_MANTHR_MIN 0 diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 52d098101d..d0732e90ef 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -3,6 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default EKF2_MULTI_IMU 0 + # Mavlink ethernet (CFG 1000) param set-default MAV_2_CONFIG 1000 param set-default MAV_2_BROADCAST 1 @@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 param set-default SENS_EN_THERMAL 1 +param set-default SENS_IMU_MODE 1 param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_FF 0.0 #param set-default SENS_IMU_TEMP_I 0.025 diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults index 717632839b..1b92949d1d 100644 --- a/boards/ark/pi6x/init/rc.board_defaults +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -32,10 +32,11 @@ then param set-default SENS_TEMP_ID 2490378 fi -param set-default EKF2_MULTI_IMU 2 +param set-default EKF2_MULTI_IMU 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_OF_N_MIN 0.05 param set-default EKF2_RNG_A_HMAX 25 param set-default EKF2_RNG_QLTY_T 0.1 param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_MODE 1 diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 49b8f9e0d5..75e689ed42 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 39ed637d33..12259d0f34 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index fe8405e4d5..b5b1ab5275 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index e4ca03c8b5..a023dc8d3d 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index c912a7a6cb..2d92e89d96 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index 330e0fa594..fed1a0756b 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -11,11 +11,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index ef7458f7c3..149bcaff21 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index e9104cfaee..fbd4d3fdf0 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -104,3 +104,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 479e394f6f..2e2350dd5a 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index cc31ed3c2e..a74698d42d 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_BOARD_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y CONFIG_DRIVERS_SW_CRYPTO=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index b5f586406e..2c9e2d20d3 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -16,7 +16,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index f598bbb5d9..e8bcadebd7 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -45,4 +45,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index bc4c7d5d3f..42f8ec21ce 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6x/multicopter.px4board b/boards/px4/fmu-v6x/multicopter.px4board index 683220e0d7..9f540f567d 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -7,7 +7,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_COMMON_RC=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 9fdee39f1e..f9fea7df79 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef6..5636d855e5 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y diff --git a/msg/PwmInput.msg b/msg/PwmInput.msg index fcc7dbe4ac..805d6fbd6b 100644 --- a/msg/PwmInput.msg +++ b/msg/PwmInput.msg @@ -1,4 +1,4 @@ -uint64 timestamp # Time since system start (microseconds) -uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) -uint32 pulse_width # Pulse width, timer counts -uint32 period # Period, timer counts +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts (microseconds) +uint32 period # Period, timer counts (microseconds) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index ca69e50fdb..b4de2cf422 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +# rpm values of 0.0 mean within a timeout there is no movement measured float32 rpm_estimate # filtered revolutions per minute float32 rpm_raw diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 4c875d0fdf..9a213327fb 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 4c875d0fdffb4f9ba0e0e341ae567d4d0a544e46 +Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c diff --git a/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp index 3844df4c84..c6f0425569 100644 --- a/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/lib/actuator_effectiveness/ActuatorEffectivenessRotors.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/lib/actuator_effectiveness/CMakeLists.txt b/src/lib/actuator_effectiveness/CMakeLists.txt index 8df0043110..d320029cdf 100644 --- a/src/lib/actuator_effectiveness/CMakeLists.txt +++ b/src/lib/actuator_effectiveness/CMakeLists.txt @@ -34,10 +34,13 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp +<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt ActuatorEffectivenessRotors.cpp ActuatorEffectivenessRotors.hpp ActuatorEffectivenessThrusters.cpp ActuatorEffectivenessThrusters.hpp +======== +>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -45,7 +48,11 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D target_link_libraries(ActuatorEffectiveness PRIVATE mathlib + PID ) +<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness) +======== +>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt diff --git a/src/lib/control_allocation/CMakeLists.txt b/src/lib/control_allocation/CMakeLists.txt index 82f9bf37e2..eaf8abec61 100644 --- a/src/lib/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/CMakeLists.txt @@ -1,6 +1,10 @@ ############################################################################ # +<<<<<<< HEAD # Copyright (c) 2019 PX4 Development Team. All rights reserved. +======= +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +>>>>>>> origin # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -48,3 +52,5 @@ target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) px4_add_unit_gtest(SRC ControlAllocationMetric.cpp LINKLIBS ControlAllocation) # px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +add_subdirectory(control_allocation) +add_subdirectory(actuator_effectiveness) diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp new file mode 100644 index 0000000000..b36da8230f --- /dev/null +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectiveness.hpp" + +#include + +int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const matrix::Vector3f &torque, + const matrix::Vector3f &thrust) +{ + int actuator_idx = num_actuators_matrix[selected_matrix]; + + if (actuator_idx >= NUM_ACTUATORS) { + PX4_ERR("Too many actuators"); + return -1; + } + + if ((int)type < (int)ActuatorType::COUNT - 1 && num_actuators[(int)type + 1] > 0) { + PX4_ERR("Trying to add actuators in the wrong order (add motors first, then servos)"); + return -1; + } + + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1); + effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2); + matrix_selection_indexes[totalNumActuators()] = selected_matrix; + ++num_actuators[(int)type]; + return num_actuators_matrix[selected_matrix]++; +} + +void ActuatorEffectiveness::Configuration::actuatorsAdded(ActuatorType type, int count) +{ + int total_count = totalNumActuators(); + + for (int i = 0; i < count; ++i) { + matrix_selection_indexes[i + total_count] = selected_matrix; + } + + num_actuators[(int)type] += count; + num_actuators_matrix[selected_matrix] += count; +} + +int ActuatorEffectiveness::Configuration::totalNumActuators() const +{ + int total_count = 0; + + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + total_count += num_actuators_matrix[i]; + } + + return total_count; +} + +void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) +{ + for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) { + const uint32_t motor_mask = (1u << actuator_idx); + + if (stoppable_motors_mask & motor_mask) { + + // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy) + if (fabsf(actuator_sp(actuator_idx)) < .02f) { + _stopped_motors_mask |= motor_mask; + + } else { + _stopped_motors_mask &= ~motor_mask; + } + } + } +} diff --git a/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp new file mode 100644 index 0000000000..12ce9c7f4f --- /dev/null +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * 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 ActuatorEffectiveness.hpp + * + * Interface for Actuator Effectiveness + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +#include +#include + +enum class AllocationMethod { + NONE = -1, + PSEUDO_INVERSE = 0, + SEQUENTIAL_DESATURATION = 1, + AUTO = 2, + METRIC = 3, +}; + +enum class ActuatorType { + MOTORS = 0, + SERVOS, + THRUSTERS, + COUNT +}; + +enum class EffectivenessUpdateReason { + NO_EXTERNAL_UPDATE = 0, + CONFIGURATION_UPDATE = 1, + MOTOR_ACTIVATION_UPDATE = 2, +}; + + +class ActuatorEffectiveness +{ +public: + ActuatorEffectiveness() = default; + virtual ~ActuatorEffectiveness() = default; + + static constexpr int NUM_ACTUATORS = 16; + static constexpr int NUM_AXES = 6; + + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + + static constexpr int MAX_NUM_MATRICES = 2; + + using EffectivenessMatrix = matrix::Matrix; + using ActuatorVector = matrix::Vector; + + enum class FlightPhase { + HOVER_FLIGHT = 0, + FORWARD_FLIGHT = 1, + TRANSITION_HF_TO_FF = 2, + TRANSITION_FF_TO_HF = 3 + }; + + struct Configuration { + /** + * Add an actuator to the selected matrix, returning the index, or -1 on error + */ + int addActuator(ActuatorType type, const matrix::Vector3f &torque, const matrix::Vector3f &thrust); + + /** + * Call this after manually adding N actuators to the selected matrix + */ + void actuatorsAdded(ActuatorType type, int count); + + int totalNumActuators() const; + + /// Configured effectiveness matrix. Actuators are expected to be filled in order, motors first, then servos + EffectivenessMatrix effectiveness_matrices[MAX_NUM_MATRICES]; + + int num_actuators_matrix[MAX_NUM_MATRICES]; ///< current amount, and next actuator index to fill in to effectiveness_matrices + ActuatorVector trim[MAX_NUM_MATRICES]; + + ActuatorVector linearization_point[MAX_NUM_MATRICES]; + + int selected_matrix; + + uint8_t matrix_selection_indexes[NUM_ACTUATORS * MAX_NUM_MATRICES]; + int num_actuators[(int)ActuatorType::COUNT]; + }; + + /** + * Set the current flight phase + * + * @param Flight phase + */ + virtual void setFlightPhase(const FlightPhase &flight_phase) + { + _flight_phase = flight_phase; + } + + /** + * Get the number of effectiveness matrices. Must be <= MAX_NUM_MATRICES. + * This is expected to stay constant. + */ + virtual int numMatrices() const { return 1; } + + /** + * Get the desired allocation method(s) for each matrix, if configured as AUTO + */ + virtual void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const + { + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + allocation_method_out[i] = AllocationMethod::PSEUDO_INVERSE; + } + } + + /** + * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized + */ + virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const + { + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + normalize[i] = false; + } + } + + /** + * Get the control effectiveness matrix if updated + * + * @return true if updated and matrix is set + */ + virtual bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { return false;} + + /** + * Get the current flight phase + * + * @return Flight phase + */ + const FlightPhase &getFlightPhase() const + { + return _flight_phase; + } + + /** + * Display name + */ + virtual const char *name() const = 0; + + /** + * Callback from the control allocation, allowing to manipulate the setpoint. + * Used to allocate auxiliary controls to actuators (e.g. flaps and spoilers). + * + * @param actuator_sp input & output setpoint + */ + virtual void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) {} + + /** + * Callback from the control allocation, allowing to manipulate the setpoint. + * This can be used to e.g. add non-linear or external terms. + * It is called after the matrix multiplication and before final clipping. + * @param actuator_sp input & output setpoint + */ + virtual void updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) {} + + /** + * Get a bitmask of motors to be stopped + */ + virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; } + + /** + * Fill in the unallocated torque and thrust, customized by effectiveness type. + * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead. + */ + virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + + /** + * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero + * + * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand + * @param actuator_sp outcome of the allocation to determine if the motor should be stopped + */ + virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); + +protected: + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; + uint32_t _stopped_motors_mask{0}; +}; diff --git a/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt new file mode 100644 index 0000000000..d320029cdf --- /dev/null +++ b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt @@ -0,0 +1,58 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(ActuatorEffectiveness + ActuatorEffectiveness.cpp + ActuatorEffectiveness.hpp +<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessThrusters.cpp + ActuatorEffectivenessThrusters.hpp +======== +>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt +) + +target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ActuatorEffectiveness + PRIVATE + mathlib + PID +) +<<<<<<<< HEAD:src/lib/actuator_effectiveness/CMakeLists.txt + +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessThrustersTest.cpp LINKLIBS ActuatorEffectiveness) +======== +>>>>>>>> origin:src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt diff --git a/src/lib/control_allocation/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt new file mode 100644 index 0000000000..98037d023d --- /dev/null +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -0,0 +1,53 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(ControlAllocation + ControlAllocation.cpp + ControlAllocation.hpp + ControlAllocationMetric.cpp + ControlAllocationMetric.hpp + ControlAllocationPseudoInverse.cpp + ControlAllocationPseudoInverse.hpp + ControlAllocationSequentialDesaturation.cpp + ControlAllocationSequentialDesaturation.hpp +) +target_compile_options(ControlAllocation PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ControlAllocation PRIVATE mathlib) + +px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) +<<<<<<<< HEAD:src/lib/control_allocation/CMakeLists.txt +px4_add_unit_gtest(SRC ControlAllocationMetric.cpp LINKLIBS ControlAllocation) +======== +>>>>>>>> origin:src/lib/control_allocation/control_allocation/CMakeLists.txt +# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp new file mode 100644 index 0000000000..dfdf9ca7f7 --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 ControlAllocation.cpp + * + * Interface for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include "ControlAllocation.hpp" + +ControlAllocation::ControlAllocation() +{ + _control_allocation_scale.setAll(1.f); + _actuator_min.setAll(0.f); + _actuator_max.setAll(1.f); +} + +void +ControlAllocation::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) +{ + _effectiveness = effectiveness; + ActuatorVector linearization_point_clipped = linearization_point; + clipActuatorSetpoint(linearization_point_clipped); + _actuator_trim = actuator_trim + linearization_point_clipped; + clipActuatorSetpoint(_actuator_trim); + _num_actuators = num_actuators; + _control_trim = _effectiveness * linearization_point_clipped; +} + +void +ControlAllocation::setActuatorSetpoint( + const matrix::Vector &actuator_sp) +{ + // Set actuator setpoint + _actuator_sp = actuator_sp; + + // Clip + clipActuatorSetpoint(_actuator_sp); +} + +void +ControlAllocation::clipActuatorSetpoint(matrix::Vector &actuator) const +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_max(i) < _actuator_min(i)) { + actuator(i) = _actuator_trim(i); + + } else if (actuator(i) < _actuator_min(i)) { + actuator(i) = _actuator_min(i); + + } else if (actuator(i) > _actuator_max(i)) { + actuator(i) = _actuator_max(i); + } + } +} + +matrix::Vector +ControlAllocation::normalizeActuatorSetpoint(const matrix::Vector &actuator) +const +{ + matrix::Vector actuator_normalized; + + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_min(i) < _actuator_max(i)) { + actuator_normalized(i) = (actuator(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + + } else { + actuator_normalized(i) = (_actuator_trim(i) - _actuator_min(i)) / (_actuator_max(i) - _actuator_min(i)); + } + } + + return actuator_normalized; +} + +void ControlAllocation::applySlewRateLimit(float dt) +{ + for (int i = 0; i < _num_actuators; i++) { + if (_actuator_slew_rate_limit(i) > FLT_EPSILON) { + float delta_sp_max = dt * (_actuator_max(i) - _actuator_min(i)) / _actuator_slew_rate_limit(i); + float delta_sp = _actuator_sp(i) - _prev_actuator_sp(i); + + if (delta_sp > delta_sp_max) { + _actuator_sp(i) = _prev_actuator_sp(i) + delta_sp_max; + + } else if (delta_sp < -delta_sp_max) { + _actuator_sp(i) = _prev_actuator_sp(i) - delta_sp_max; + } + } + } +} diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp new file mode 100644 index 0000000000..e1eefe64a2 --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -0,0 +1,251 @@ +/**************************************************************************** + * + * 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 ControlAllocation.hpp + * + * Interface for Control Allocation Algorithms + * + * Implementers of this interface are expected to update the members + * of this base class in the `allocate` method. + * + * Example usage: + * ``` + * [...] + * // Initialization + * ControlAllocationMethodImpl alloc(); + * alloc.setEffectivenessMatrix(effectiveness, actuator_trim); + * alloc.setActuatorMin(actuator_min); + * alloc.setActuatorMin(actuator_max); + * + * while (1) { + * [...] + * + * // Set control setpoint, allocate actuator setpoint, retrieve actuator setpoint + * alloc.setControlSetpoint(control_sp); + * alloc.allocate(); + * actuator_sp = alloc.getActuatorSetpoint(); + * + * // Check if the control setpoint was fully allocated + * unallocated_control = control_sp - alloc.getAllocatedControl() + * + * [...] + * } + * ``` + * + * + * @author Julien Lecoeur + */ + +#pragma once + +#include + +<<<<<<<< HEAD:src/lib/control_allocation/ControlAllocation.hpp +#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +======== +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" +>>>>>>>> origin:src/lib/control_allocation/control_allocation/ControlAllocation.hpp + +class ControlAllocation +{ +public: + ControlAllocation(); + virtual ~ControlAllocation() = default; + + static constexpr uint8_t NUM_ACTUATORS = ActuatorEffectiveness::NUM_ACTUATORS; + static constexpr uint8_t NUM_AXES = ActuatorEffectiveness::NUM_AXES; + + typedef matrix::Vector ActuatorVector; + + enum ControlAxis { + ROLL = 0, + PITCH, + YAW, + THRUST_X, + THRUST_Y, + THRUST_Z + }; + + /** + * Allocate control setpoint to actuators + */ + virtual void allocate() = 0; + + /** + * Set actuator failure flag + * This prevents a change of the scaling in the matrix normalization step + * in case of a motor failure. + * + * @param failure Motor failure flag + */ + void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; } + + /** + * Set the control effectiveness matrix + * + * @param B Effectiveness matrix + */ + virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale); + + /** + * Get the allocated actuator vector + * + * @return Actuator vector + */ + const matrix::Vector &getActuatorSetpoint() const { return _actuator_sp; } + + /** + * Set the desired control vector + * + * @param Control vector + */ + void setControlSetpoint(const matrix::Vector &control) { _control_sp = control; } + + /** + * Get the desired control vector + * + * @return Control vector + */ + const matrix::Vector &getControlSetpoint() const { return _control_sp; } + + /** + * Get the allocated control vector + * + * @return Control vector + */ + matrix::Vector getAllocatedControl() const + { return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); } + + /** + * Get the control effectiveness matrix + * + * @return Effectiveness matrix + */ + const matrix::Matrix &getEffectivenessMatrix() const { return _effectiveness; } + + /** + * Set the minimum actuator values + * + * @param actuator_min Minimum actuator values + */ + void setActuatorMin(const matrix::Vector &actuator_min) { _actuator_min = actuator_min; } + + /** + * Get the minimum actuator values + * + * @return Minimum actuator values + */ + const matrix::Vector &getActuatorMin() const { return _actuator_min; } + + /** + * Set the maximum actuator values + * + * @param actuator_max Maximum actuator values + */ + void setActuatorMax(const matrix::Vector &actuator_max) { _actuator_max = actuator_max; } + + /** + * Get the maximum actuator values + * + * @return Maximum actuator values + */ + const matrix::Vector &getActuatorMax() const { return _actuator_max; } + + /** + * Set the current actuator setpoint. + * + * Use this when a new allocation method is started to initialize it properly. + * In most cases, it is not needed to call this method before `allocate()`. + * Indeed the previous actuator setpoint is expected to be stored during calls to `allocate()`. + * + * @param actuator_sp Actuator setpoint + */ + void setActuatorSetpoint(const matrix::Vector &actuator_sp); + + void setSlewRateLimit(const matrix::Vector &slew_rate_limit) + { _actuator_slew_rate_limit = slew_rate_limit; } + + /** + * Apply slew rate to current actuator setpoint + */ + void applySlewRateLimit(float dt); + + /** + * Clip the actuator setpoint between minimum and maximum values. + * + * The output is in the range [min; max] + * + * @param actuator Actuator vector to clip + */ + void clipActuatorSetpoint(matrix::Vector &actuator) const; + + void clipActuatorSetpoint() { clipActuatorSetpoint(_actuator_sp); } + + /** + * Normalize the actuator setpoint between minimum and maximum values. + * + * The output is in the range [-1; +1] + * + * @param actuator Actuator vector to normalize + * + * @return Clipped actuator setpoint + */ + matrix::Vector normalizeActuatorSetpoint(const matrix::Vector &actuator) + const; + + virtual void updateParameters() {} + + int numConfiguredActuators() const { return _num_actuators; } + + void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; } + +protected: + friend class ControlAllocator; // for _actuator_sp + + matrix::Matrix _effectiveness; ///< Effectiveness matrix + matrix::Vector _control_allocation_scale; ///< Scaling applied during allocation + matrix::Vector _actuator_trim; ///< Neutral actuator values + matrix::Vector _actuator_min; ///< Minimum actuator values + matrix::Vector _actuator_max; ///< Maximum actuator values + matrix::Vector _actuator_slew_rate_limit; ///< Slew rate limit + matrix::Vector _prev_actuator_sp; ///< Previous actuator setpoint + matrix::Vector _actuator_sp; ///< Actuator setpoint + matrix::Vector _control_sp; ///< Control setpoint + matrix::Vector _control_trim; ///< Control at trim actuator values + int _num_actuators{0}; + bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns + bool _had_actuator_failure{false}; +}; diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp new file mode 100644 index 0000000000..5d86814d7e --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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 ControlAllocationPseudoInverse.cpp + * + * Simple Control Allocation Algorithm + * + * @author Julien Lecoeur + */ + +#include "ControlAllocationPseudoInverse.hpp" + +void +ControlAllocationPseudoInverse::setEffectivenessMatrix( + const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) +{ + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators, + update_normalization_scale); + _mix_update_needed = true; + _normalization_needs_update = update_normalization_scale; +} + +void +ControlAllocationPseudoInverse::updatePseudoInverse() +{ + if (_mix_update_needed) { + matrix::geninv(_effectiveness, _mix); + + if (_normalization_needs_update && !_had_actuator_failure) { + updateControlAllocationMatrixScale(); + _normalization_needs_update = false; + } + + normalizeControlAllocationMatrix(); + _mix_update_needed = false; + } +} + +void +ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() +{ + // Same scale on roll and pitch + if (_normalize_rpy) { + + int num_non_zero_roll_torque = 0; + int num_non_zero_pitch_torque = 0; + + for (int i = 0; i < _num_actuators; i++) { + + if (fabsf(_mix(i, 0)) > 1e-3f) { + ++num_non_zero_roll_torque; + } + + if (fabsf(_mix(i, 1)) > 1e-3f) { + ++num_non_zero_pitch_torque; + } + } + + float roll_norm_scale = 1.f; + + if (num_non_zero_roll_torque > 0) { + roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f)); + } + + float pitch_norm_scale = 1.f; + + if (num_non_zero_pitch_torque > 0) { + pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f)); + } + + _control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale); + _control_allocation_scale(1) = _control_allocation_scale(0); + + // Scale yaw separately + _control_allocation_scale(2) = _mix.col(2).max(); + + } else { + _control_allocation_scale(0) = 1.f; + _control_allocation_scale(1) = 1.f; + _control_allocation_scale(2) = 1.f; + } + + // Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators + // (for tilted actuators) + _control_allocation_scale(THRUST_Z) = 1.f; + + for (int axis_idx = 2; axis_idx >= 0; --axis_idx) { + int num_non_zero_thrust = 0; + float norm_sum = 0.f; + + for (int i = 0; i < _num_actuators; i++) { + float norm = fabsf(_mix(i, 3 + axis_idx)); + norm_sum += norm; + + if (norm > FLT_EPSILON) { + ++num_non_zero_thrust; + } + } + + if (num_non_zero_thrust > 0) { + _control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust; + + } else { + _control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z); + } + } +} + +void +ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() +{ + if (_control_allocation_scale(0) > FLT_EPSILON) { + _mix.col(0) /= _control_allocation_scale(0); + _mix.col(1) /= _control_allocation_scale(1); + } + + if (_control_allocation_scale(2) > FLT_EPSILON) { + _mix.col(2) /= _control_allocation_scale(2); + } + + if (_control_allocation_scale(3) > FLT_EPSILON) { + _mix.col(3) /= _control_allocation_scale(3); + _mix.col(4) /= _control_allocation_scale(4); + _mix.col(5) /= _control_allocation_scale(5); + } + + // Set all the small elements to 0 to avoid issues + // in the control allocation algorithms + for (int i = 0; i < _num_actuators; i++) { + for (int j = 0; j < NUM_AXES; j++) { + if (fabsf(_mix(i, j)) < 1e-3f) { + _mix(i, j) = 0.f; + } + } + } +} + +void +ControlAllocationPseudoInverse::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + _prev_actuator_sp = _actuator_sp; + + // Allocate + _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); +} diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp new file mode 100644 index 0000000000..27c367376b --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * 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 ControlAllocationPseudoInverse.hpp + * + * Simple Control Allocation Algorithm + * + * It computes the pseudo-inverse of the effectiveness matrix + * Actuator saturation is handled by simple clipping, do not + * expect good performance in case of actuator saturation. + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "ControlAllocation.hpp" + +class ControlAllocationPseudoInverse: public ControlAllocation +{ +public: + ControlAllocationPseudoInverse() = default; + virtual ~ControlAllocationPseudoInverse() = default; + + void allocate() override; + void setEffectivenessMatrix(const matrix::Matrix &effectiveness, + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) override; + +protected: + matrix::Matrix _mix; + + bool _mix_update_needed{false}; + + /** + * Recalculate pseudo inverse if required. + * + */ + void updatePseudoInverse(); + +private: + void normalizeControlAllocationMatrix(); + void updateControlAllocationMatrixScale(); + bool _normalization_needs_update{false}; +}; diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp new file mode 100644 index 0000000000..89faab8c92 --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 ControlAllocationPseudoInverseTest.cpp + * + * Tests for Control Allocation Algorithms + * + * @author Julien Lecoeur + */ + +#include +#include + +using namespace matrix; + +TEST(ControlAllocationTest, AllZeroCase) +{ + ControlAllocationPseudoInverse method; + + matrix::Vector control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + matrix::Vector actuator_sp_expected; + + method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false); + method.setControlSetpoint(control_sp); + method.allocate(); + method.clipActuatorSetpoint(); + actuator_sp = method.getActuatorSetpoint(); + control_allocated_expected = method.getAllocatedControl(); + + EXPECT_EQ(actuator_sp, actuator_sp_expected); + EXPECT_EQ(control_allocated, control_allocated_expected); +} diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp new file mode 100644 index 0000000000..4b28f44dac --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * 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 ControlAllocationSequentialDesaturation.cpp + * + * @author Roman Bapst + * @author Beat Küng + */ + +#include "ControlAllocationSequentialDesaturation.hpp" + + +void +ControlAllocationSequentialDesaturation::allocate() +{ + //Compute new gains if needed + updatePseudoInverse(); + + _prev_actuator_sp = _actuator_sp; + + switch (_param_mc_airmode.get()) { + case 1: + mixAirmodeRP(); + break; + + case 2: + mixAirmodeRPY(); + break; + + default: + mixAirmodeDisabled(); + break; + } +} + +void ControlAllocationSequentialDesaturation::desaturateActuators( + ActuatorVector &actuator_sp, + const ActuatorVector &desaturation_vector, bool increase_only) +{ + float gain = computeDesaturationGain(desaturation_vector, actuator_sp); + + if (increase_only && gain < 0.f) { + return; + } + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } + + gain = 0.5f * computeDesaturationGain(desaturation_vector, actuator_sp); + + for (int i = 0; i < _num_actuators; i++) { + actuator_sp(i) += gain * desaturation_vector(i); + } +} + +float ControlAllocationSequentialDesaturation::computeDesaturationGain(const ActuatorVector &desaturation_vector, + const ActuatorVector &actuator_sp) +{ + float k_min = 0.f; + float k_max = 0.f; + + for (int i = 0; i < _num_actuators; i++) { + // Do not use try to desaturate using an actuator with weak effectiveness to avoid large desaturation gains + if (fabsf(desaturation_vector(i)) < 0.2f) { + continue; + } + + if (actuator_sp(i) < _actuator_min(i)) { + float k = (_actuator_min(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + + if (actuator_sp(i) > _actuator_max(i)) { + float k = (_actuator_max(i) - actuator_sp(i)) / desaturation_vector(i); + + if (k < k_min) { k_min = k; } + + if (k > k_max) { k_max = k; } + } + } + + // Reduce the saturation as much as possible + return k_min + k_max; +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRP() +{ + // Airmode for roll and pitch, but not yaw + + // Mix without yaw + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeRPY() +{ + // Airmode for roll, pitch and yaw + + // Do full mixing + ActuatorVector thrust_z; + ActuatorVector yaw; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + yaw(i) = _mix(i, ControlAxis::YAW); + } + + desaturateActuators(_actuator_sp, thrust_z); + + // Unsaturate yaw (in case upper and lower bounds are exceeded) + // to prioritize roll/pitch over yaw. + desaturateActuators(_actuator_sp, yaw); +} + +void +ControlAllocationSequentialDesaturation::mixAirmodeDisabled() +{ + // Airmode disabled: never allow to increase the thrust to unsaturate a motor + + // Mix without yaw + ActuatorVector thrust_z; + ActuatorVector roll; + ActuatorVector pitch; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) = _actuator_trim(i) + + _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + + _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + + _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + roll(i) = _mix(i, ControlAxis::ROLL); + pitch(i) = _mix(i, ControlAxis::PITCH); + } + + // only reduce thrust + desaturateActuators(_actuator_sp, thrust_z, true); + + // Reduce roll/pitch acceleration if needed to unsaturate + desaturateActuators(_actuator_sp, roll); + desaturateActuators(_actuator_sp, pitch); + + // Mix yaw independently + mixYaw(); +} + +void +ControlAllocationSequentialDesaturation::mixYaw() +{ + // Add yaw to outputs + ActuatorVector yaw; + ActuatorVector thrust_z; + + for (int i = 0; i < _num_actuators; i++) { + _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _mix(i, ControlAxis::YAW); + thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + } + + // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), + // and allow some yaw response at maximum thrust + ActuatorVector max_prev = _actuator_max; + _actuator_max += (_actuator_max - _actuator_min) * MINIMUM_YAW_MARGIN; + desaturateActuators(_actuator_sp, yaw); + _actuator_max = max_prev; + + // reduce thrust only + desaturateActuators(_actuator_sp, thrust_z, true); +} + +void +ControlAllocationSequentialDesaturation::updateParameters() +{ + updateParams(); +} diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp new file mode 100644 index 0000000000..dfa69d9777 --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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 ControlAllocationSequentialDesaturation.hpp + * + * Control Allocation Algorithm which sequentially modifies control demands in order to + * eliminate the saturation of the actuator setpoint vector. + * + * + * @author Roman Bapst + */ + +#pragma once + +#include "ControlAllocationPseudoInverse.hpp" + +#include + +class ControlAllocationSequentialDesaturation: public ControlAllocationPseudoInverse, public ModuleParams +{ +public: + + ControlAllocationSequentialDesaturation() : ModuleParams(nullptr) {} + virtual ~ControlAllocationSequentialDesaturation() = default; + + void allocate() override; + + void updateParameters() override; + + // This is the minimum actuator yaw granted when the controller is saturated. + // In the yaw-only case where outputs are saturated, thrust is reduced by up to this amount. + static constexpr float MINIMUM_YAW_MARGIN{0.15f}; +private: + + /** + * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector. + * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular + * acceleration on a specific axis. + * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the + * saturation will be minimized by shifting the vertical thrust setpoint, without changing the + * roll/pitch/yaw accelerations. + * + * Note that as we only slide along the given axis, in extreme cases outputs can still contain values + * outside of [min_output, max_output]. + * + * @param actuator_sp Actuator setpoint, vector that is modified + * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale + * @param increase_only if true, only allow to increase (add) a fraction of desaturation_vector + */ + void desaturateActuators(ActuatorVector &actuator_sp, const ActuatorVector &desaturation_vector, + bool increase_only = false); + + /** + * Computes the gain k by which desaturation_vector has to be multiplied + * in order to unsaturate the output that has the greatest saturation. + * + * @return desaturation gain + */ + float computeDesaturationGain(const ActuatorVector &desaturation_vector, const ActuatorVector &actuator_sp); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: airmode for roll/pitch: + * thrust is increased/decreased as much as required to meet the demanded roll/pitch. + * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior. + */ + void mixAirmodeRP(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: full airmode for roll/pitch/yaw: + * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, + * while giving priority to roll and pitch over yaw. + */ + void mixAirmodeRPY(); + + /** + * Mix roll, pitch, yaw, thrust and set the actuator setpoint. + * + * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded + * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed. + * Thrust can be reduced to unsaturate the upper side. + * @see mixYaw() for the exact yaw behavior. + */ + void mixAirmodeDisabled(); + + /** + * Mix yaw by updating the actuator setpoint (that already contains roll/pitch/thrust). + * + * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow + * some yaw control on the upper end. On the lower end thrust will never be increased, + * but yaw is decreased as much as required. + */ + void mixYaw(); + + DEFINE_PARAMETERS( + (ParamInt) _param_mc_airmode ///< air-mode + ); +}; diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp new file mode 100644 index 0000000000..2e0af6bff4 --- /dev/null +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ControlAllocationSequentialDesaturationTest.cpp + * + * Tests for Control Allocation Sequential Desaturation Algorithms + * + */ + +#include +#include +#include + +using namespace matrix; + +namespace +{ + +// Makes and returns a Geometry object for a "standard" quad-x quadcopter. +ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() +{ + ActuatorEffectivenessRotors::Geometry geometry = {}; + geometry.rotors[0].position(0) = 1.0f; + geometry.rotors[0].position(1) = 1.0f; + geometry.rotors[0].position(2) = 0.0f; + geometry.rotors[0].axis(0) = 0.0f; + geometry.rotors[0].axis(1) = 0.0f; + geometry.rotors[0].axis(2) = -1.0f; + geometry.rotors[0].thrust_coef = 1.0f; + geometry.rotors[0].moment_ratio = 0.05f; + + geometry.rotors[1].position(0) = -1.0f; + geometry.rotors[1].position(1) = -1.0f; + geometry.rotors[1].position(2) = 0.0f; + geometry.rotors[1].axis(0) = 0.0f; + geometry.rotors[1].axis(1) = 0.0f; + geometry.rotors[1].axis(2) = -1.0f; + geometry.rotors[1].thrust_coef = 1.0f; + geometry.rotors[1].moment_ratio = 0.05f; + + geometry.rotors[2].position(0) = 1.0f; + geometry.rotors[2].position(1) = -1.0f; + geometry.rotors[2].position(2) = 0.0f; + geometry.rotors[2].axis(0) = 0.0f; + geometry.rotors[2].axis(1) = 0.0f; + geometry.rotors[2].axis(2) = -1.0f; + geometry.rotors[2].thrust_coef = 1.0f; + geometry.rotors[2].moment_ratio = -0.05f; + + geometry.rotors[3].position(0) = -1.0f; + geometry.rotors[3].position(1) = 1.0f; + geometry.rotors[3].position(2) = 0.0f; + geometry.rotors[3].axis(0) = 0.0f; + geometry.rotors[3].axis(1) = 0.0f; + geometry.rotors[3].axis(2) = -1.0f; + geometry.rotors[3].thrust_coef = 1.0f; + geometry.rotors[3].moment_ratio = -0.05f; + + geometry.num_rotors = 4; + + return geometry; +} + +// Returns an effective matrix for a sample quad-copter configuration. +ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() +{ + ActuatorEffectiveness::EffectivenessMatrix effectiveness; + effectiveness.setZero(); + const auto geometry = make_quad_x_geometry(); + ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + return effectiveness; +} + +// Configures a ControlAllocationSequentialDesaturation object for a sample quad-copter. +void setup_quad_allocator(ControlAllocationSequentialDesaturation &allocator) +{ + const auto effectiveness = make_quad_x_effectiveness(); + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + constexpr bool UPDATE_NORMALIZATION_SCALE{false}; + allocator.setEffectivenessMatrix( + effectiveness, + actuator_trim, + linearization_point, + ActuatorEffectiveness::NUM_ACTUATORS, + UPDATE_NORMALIZATION_SCALE + ); +} + +static constexpr float EXPECT_NEAR_TOL{1e-4f}; + +} // namespace + +// This tests that yaw-only control setpoint at zero actuator setpoint results in zero actuator +// allocation. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledOnlyYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = 1.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = 0.f; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + matrix::Vector zero; + EXPECT_EQ(actuator_sp, zero); +} + +// This tests that a control setpoint for z-thrust returns the desired actuator setpoint. +// Each motor should have an actuator setpoint that when summed together should be equal to +// control setpoint. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustZ) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + constexpr int MOTOR_COUNT{4}; + constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT}; + + for (int i{0}; i < MOTOR_COUNT; ++i) { + EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test does not saturate the yaw response. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is low enough to not saturate the motors. + constexpr float YAW_CONTROL_SP{0.02f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, + // this will need to be changed. + constexpr float YAW_EFFECTIVENESS_FACTOR{5.f}; + constexpr float YAW_DIFF_PER_MOTOR{YAW_CONTROL_SP * YAW_EFFECTIVENESS_FACTOR}; + // At yaw condition, there will be 2 different actuator values. + constexpr int MOTOR_COUNT{4}; + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + YAW_DIFF_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - YAW_DIFF_PER_MOTOR}; + + for (int i{0}; i < MOTOR_COUNT / 2; ++i) { + EXPECT_NEAR(actuator_sp(i), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT / 2}; i < MOTOR_COUNT; ++i) { + EXPECT_NEAR(actuator_sp(i), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test saturates the yaw response, but does not reduce total thrust. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndSaturatedYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is arbitrarily high to trigger strongest possible (saturated) yaw response. + constexpr float YAW_CONTROL_SP{0.25f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // At max yaw, only 2 motors will carry all of the thrust. + constexpr int YAW_MOTORS{2}; + constexpr float THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / YAW_MOTORS}; + + for (int i{0}; i < YAW_MOTORS; ++i) { + EXPECT_NEAR(actuator_sp(i), THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + } + + for (int i{YAW_MOTORS}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. +// This test does not saturate the pitch response. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledThrustAndPitch) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f}; + // This is low enough to not saturate the motors. + constexpr float PITCH_CONTROL_SP{0.1f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // This value is based off of the effectiveness matrix. If the effectiveness matrix is changed, + // this will need to be changed. + constexpr int MOTOR_COUNT{4}; + constexpr float PITCH_DIFF_PER_MOTOR{PITCH_CONTROL_SP / MOTOR_COUNT}; + // At control set point, there will be 2 different actuator values. + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_DIFF_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_DIFF_PER_MOTOR}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + yaw returns the desired actuator setpoint. +// This test saturates yaw and demonstrates reduction of thrust for yaw. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndYaw) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float DESIRED_THRUST_Z_PER_MOTOR{0.8f}; + constexpr int MOTOR_COUNT{4}; + constexpr float THRUST_Z_TOTAL{-DESIRED_THRUST_Z_PER_MOTOR * MOTOR_COUNT}; + // This is arbitrarily high to trigger strongest possible (saturated) yaw response. + constexpr float YAW_CONTROL_SP{1.f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = 0.f; + control_sp(ControlAllocation::ControlAxis::YAW) = YAW_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + // In the case of yaw saturation, thrust per motor will be reduced by the hard-coded + // magic-number yaw margin of 0.15f. + constexpr float YAW_MARGIN{0.15f}; // get this from a centralized source when available. + constexpr float YAW_DIFF_PER_MOTOR{1.0f + YAW_MARGIN - DESIRED_THRUST_Z_PER_MOTOR}; + // At control set point, there will be 2 different actuator values. + constexpr float HIGH_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR + YAW_DIFF_PER_MOTOR - YAW_MARGIN}; + constexpr float LOW_THRUST_Z_PER_MOTOR{DESIRED_THRUST_Z_PER_MOTOR - YAW_DIFF_PER_MOTOR - YAW_MARGIN}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} + +// This tests that a control setpoint for z-thrust + pitch returns the desired actuator setpoint. +// This test saturates the pitch response such that thrust is reduced to (partially) compensate. +TEST(ControlAllocationSequentialDesaturationTest, AirmodeDisabledReducedThrustAndPitch) +{ + ControlAllocationSequentialDesaturation allocator; + setup_quad_allocator(allocator); + matrix::Vector control_sp; + // Negative, because +z is "downward". + constexpr float THRUST_Z_TOTAL{-0.75f * 4.f}; + // This is high enough to saturate the pitch control. + constexpr float PITCH_CONTROL_SP{2.f}; + control_sp(ControlAllocation::ControlAxis::ROLL) = 0.f; + control_sp(ControlAllocation::ControlAxis::PITCH) = PITCH_CONTROL_SP; + control_sp(ControlAllocation::ControlAxis::YAW) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_X) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Y) = 0.f; + control_sp(ControlAllocation::ControlAxis::THRUST_Z) = THRUST_Z_TOTAL; + allocator.setControlSetpoint(control_sp); + + // Since MC_AIRMODE was not set explicitly, assume airmode is disabled. + allocator.allocate(); + + const auto &actuator_sp = allocator.getActuatorSetpoint(); + constexpr int MOTOR_COUNT{4}; + // The maximum actuator value is + // THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT. + // The amount over 1 is the amount that each motor is reduced by. + // At control set point, there will be 2 different actuator values. + constexpr float OVERAGE_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - 1}; + EXPECT_TRUE(OVERAGE_PER_MOTOR > 0.f); + constexpr float HIGH_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT + PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; + constexpr float LOW_THRUST_Z_PER_MOTOR{-THRUST_Z_TOTAL / MOTOR_COUNT - PITCH_CONTROL_SP / MOTOR_COUNT - OVERAGE_PER_MOTOR}; + EXPECT_NEAR(actuator_sp(0), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(1), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(2), HIGH_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + EXPECT_NEAR(actuator_sp(3), LOW_THRUST_Z_PER_MOTOR, EXPECT_NEAR_TOL); + + for (int i{MOTOR_COUNT}; i < ActuatorEffectiveness::NUM_ACTUATORS; ++i) { + EXPECT_NEAR(actuator_sp(i), 0.f, EXPECT_NEAR_TOL); + } +} diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5fc8ee4d8f..fef03b92c2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, + (_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index cf69743af6..04c1a0d05a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -330,7 +330,6 @@ private: param_t _param_rc_map_fltmode{PARAM_INVALID}; DEFINE_PARAMETERS( - (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, @@ -347,6 +346,7 @@ private: (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, + (ParamFloat) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e..0bc3f9c86e 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -314,7 +314,7 @@ ControlAllocator::Run() #endif // Check if parameters have changed - if (_parameter_update_sub.updated() && !_armed) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 446fd44926..7047363cc8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index c640ba83c7..b7906669f2 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 55c6282b46..f0b095709e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b67..76481da71b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector @@ -41,6 +41,8 @@ #include #include +#include "RpmControl.hpp" + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -131,4 +133,6 @@ private: bool _main_motor_engaged{true}; const ActuatorType _tail_actuator_type; + + RpmControl _rpm_control{this}; }; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index a01306d972..a507aee2dd 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index d35ecadc54..0b12482781 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index 11111e073a..e5b7f3dcf8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 48aa604eae..c6f0425569 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -41,7 +41,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 5c1388de15..f6108b4baf 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness { diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 2160ca929c..9f945a6cd8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index ba5255eb52..708f104faa 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -39,7 +39,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index e96ec61def..512bd41f41 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index 936d0e4b67..1255d1b282 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 8235747dde..1b89da8d9b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -33,7 +33,7 @@ #pragma once -#include "actuator_effectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt index 14c3a37ecd..349893a3ea 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -27,6 +27,8 @@ px4_add_library(VehicleActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp + RpmControl.cpp ) target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 0000000000..53d9766e2d --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RpmControl.hpp" + +#include + +using namespace time_literals; + +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pid.setOutputLimit(PID_OUTPUT_LIMIT); + _pid.setIntegralLimit(PID_OUTPUT_LIMIT); +}; + +void RpmControl::setSpoolupProgress(float spoolup_progress) +{ + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) { + _pid.resetIntegral(); + } +} + +float RpmControl::getActuatorCorrection() +{ + hrt_abstime now = hrt_absolute_time(); + + // RPM measurement update + if (_rpm_sub.updated()) { + rpm_s rpm{}; + + if (_rpm_sub.copy(&rpm)) { + const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f); + _timestamp_last_measurement = rpm.timestamp; + + const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; + } + } + + // Timeout + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { + _pid.resetIntegral(); + _actuator_correction = 0.f; + } + + return _actuator_correction; +} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp new file mode 100644 index 0000000000..5fd0c96d91 --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RpmControl.hpp + * + * Control rpm of a helicopter rotor. + * Input: PWM input pulse period from an rpm sensor + * Output: Duty cycle command for the ESC + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include +#include + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent); + ~RpmControl() = default; + + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); + +private: + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] + + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; + PID _pid; + float _spoolup_progress{0.f}; // [0,1] + hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout + float _actuator_correction{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_heli_rpm_sp, + (ParamFloat) _param_ca_heli_rpm_p, + (ParamFloat) _param_ca_heli_rpm_i + ) +}; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df4..013b954033 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -528,6 +528,41 @@ parameters: which is mostly the case when the main rotor turns counter-clockwise. type: boolean default: 0 + CA_HELI_RPM_SP: + description: + short: Setpoint for main rotor rpm + long: | + Requires rpm feedback for the controller. + type: float + decimal: 0 + increment: 1 + min: 100 + max: 10000 + default: 1500 + CA_HELI_RPM_P: + description: + short: Proportional gain for rpm control + long: | + Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it. + + motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000 + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 + CA_HELI_RPM_I: + description: + short: Integral gain for rpm control + long: | + Same definition as the proportional gain but for integral. + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 # Others CA_FAILURE_MODE: diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 0c9a5df6cb..e7b2eea396 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -280,7 +280,17 @@ void Ekf::constrainStateVariances() void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), min, max); + if (P(i, i) < min) { + P(i, i) = min; + + } else if (P(i, i) > max) { + // Constrain the variance growth by fusing zero innovation as clipping the variance + // would artifically increase the correlation between states and destabilize the filter. + const float innov = 0.f; + const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) + const float innov_var = P(i, i) + R; + fuseDirectStateMeasurement(innov, innov_var, R, i); + } } } @@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float limited_max = math::constrain(state_var_max, min, max); float limited_min = math::constrain(limited_max / max_ratio, min, max); - for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), limited_min, limited_max); - } + constrainStateVar(state, limited_min, limited_max); } void Ekf::resetQuatCov(const float yaw_noise) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 93f96a656c..04bcfc104c 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -34,7 +34,7 @@ depends on MODULES_EKF2 menuconfig EKF2_AUX_GLOBAL_POSITION depends on MODULES_EKF2 bool "aux global position fusion support" - default n + default y ---help--- EKF2 auxiliary global position fusion support. diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d1e67a5cd7..3e91b24619 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); - add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); @@ -251,6 +250,10 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + +#ifdef CONFIG_BOARD_UAVCAN_INTERFACES + add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES); +#endif } void LoggedTopics::add_high_rate_topics() diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c15..995eea4256 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -98,9 +99,11 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -118,8 +121,10 @@ private: matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + float _hover_thrust{NAN}; + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index f6cc5db43d..22b78d519c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; + { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } + } + } + + if (!PX4_ISFINITE(_hover_thrust)) { + _hover_thrust = _param_mpc_thr_hover.get(); + } + + // throttle_stick_input is in range [-1, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; - default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle - thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, - {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale to hover throttle at 0 stick input + thrust = math::interpolateNXY(throttle_stick_input, + {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()}); break; } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 07c23bbb5d..d8b97336b9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run() } } - // new local position setpoint needed every iteration - if (!_vehicle_local_position_setpoint_sub.updated()) { - return; - } - // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run() _hover_thrust_ekf.predict(dt); - vehicle_local_position_setpoint_s local_pos_sp; + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); - if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { - if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + control_allocator_status_s control_allocator_status; + + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint) + && _control_allocator_status_sub.update(&control_allocator_status) + && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms) + && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms) + ) { + matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust); + matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated; + + const matrix::Quatf q_att{vehicle_attitude.q}; + matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated); + + if (PX4_ISFINITE(thrust_allocated(2))) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects @@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run() 1.f); _hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z)); - _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); @@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run() _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state(); - publishStatus(local_pos.timestamp); + publishStatus(vehicle_thrust_setpoint.timestamp); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index b1d0324298..1738cfe785 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -54,10 +54,12 @@ #include #include #include +#include #include #include -#include #include +#include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +103,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; hrt_abstime _timestamp_last{0};