From f5c05f6d0113304e480e3db1ea94dd6d0ed2ff34 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Fri, 10 Jan 2025 17:22:02 +0100 Subject: [PATCH 01/24] Take 2: Cleanup circular dependencies ActuatorEffectiveness --- .../ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp | 1 - .../ActuatorEffectivenessRoverAckermann.cpp | 1 - .../ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp | 1 - 3 files changed, 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index c7c2bba7d7..0a4516ba56 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessFixedWing.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 94f5db16f3..e9eda4c538 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessRoverAckermann.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 79853c7518..f15624dd67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessStandardVTOL.hpp" -#include using namespace matrix; From 28fa04438616d16363306878e6bfee0473c77afc Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 13 Jan 2025 10:30:35 +0100 Subject: [PATCH 02/24] MC-stabilized: rescale thrust input to hover thrust at zero stick input Use hover thrust estimate in stabilized mode to rescale stick inputs. Prevents vehicle from losing/gaining altitude when switching from position to stabilized mode. --- src/modules/mc_att_control/mc_att_control.hpp | 13 ++++++---- .../mc_att_control/mc_att_control_main.cpp | 24 +++++++++++++++---- 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c15..995eea4256 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -98,9 +99,11 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -118,8 +121,10 @@ private: matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + float _hover_thrust{NAN}; + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index f6cc5db43d..22b78d519c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; + { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } + } + } + + if (!PX4_ISFINITE(_hover_thrust)) { + _hover_thrust = _param_mpc_thr_hover.get(); + } + + // throttle_stick_input is in range [-1, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; - default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle - thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, - {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale to hover throttle at 0 stick input + thrust = math::interpolateNXY(throttle_stick_input, + {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()}); break; } From 879e0ea9b10493029851bdbc651c3ce6d7a46a56 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 13 Jan 2025 10:33:14 +0100 Subject: [PATCH 03/24] MC-hte: use allocated thrust as input for hover thrust estimator. Improves estimates on vehicles where thrust is often saturating. --- .../MulticopterHoverThrustEstimator.cpp | 30 ++++++++++++------- .../MulticopterHoverThrustEstimator.hpp | 9 ++++-- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 07c23bbb5d..d8b97336b9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run() } } - // new local position setpoint needed every iteration - if (!_vehicle_local_position_setpoint_sub.updated()) { - return; - } - // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run() _hover_thrust_ekf.predict(dt); - vehicle_local_position_setpoint_s local_pos_sp; + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); - if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { - if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + control_allocator_status_s control_allocator_status; + + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint) + && _control_allocator_status_sub.update(&control_allocator_status) + && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms) + && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms) + ) { + matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust); + matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated; + + const matrix::Quatf q_att{vehicle_attitude.q}; + matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated); + + if (PX4_ISFINITE(thrust_allocated(2))) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects @@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run() 1.f); _hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z)); - _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); @@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run() _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state(); - publishStatus(local_pos.timestamp); + publishStatus(vehicle_thrust_setpoint.timestamp); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index b1d0324298..1738cfe785 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -54,10 +54,12 @@ #include #include #include +#include #include #include -#include #include +#include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +103,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; hrt_abstime _timestamp_last{0}; From 0fb8463b790def648c97e59359d487682528324b Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 13 Jan 2025 08:15:40 -0900 Subject: [PATCH 04/24] logger: always log can_interface_status (#24071) * logger: always log can_interface_status * logger: log topic can_interface_status using CONFIG_BOARD_UAVCAN_INTERFACES --- src/modules/logger/logged_topics.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index d1e67a5cd7..3e91b24619 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); - add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); @@ -251,6 +250,10 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + +#ifdef CONFIG_BOARD_UAVCAN_INTERFACES + add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES); +#endif } void LoggedTopics::add_high_rate_topics() From e4e975806feb46f96eb7c85d5d1568747d9fe3b4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 13 Jan 2025 12:06:53 +0100 Subject: [PATCH 05/24] Enable CONFIG_EKF2_AUX_GLOBAL_POSITION by default Signed-off-by: Silvan Fuhrer --- boards/px4/fmu-v5/rover.px4board | 1 - boards/px4/fmu-v5/stackcheck.px4board | 1 - boards/px4/fmu-v5x/rover.px4board | 1 - boards/px4/fmu-v6c/rover.px4board | 1 - boards/px4/fmu-v6u/rover.px4board | 1 - boards/px4/fmu-v6x/multicopter.px4board | 1 - boards/px4/fmu-v6x/rover.px4board | 1 - boards/px4/fmu-v6xrt/rover.px4board | 1 - boards/px4/sitl/default.px4board | 1 - src/modules/ekf2/Kconfig | 2 +- 10 files changed, 1 insertion(+), 10 deletions(-) diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index b5f586406e..2c9e2d20d3 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -16,7 +16,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index f598bbb5d9..e8bcadebd7 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -45,4 +45,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index bc4c7d5d3f..42f8ec21ce 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6x/multicopter.px4board b/boards/px4/fmu-v6x/multicopter.px4board index 683220e0d7..9f540f567d 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -7,7 +7,6 @@ CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_COMMON_RC=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 53c70cc09f..9eed13c665 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -12,7 +12,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 9fdee39f1e..f9fea7df79 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_DRIVERS_ROBOCLAW=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set CONFIG_MODULES_ROVER_ACKERMANN=y CONFIG_MODULES_ROVER_DIFFERENTIAL=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 1584f93ef6..5636d855e5 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y -CONFIG_EKF2_AUX_GLOBAL_POSITION=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 93f96a656c..04bcfc104c 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -34,7 +34,7 @@ depends on MODULES_EKF2 menuconfig EKF2_AUX_GLOBAL_POSITION depends on MODULES_EKF2 bool "aux global position fusion support" - default n + default y ---help--- EKF2 auxiliary global position fusion support. From a16f7859ac00b0c1c207789030ca9e5dd41aed04 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 13 Jan 2025 13:39:43 +0100 Subject: [PATCH 06/24] boards: disable CONFIG_EKF2_AUX_GLOBAL_POSITION on some boards Signed-off-by: Silvan Fuhrer --- boards/holybro/kakutef7/default.px4board | 1 + boards/holybro/kakuteh7/default.px4board | 1 + boards/holybro/kakuteh7mini/default.px4board | 1 + boards/holybro/kakuteh7v2/default.px4board | 1 + boards/modalai/fc-v1/default.px4board | 1 + boards/px4/fmu-v2/default.px4board | 1 + boards/px4/fmu-v5/cryptotest.px4board | 1 + 7 files changed, 7 insertions(+) diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 49b8f9e0d5..75e689ed42 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 39ed637d33..12259d0f34 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index fe8405e4d5..b5b1ab5275 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index e4ca03c8b5..a023dc8d3d 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index e9104cfaee..fbd4d3fdf0 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -104,3 +104,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 479e394f6f..2e2350dd5a 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set # CONFIG_EKF2_AUXVEL is not set # CONFIG_EKF2_BARO_COMPENSATION is not set # CONFIG_EKF2_DRAG_FUSION is not set diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index cc31ed3c2e..a74698d42d 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_BOARD_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y CONFIG_DRIVERS_SW_CRYPTO=y +# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub" From e89a79b382e7688034450b7da767a78a178ed6b9 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 10 Jan 2025 17:09:01 -0700 Subject: [PATCH 07/24] boards: disable multi ekf on all ark flight controllers --- boards/ark/fmu-v6x/init/rc.board_defaults | 3 +++ boards/ark/pi6x/init/rc.board_defaults | 3 ++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 52d098101d..d0732e90ef 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -3,6 +3,8 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default EKF2_MULTI_IMU 0 + # Mavlink ethernet (CFG 1000) param set-default MAV_2_CONFIG 1000 param set-default MAV_2_BROADCAST 1 @@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 param set-default SENS_EN_THERMAL 1 +param set-default SENS_IMU_MODE 1 param set-default SENS_IMU_TEMP 10.0 #param set-default SENS_IMU_TEMP_FF 0.0 #param set-default SENS_IMU_TEMP_I 0.025 diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults index 717632839b..1b92949d1d 100644 --- a/boards/ark/pi6x/init/rc.board_defaults +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -32,10 +32,11 @@ then param set-default SENS_TEMP_ID 2490378 fi -param set-default EKF2_MULTI_IMU 2 +param set-default EKF2_MULTI_IMU 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_OF_N_MIN 0.05 param set-default EKF2_RNG_A_HMAX 25 param set-default EKF2_RNG_QLTY_T 0.1 param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_MODE 1 From 50092a7f67606967419a839a3fe192c59aa05735 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Jan 2025 09:53:32 +0100 Subject: [PATCH 08/24] NuttX: update submodule to branch px4_firmware_nuttx-10.3.0+ with "FlexSPI allow RWW" merged --- platforms/nuttx/NuttX/nuttx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 4c875d0fdf..9a213327fb 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 4c875d0fdffb4f9ba0e0e341ae567d4d0a544e46 +Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c From 940fe45ba7e92af35c2a73355198f9be55b6cf51 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 19:53:44 +0100 Subject: [PATCH 09/24] ControlAllocator: introduce helicopter rotor rpm controller --- msg/CMakeLists.txt | 1 + msg/PwmInput.msg | 8 +- msg/RpmControlStatus.msg | 7 + .../ActuatorEffectivenessHelicopter.cpp | 7 +- .../ActuatorEffectivenessHelicopter.hpp | 4 + .../ActuatorEffectiveness/CMakeLists.txt | 2 + .../ActuatorEffectiveness/RpmControl.hpp | 150 ++++++++++++++++++ .../control_allocator/ControlAllocator.cpp | 2 +- src/modules/control_allocator/module.yaml | 35 ++++ 9 files changed, 209 insertions(+), 7 deletions(-) create mode 100644 msg/RpmControlStatus.msg create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7abae77293..d8d2393c00 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,6 +181,7 @@ set(msg_files RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg + RpmControlStatus.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/PwmInput.msg b/msg/PwmInput.msg index fcc7dbe4ac..805d6fbd6b 100644 --- a/msg/PwmInput.msg +++ b/msg/PwmInput.msg @@ -1,4 +1,4 @@ -uint64 timestamp # Time since system start (microseconds) -uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) -uint32 pulse_width # Pulse width, timer counts -uint32 period # Period, timer counts +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts (microseconds) +uint32 period # Period, timer counts (microseconds) diff --git a/msg/RpmControlStatus.msg b/msg/RpmControlStatus.msg new file mode 100644 index 0000000000..c1ed3dd997 --- /dev/null +++ b/msg/RpmControlStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 rpm_raw # measured rpm +float32 rpm_estimate # filtered rpm +float32 rpm_setpoint # desired rpm + +float32 output diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b67..76481da71b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector #include +#include "RpmControl.hpp" + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -131,4 +133,6 @@ private: bool _main_motor_engaged{true}; const ActuatorType _tail_actuator_type; + + RpmControl _rpm_control{this}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 2406b81bf8..a203ac9821 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -69,6 +70,7 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D target_link_libraries(ActuatorEffectiveness PRIVATE mathlib + PID ) px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp new file mode 100644 index 0000000000..a5f413178e --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RpmControl.hpp + * + * Control rpm of a helicopter rotor. + * Input: PWM input pulse period from an rpm sensor + * Output: Duty cycle command for the ESC + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +using namespace time_literals; + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + ~RpmControl() = default; + + void setSpoolupProgress(float spoolup_progress) + { + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < .8f) { + _pid.resetIntegral(); + } + } + + float getActuatorCorrection() + { + if (_pwm_input_sub.updated()) { + pwm_input_s pwm_input{}; + + if (_pwm_input_sub.copy(&pwm_input)) { + if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) { + // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute + _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 1.f); + + } else { + _rpm_raw = 0; + } + + _timestamp_last_rpm_measurement = pwm_input.timestamp; + } + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); + _timestamp_last_update = now; + + _rpm_filter.setParameters(dt, 0.5f); + _rpm_filter.update(_rpm_raw); + + const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s); + const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f; + + if (no_rpm_pulse_timeout && no_excessive_rpm) { + const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + + } else { + _pid.setGains(0.f, 0.f, 0.f); + } + + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + + float output = _pid.update(_rpm_filter.getState(), dt, true); + + rpm_control_status_s rpm_control_status{}; + rpm_control_status.rpm_raw = _rpm_raw; + rpm_control_status.rpm_estimate = _rpm_filter.getState(); + rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); + rpm_control_status.output = output; + rpm_control_status.timestamp = hrt_absolute_time(); + _rpm_control_status_pub.publish(rpm_control_status); + + // Publish estimated rpm for MAVLink -> UI in ground station + rpm_s rpm{ + .timestamp = hrt_absolute_time(), + .indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm + }; + + _rpm_pub.publish(rpm); + + return output; + } + +private: + uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; + uORB::Publication _rpm_pub {ORB_ID::rpm}; + + float _rpm_raw{0.f}; + float _spoolup_progress{0.f}; + AlphaFilter _rpm_filter; + PID _pid; + hrt_abstime _timestamp_last_update{0}; + hrt_abstime _timestamp_last_rpm_measurement{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_heli_rpm_sp, + (ParamFloat) _param_ca_heli_rpm_p, + (ParamFloat) _param_ca_heli_rpm_i + ) +}; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e..0bc3f9c86e 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -314,7 +314,7 @@ ControlAllocator::Run() #endif // Check if parameters have changed - if (_parameter_update_sub.updated() && !_armed) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df4..013b954033 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -528,6 +528,41 @@ parameters: which is mostly the case when the main rotor turns counter-clockwise. type: boolean default: 0 + CA_HELI_RPM_SP: + description: + short: Setpoint for main rotor rpm + long: | + Requires rpm feedback for the controller. + type: float + decimal: 0 + increment: 1 + min: 100 + max: 10000 + default: 1500 + CA_HELI_RPM_P: + description: + short: Proportional gain for rpm control + long: | + Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it. + + motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000 + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 + CA_HELI_RPM_I: + description: + short: Integral gain for rpm control + long: | + Same definition as the proportional gain but for integral. + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 # Others CA_FAILURE_MODE: From ee67e4bb282386c20ea85c7a24d5d7eeee519e35 Mon Sep 17 00:00:00 2001 From: oravla5 Date: Wed, 11 Dec 2024 18:55:57 +0100 Subject: [PATCH 10/24] RpmControl: class clean up --- .../ActuatorEffectiveness/RpmControl.hpp | 53 +++++++------------ 1 file changed, 18 insertions(+), 35 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index a5f413178e..fb509a5715 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -72,33 +72,24 @@ public: float getActuatorCorrection() { - if (_pwm_input_sub.updated()) { - pwm_input_s pwm_input{}; + if (_rpm_sub.updated()) { + rpm_s rpm_input{}; - if (_pwm_input_sub.copy(&pwm_input)) { - if ((1 < pwm_input.period) && (pwm_input.period < 1_s)) { - // 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute - _rpm_raw = 60.f * 1e6f / (static_cast(pwm_input.period) * 1.f); - - } else { - _rpm_raw = 0; - } - - _timestamp_last_rpm_measurement = pwm_input.timestamp; + if (_rpm_sub.copy(&rpm_input)) { + _rpm_estimate = rpm_input.rpm_estimate; + _rpm_raw = rpm_input.rpm_raw; + _timestamp_last_rpm_measurement = rpm_input.timestamp; } } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 0.01f, 1.f); - _timestamp_last_update = now; + hrt_abstime current_time = hrt_absolute_time(); + const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = current_time; - _rpm_filter.setParameters(dt, 0.5f); - _rpm_filter.update(_rpm_raw); + const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s; + const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; - const bool no_rpm_pulse_timeout = now < (_timestamp_last_rpm_measurement + 1_s); - const bool no_excessive_rpm = _rpm_filter.getState() < 1800.f; - - if (no_rpm_pulse_timeout && no_excessive_rpm) { + if (rpm_measurement_timeout && no_excessive_rpm) { const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); @@ -109,38 +100,30 @@ public: _pid.setOutputLimit(.5f); _pid.setIntegralLimit(.5f); - float output = _pid.update(_rpm_filter.getState(), dt, true); + float output = _pid.update(_rpm_estimate, dt, true); rpm_control_status_s rpm_control_status{}; rpm_control_status.rpm_raw = _rpm_raw; - rpm_control_status.rpm_estimate = _rpm_filter.getState(); + rpm_control_status.rpm_estimate = _rpm_estimate;; rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); rpm_control_status.output = output; rpm_control_status.timestamp = hrt_absolute_time(); _rpm_control_status_pub.publish(rpm_control_status); - // Publish estimated rpm for MAVLink -> UI in ground station - rpm_s rpm{ - .timestamp = hrt_absolute_time(), - .indicated_frequency_rpm = _rpm_filter.getState() // scale up to 10'000rpm - }; - - _rpm_pub.publish(rpm); - return output; } private: - uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)}; + static constexpr float RPM_MAX_VALUE = 1800.f; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; - uORB::Publication _rpm_pub {ORB_ID::rpm}; + float _rpm_estimate{0.f}; float _rpm_raw{0.f}; float _spoolup_progress{0.f}; - AlphaFilter _rpm_filter; PID _pid; - hrt_abstime _timestamp_last_update{0}; hrt_abstime _timestamp_last_rpm_measurement{0}; + hrt_abstime _timestamp_last_update{0}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_heli_rpm_sp, From cd0e04f8b07df05b388f31c1ef24a67cd2cb675f Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:18:51 +0100 Subject: [PATCH 11/24] RpmControl: name current timestamp now following the convention --- .../ActuatorEffectiveness/RpmControl.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index fb509a5715..c4485c57ec 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -82,11 +82,11 @@ public: } } - hrt_abstime current_time = hrt_absolute_time(); - const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = current_time; + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = now; - const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s; + const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; if (rpm_measurement_timeout && no_excessive_rpm) { From 4050cedfafc996b9b40a16449196c6089f996ca9 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:19:53 +0100 Subject: [PATCH 12/24] RpmControl: call local message instance after message name following the convention --- .../ActuatorEffectiveness/RpmControl.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index c4485c57ec..9bdbf562b2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -73,12 +73,12 @@ public: float getActuatorCorrection() { if (_rpm_sub.updated()) { - rpm_s rpm_input{}; + rpm_s rpm{}; - if (_rpm_sub.copy(&rpm_input)) { - _rpm_estimate = rpm_input.rpm_estimate; - _rpm_raw = rpm_input.rpm_raw; - _timestamp_last_rpm_measurement = rpm_input.timestamp; + if (_rpm_sub.copy(&rpm)) { + _rpm_estimate = rpm.rpm_estimate; + _rpm_raw = rpm.rpm_raw; + _timestamp_last_rpm_measurement = rpm.timestamp; } } From ddd410e9d8aa114a2cba2c864863f757af08d3df Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 11 Dec 2024 20:27:59 +0100 Subject: [PATCH 13/24] RpmControl: remove status message because it by now only contains redundant information --- msg/CMakeLists.txt | 1 - msg/RpmControlStatus.msg | 7 ------- .../ActuatorEffectiveness/RpmControl.hpp | 12 ------------ 3 files changed, 20 deletions(-) delete mode 100644 msg/RpmControlStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d8d2393c00..7abae77293 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -181,7 +181,6 @@ set(msg_files RoverMecanumSetpoint.msg RoverMecanumStatus.msg Rpm.msg - RpmControlStatus.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/RpmControlStatus.msg b/msg/RpmControlStatus.msg deleted file mode 100644 index c1ed3dd997..0000000000 --- a/msg/RpmControlStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 rpm_raw # measured rpm -float32 rpm_estimate # filtered rpm -float32 rpm_setpoint # desired rpm - -float32 output diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 9bdbf562b2..9af733995d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -50,7 +50,6 @@ #include #include #include -#include using namespace time_literals; @@ -77,7 +76,6 @@ public: if (_rpm_sub.copy(&rpm)) { _rpm_estimate = rpm.rpm_estimate; - _rpm_raw = rpm.rpm_raw; _timestamp_last_rpm_measurement = rpm.timestamp; } } @@ -102,24 +100,14 @@ public: float output = _pid.update(_rpm_estimate, dt, true); - rpm_control_status_s rpm_control_status{}; - rpm_control_status.rpm_raw = _rpm_raw; - rpm_control_status.rpm_estimate = _rpm_estimate;; - rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get(); - rpm_control_status.output = output; - rpm_control_status.timestamp = hrt_absolute_time(); - _rpm_control_status_pub.publish(rpm_control_status); - return output; } private: static constexpr float RPM_MAX_VALUE = 1800.f; uORB::Subscription _rpm_sub{ORB_ID(rpm)}; - uORB::Publication _rpm_control_status_pub{ORB_ID(rpm_control_status)}; float _rpm_estimate{0.f}; - float _rpm_raw{0.f}; float _spoolup_progress{0.f}; PID _pid; hrt_abstime _timestamp_last_rpm_measurement{0}; From 2772ae7e0e38da0b55c5d2c8069a471a72b73c0a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:17:12 +0100 Subject: [PATCH 14/24] RpmControl: maximum rpm outliers are now caught by RpmCapture --- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 9af733995d..396183382f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -85,9 +85,8 @@ public: _timestamp_last_update = now; const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE; - if (rpm_measurement_timeout && no_excessive_rpm) { + if (rpm_measurement_timeout) { const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); @@ -104,7 +103,6 @@ public: } private: - static constexpr float RPM_MAX_VALUE = 1800.f; uORB::Subscription _rpm_sub{ORB_ID(rpm)}; float _rpm_estimate{0.f}; From 1c4325db6d299eb11454bcbbfdfa043ff4d188b8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 16:30:43 +0100 Subject: [PATCH 15/24] RpmControl: split into cpp source file fixing includes --- .../ActuatorEffectiveness/CMakeLists.txt | 1 + .../ActuatorEffectiveness/RpmControl.cpp | 83 +++++++++++++++++++ .../ActuatorEffectiveness/RpmControl.hpp | 52 +----------- 3 files changed, 88 insertions(+), 48 deletions(-) create mode 100644 src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index a203ac9821..10ff984b29 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessTailsitterVTOL.hpp ActuatorEffectivenessRoverAckermann.hpp ActuatorEffectivenessRoverAckermann.cpp + RpmControl.cpp RpmControl.hpp ) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 0000000000..56bccfe779 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "RpmControl.hpp" + +#include + +using namespace time_literals; + +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + +void RpmControl::setSpoolupProgress(float spoolup_progress) +{ + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < .8f) { + _pid.resetIntegral(); + } +} + +float RpmControl::getActuatorCorrection() +{ + if (_rpm_sub.updated()) { + rpm_s rpm{}; + + if (_rpm_sub.copy(&rpm)) { + _rpm_estimate = rpm.rpm_estimate; + _timestamp_last_rpm_measurement = rpm.timestamp; + } + } + + hrt_abstime now = hrt_absolute_time(); + const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); + _timestamp_last_update = now; + + const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; + + if (rpm_measurement_timeout) { + const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + + } else { + _pid.setGains(0.f, 0.f, 0.f); + } + + _pid.setOutputLimit(.5f); + _pid.setIntegralLimit(.5f); + + float output = _pid.update(_rpm_estimate, dt, true); + + return output; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index 396183382f..b5930902db 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -43,64 +43,20 @@ #pragma once -#include #include - +#include #include #include -#include #include -using namespace time_literals; - class RpmControl : public ModuleParams { public: - RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; + RpmControl(ModuleParams *parent); ~RpmControl() = default; - void setSpoolupProgress(float spoolup_progress) - { - _spoolup_progress = spoolup_progress; - _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); - - if (_spoolup_progress < .8f) { - _pid.resetIntegral(); - } - } - - float getActuatorCorrection() - { - if (_rpm_sub.updated()) { - rpm_s rpm{}; - - if (_rpm_sub.copy(&rpm)) { - _rpm_estimate = rpm.rpm_estimate; - _timestamp_last_rpm_measurement = rpm.timestamp; - } - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = now; - - const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - - if (rpm_measurement_timeout) { - const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; - _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); - - } else { - _pid.setGains(0.f, 0.f, 0.f); - } - - _pid.setOutputLimit(.5f); - _pid.setIntegralLimit(.5f); - - float output = _pid.update(_rpm_estimate, dt, true); - - return output; - } + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); private: uORB::Subscription _rpm_sub{ORB_ID(rpm)}; From 2506bd3b5dea49042fafe9aca4f16d23437a02c0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 Dec 2024 18:39:15 +0100 Subject: [PATCH 16/24] RpmControl: simplify the entire control logic --- .../ActuatorEffectiveness/RpmControl.cpp | 42 +++++++++---------- .../ActuatorEffectiveness/RpmControl.hpp | 11 ++--- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index 56bccfe779..f61a392ad4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -37,47 +37,45 @@ using namespace time_literals; -RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {}; +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pid.setOutputLimit(PID_OUTPUT_LIMIT); + _pid.setIntegralLimit(PID_OUTPUT_LIMIT); +}; void RpmControl::setSpoolupProgress(float spoolup_progress) { _spoolup_progress = spoolup_progress; _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); - if (_spoolup_progress < .8f) { + if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) { _pid.resetIntegral(); } } float RpmControl::getActuatorCorrection() { + hrt_abstime now = hrt_absolute_time(); + + // RPM measurement update if (_rpm_sub.updated()) { rpm_s rpm{}; if (_rpm_sub.copy(&rpm)) { - _rpm_estimate = rpm.rpm_estimate; - _timestamp_last_rpm_measurement = rpm.timestamp; + const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f); + _timestamp_last_measurement = rpm.timestamp; + + const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); } } - hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f); - _timestamp_last_update = now; - - const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s; - - if (rpm_measurement_timeout) { - const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f; - _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); - - } else { - _pid.setGains(0.f, 0.f, 0.f); + // Timeout + if (now > _timestamp_last_measurement + 1_s) { + _pid.resetIntegral(); + _actuator_correction = 0.f; } - _pid.setOutputLimit(.5f); - _pid.setIntegralLimit(.5f); - - float output = _pid.update(_rpm_estimate, dt, true); - - return output; + return _actuator_correction; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b5930902db..b412445048 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -59,13 +59,14 @@ public: float getActuatorCorrection(); private: - uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] - float _rpm_estimate{0.f}; - float _spoolup_progress{0.f}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; PID _pid; - hrt_abstime _timestamp_last_rpm_measurement{0}; - hrt_abstime _timestamp_last_update{0}; + float _spoolup_progress{0.f}; // [0,1] + hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout + float _actuator_correction{0.f}; DEFINE_PARAMETERS( (ParamFloat) _param_ca_heli_rpm_sp, From 5e2848312d2a074bb6936be97b72712a237f58af Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 19:28:20 +0100 Subject: [PATCH 17/24] Commander: start timer for auto disarm after spoolup --- src/modules/commander/Commander.cpp | 3 ++- src/modules/commander/Commander.hpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5fc8ee4d8f..fef03b92c2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, + (_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index cf69743af6..04c1a0d05a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -330,7 +330,6 @@ private: param_t _param_rc_map_fltmode{PARAM_INVALID}; DEFINE_PARAMETERS( - (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, @@ -347,6 +346,7 @@ private: (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, + (ParamFloat) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max From b584f8381c5ce7b004ba442c454d92339565214d Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 20:54:20 +0100 Subject: [PATCH 18/24] Helicopter defaults: don't auto disarm so quickly after spoolup --- ROMFS/px4fmu_common/init.d/rc.heli_defaults | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca268..93e6f0d33f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -12,6 +12,7 @@ param set-default MAV_TYPE 4 param set-default COM_PREARM_MODE 2 param set-default COM_SPOOLUP_TIME 10 +param set-default COM_DISARM_PRFLT 60 # No need for minimum collective pitch (or airmode) to keep torque authority param set-default MPC_MANTHR_MIN 0 From bc92008885f82966578ec2484af6e0e8eae0d756 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 Dec 2024 21:02:51 +0100 Subject: [PATCH 19/24] RpmControl: Better consider the case where there's no rpm measurement (anymore) --- msg/Rpm.msg | 1 + .../control_allocator/ActuatorEffectiveness/RpmControl.cpp | 6 +++++- .../control_allocator/ActuatorEffectiveness/RpmControl.hpp | 1 + 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index ca69e50fdb..b4de2cf422 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) +# rpm values of 0.0 mean within a timeout there is no movement measured float32 rpm_estimate # filtered revolutions per minute float32 rpm_raw diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp index f61a392ad4..53d9766e2d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp @@ -68,11 +68,15 @@ float RpmControl::getActuatorCorrection() const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; } } // Timeout - if (now > _timestamp_last_measurement + 1_s) { + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { _pid.resetIntegral(); _actuator_correction = 0.f; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp index b412445048..5fd0c96d91 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp @@ -63,6 +63,7 @@ private: static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; PID _pid; float _spoolup_progress{0.f}; // [0,1] hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout From 0723f75993d6dc37d4b4a7aa2c87f4da8679a72b Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Thu, 19 Dec 2024 08:06:58 -0800 Subject: [PATCH 20/24] ci: move to px4 git tag versions --- .github/workflows/dev_container.yml | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index bcfbdc2c6b..f64d29b684 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -17,7 +17,7 @@ on: jobs: build: name: Build and Push Container - runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false] + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] steps: - uses: actions/checkout@v4 with: @@ -52,12 +52,7 @@ jobs: ghcr.io/PX4/px4-dev ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }} tags: | - type=semver,pattern={{version}} - type=semver,pattern={{major}}.{{minor}} - type=semver,pattern={{major}} - type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700 - type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600 - type=ref,event=branch,suffix=,priority=500 + type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000 - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 From 12a9087e92cea4dd9e8741f8d7b237fa92a1fea5 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 14 Jan 2025 12:33:32 +0100 Subject: [PATCH 21/24] ekf2: constrain max variance by zero innovation update Clipping the variance of the covariance matrix has a destabilizing effect as it increases the correlation between the states. --- src/modules/ekf2/EKF/covariance.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 0c9a5df6cb..e7b2eea396 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -280,7 +280,17 @@ void Ekf::constrainStateVariances() void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), min, max); + if (P(i, i) < min) { + P(i, i) = min; + + } else if (P(i, i) > max) { + // Constrain the variance growth by fusing zero innovation as clipping the variance + // would artifically increase the correlation between states and destabilize the filter. + const float innov = 0.f; + const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) + const float innov_var = P(i, i) + R; + fuseDirectStateMeasurement(innov, innov_var, R, i); + } } } @@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float limited_max = math::constrain(state_var_max, min, max); float limited_min = math::constrain(limited_max / max_ratio, min, max); - for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), limited_min, limited_max); - } + constrainStateVar(state, limited_min, limited_max); } void Ekf::resetQuatCov(const float yaw_noise) From 9f8325e8e06c51192251686cf87a42645715c4be Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 15 Jan 2025 00:39:11 +0000 Subject: [PATCH 22/24] Update submodule mavlink to latest Wed Jan 15 00:39:11 UTC 2025 - mavlink in PX4/Firmware (fd5b52d4c53f35a520646a6c4ec75588f6b87e0f): https://github.com/mavlink/mavlink/commit/5e3a42b8f3f53038f2779f9f69bd64767b913bb8 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/619947d8bc29e80eecff18b0f4fecc42d9e171dd - Changes: https://github.com/mavlink/mavlink/compare/5e3a42b8f3f53038f2779f9f69bd64767b913bb8...619947d8bc29e80eecff18b0f4fecc42d9e171dd 619947d8 2024-12-19 Hamish Willee - common.xml - PING fix (#2197) 2f44ceff 2024-12-18 Julian Oes - common: use camera ID for CAMERA_IMAGE_CAPTURED (#2196) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 5e3a42b8f3..619947d8bc 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 5e3a42b8f3f53038f2779f9f69bd64767b913bb8 +Subproject commit 619947d8bc29e80eecff18b0f4fecc42d9e171dd From 3064a4ad4c8d14a5debe7ed564ddd36a22a72701 Mon Sep 17 00:00:00 2001 From: Minderring <1701213518@sz.edu.pku.cn> Date: Tue, 14 Jan 2025 21:44:01 +0800 Subject: [PATCH 23/24] boards configs: add airspeed driver for micoair743 aio and v2 --- boards/micoair/h743-aio/default.px4board | 6 +++++- boards/micoair/h743-v2/default.px4board | 6 +++++- boards/micoair/h743/default.px4board | 6 +++++- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index c912a7a6cb..2d92e89d96 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index 330e0fa594..fed1a0756b 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -11,11 +11,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index ef7458f7c3..149bcaff21 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y +CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_BOSCH_BMI270=y -CONFIG_COMMON_INS=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PPS_CAPTURE=y CONFIG_DRIVERS_PWM_OUT=y From 974446c0e8f8e06255f7c1a5a8a846f781a94357 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Wed, 15 Jan 2025 10:12:29 +0100 Subject: [PATCH 24/24] Make control allocation and actuator effectiveness a non-module-specific library (#24196) * Remove more circular dependencies with ActuatorEffectiveness * Separate vehicle specific actuator effectiveness Keep actuator effectivenss in control allocator * Remove test dependency for now * Group library directories Fix * Change directory names * Rebase fix --- src/lib/CMakeLists.txt | 1 + src/lib/control_allocation/CMakeLists.txt | 35 +++++++++++++++ .../ActuatorEffectiveness.cpp | 0 .../ActuatorEffectiveness.hpp | 0 .../actuator_effectiveness}/CMakeLists.txt | 33 -------------- .../control_allocation}/CMakeLists.txt | 2 +- .../control_allocation}/ControlAllocation.cpp | 0 .../control_allocation}/ControlAllocation.hpp | 2 +- .../ControlAllocationPseudoInverse.cpp | 0 .../ControlAllocationPseudoInverse.hpp | 0 .../ControlAllocationPseudoInverseTest.cpp | 0 ...ontrolAllocationSequentialDesaturation.cpp | 0 ...ontrolAllocationSequentialDesaturation.hpp | 0 ...olAllocationSequentialDesaturationTest.cpp | 2 +- src/modules/control_allocator/CMakeLists.txt | 4 +- .../ActuatorEffectivenessControlSurfaces.cpp | 0 .../ActuatorEffectivenessControlSurfaces.hpp | 2 +- .../ActuatorEffectivenessCustom.cpp | 0 .../ActuatorEffectivenessCustom.hpp | 2 +- .../ActuatorEffectivenessFixedWing.cpp | 0 .../ActuatorEffectivenessFixedWing.hpp | 2 +- .../ActuatorEffectivenessHelicopter.cpp | 0 .../ActuatorEffectivenessHelicopter.hpp | 2 +- ...ActuatorEffectivenessHelicopterCoaxial.cpp | 0 ...ActuatorEffectivenessHelicopterCoaxial.hpp | 2 +- .../ActuatorEffectivenessHelicopterTest.cpp | 0 .../ActuatorEffectivenessMCTilt.cpp | 0 .../ActuatorEffectivenessMCTilt.hpp | 2 +- .../ActuatorEffectivenessMultirotor.cpp | 0 .../ActuatorEffectivenessMultirotor.hpp | 2 +- .../ActuatorEffectivenessRotors.cpp | 0 .../ActuatorEffectivenessRotors.hpp | 2 +- .../ActuatorEffectivenessRotorsTest.cpp | 0 .../ActuatorEffectivenessRoverAckermann.cpp | 0 .../ActuatorEffectivenessRoverAckermann.hpp | 2 +- .../ActuatorEffectivenessStandardVTOL.cpp | 0 .../ActuatorEffectivenessStandardVTOL.hpp | 2 +- .../ActuatorEffectivenessTailsitterVTOL.cpp | 0 .../ActuatorEffectivenessTailsitterVTOL.hpp | 2 +- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 0 .../ActuatorEffectivenessTiltrotorVTOL.hpp | 2 +- .../ActuatorEffectivenessTilts.cpp | 0 .../ActuatorEffectivenessTilts.hpp | 2 +- .../ActuatorEffectivenessUUV.cpp | 0 .../ActuatorEffectivenessUUV.hpp | 2 +- .../CMakeLists.txt | 43 +++++++++++++++++++ .../RpmControl.cpp | 0 .../RpmControl.hpp | 0 48 files changed, 98 insertions(+), 52 deletions(-) create mode 100644 src/lib/control_allocation/CMakeLists.txt rename src/{modules/control_allocator/ActuatorEffectiveness => lib/control_allocation/actuator_effectiveness}/ActuatorEffectiveness.cpp (100%) rename src/{modules/control_allocator/ActuatorEffectiveness => lib/control_allocation/actuator_effectiveness}/ActuatorEffectiveness.hpp (100%) rename src/{modules/control_allocator/ActuatorEffectiveness => lib/control_allocation/actuator_effectiveness}/CMakeLists.txt (61%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/CMakeLists.txt (94%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocation.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocation.hpp (99%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationPseudoInverse.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationPseudoInverse.hpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationPseudoInverseTest.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationSequentialDesaturation.cpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationSequentialDesaturation.hpp (100%) rename src/{modules/control_allocator/ControlAllocation => lib/control_allocation/control_allocation}/ControlAllocationSequentialDesaturationTest.cpp (99%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessControlSurfaces.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessControlSurfaces.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessCustom.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessCustom.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessFixedWing.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessFixedWing.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopter.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopter.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopterCoaxial.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopterCoaxial.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessHelicopterTest.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMCTilt.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMCTilt.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMultirotor.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessMultirotor.hpp (96%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRotors.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRotors.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRotorsTest.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRoverAckermann.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessRoverAckermann.hpp (96%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessStandardVTOL.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessStandardVTOL.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTailsitterVTOL.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTailsitterVTOL.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTiltrotorVTOL.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTiltrotorVTOL.hpp (98%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTilts.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessTilts.hpp (97%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessUUV.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/ActuatorEffectivenessUUV.hpp (97%) create mode 100644 src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/RpmControl.cpp (100%) rename src/modules/control_allocator/{ActuatorEffectiveness => VehicleActuatorEffectiveness}/RpmControl.hpp (100%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 69ab23aacc..8f58109b89 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL) add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL) add_subdirectory(collision_prevention EXCLUDE_FROM_ALL) add_subdirectory(component_information EXCLUDE_FROM_ALL) +add_subdirectory(control_allocation EXCLUDE_FROM_ALL) add_subdirectory(controllib EXCLUDE_FROM_ALL) add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) diff --git a/src/lib/control_allocation/CMakeLists.txt b/src/lib/control_allocation/CMakeLists.txt new file mode 100644 index 0000000000..f7c710b7f2 --- /dev/null +++ b/src/lib/control_allocation/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(control_allocation) +add_subdirectory(actuator_effectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt similarity index 61% rename from src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt rename to src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt index 10ff984b29..b8a3d0076d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt @@ -34,36 +34,6 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp - ActuatorEffectivenessUUV.cpp - ActuatorEffectivenessUUV.hpp - ActuatorEffectivenessControlSurfaces.cpp - ActuatorEffectivenessControlSurfaces.hpp - ActuatorEffectivenessCustom.cpp - ActuatorEffectivenessCustom.hpp - ActuatorEffectivenessFixedWing.cpp - ActuatorEffectivenessFixedWing.hpp - ActuatorEffectivenessHelicopter.cpp - ActuatorEffectivenessHelicopter.hpp - ActuatorEffectivenessHelicopterCoaxial.cpp - ActuatorEffectivenessHelicopterCoaxial.hpp - ActuatorEffectivenessMCTilt.cpp - ActuatorEffectivenessMCTilt.hpp - ActuatorEffectivenessMultirotor.cpp - ActuatorEffectivenessMultirotor.hpp - ActuatorEffectivenessTilts.cpp - ActuatorEffectivenessTilts.hpp - ActuatorEffectivenessRotors.cpp - ActuatorEffectivenessRotors.hpp - ActuatorEffectivenessStandardVTOL.cpp - ActuatorEffectivenessStandardVTOL.hpp - ActuatorEffectivenessTiltrotorVTOL.cpp - ActuatorEffectivenessTiltrotorVTOL.hpp - ActuatorEffectivenessTailsitterVTOL.cpp - ActuatorEffectivenessTailsitterVTOL.hpp - ActuatorEffectivenessRoverAckermann.hpp - ActuatorEffectivenessRoverAckermann.cpp - RpmControl.cpp - RpmControl.hpp ) target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) @@ -73,6 +43,3 @@ target_link_libraries(ActuatorEffectiveness mathlib PID ) - -px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness) -px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt similarity index 94% rename from src/modules/control_allocator/ControlAllocation/CMakeLists.txt rename to src/lib/control_allocation/control_allocation/CMakeLists.txt index 4da638aac8..d65bd11fa0 100644 --- a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) -px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.hpp index c60784a03c..a6e2e2b254 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -71,7 +71,7 @@ #include -#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ControlAllocation { diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 60392330c9..2e0af6bff4 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,7 +40,7 @@ #include #include -#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp> +#include using namespace matrix; diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 65872dc432..36d5c7e171 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -32,8 +32,7 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(ActuatorEffectiveness) -add_subdirectory(ControlAllocation) +add_subdirectory(VehicleActuatorEffectiveness) px4_add_module( MODULE modules__control_allocator @@ -50,6 +49,7 @@ px4_add_module( DEPENDS mathlib ActuatorEffectiveness + VehicleActuatorEffectiveness ControlAllocation px4_work_queue SlewRate diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index a0b8b06c16..7047363cc8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index 0cc1390bd2..b7906669f2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 5b4b7785b2..f0b095709e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 3c3193906b..565c8931c9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index d5316bf498..a507aee2dd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 848c8b8853..0b12482781 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index e0a355edd2..e5b7f3dcf8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 3844df4c84..c6f0425569 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 294e453ec6..f6108b4baf 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 0e5d1bfa44..9f945a6cd8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 604f05f9e0..708f104faa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -39,7 +39,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 310d937064..512bd41f41 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index d885a09155..1255d1b282 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 57e86f9436..1b89da8d9b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 0000000000..349893a3ea --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,43 @@ +px4_add_library(VehicleActuatorEffectiveness + ActuatorEffectivenessUUV.cpp + ActuatorEffectivenessUUV.hpp + ActuatorEffectivenessControlSurfaces.cpp + ActuatorEffectivenessControlSurfaces.hpp + ActuatorEffectivenessCustom.cpp + ActuatorEffectivenessCustom.hpp + ActuatorEffectivenessFixedWing.cpp + ActuatorEffectivenessFixedWing.hpp + ActuatorEffectivenessHelicopter.cpp + ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp + ActuatorEffectivenessMCTilt.cpp + ActuatorEffectivenessMCTilt.hpp + ActuatorEffectivenessMultirotor.cpp + ActuatorEffectivenessMultirotor.hpp + ActuatorEffectivenessTilts.cpp + ActuatorEffectivenessTilts.hpp + ActuatorEffectivenessRotors.cpp + ActuatorEffectivenessRotors.hpp + ActuatorEffectivenessStandardVTOL.cpp + ActuatorEffectivenessStandardVTOL.hpp + ActuatorEffectivenessTiltrotorVTOL.cpp + ActuatorEffectivenessTiltrotorVTOL.hpp + ActuatorEffectivenessTailsitterVTOL.cpp + ActuatorEffectivenessTailsitterVTOL.hpp + ActuatorEffectivenessRoverAckermann.hpp + ActuatorEffectivenessRoverAckermann.cpp + RpmControl.hpp + RpmControl.cpp +) + +target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(VehicleActuatorEffectiveness + PRIVATE + mathlib + ActuatorEffectiveness +) + +px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS VehicleActuatorEffectiveness) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp