From fdc4766da628a89d5e837928a9aed8807b826020 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 4 Jun 2025 20:18:47 +0200 Subject: [PATCH] Make sure vehicle_thrust_setpoint is always published before vehicle_torque_setpoint After f0b05ea7cfd9de4113f4cd63e8068cf8089bc158 the control allocator only has a callback on the torque setpoint and even though this should work I'm paranoid and would like to avoid surprises by always publishing the thrust before torque then the samples that were published together are also allocated together. --- .../airship_att_control/airship_att_control_main.cpp | 2 +- src/modules/fw_rate_control/FixedwingRateControl.cpp | 2 +- src/modules/fw_rate_control/FixedwingRateControl.hpp | 2 +- src/modules/mc_rate_control/MulticopterRateControl.cpp | 2 +- src/modules/mc_rate_control/MulticopterRateControl.hpp | 2 +- src/modules/vtol_att_control/vtol_att_control_main.cpp | 6 +++--- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/airship_att_control/airship_att_control_main.cpp b/src/modules/airship_att_control/airship_att_control_main.cpp index 2a18b349da..27f12b731f 100644 --- a/src/modules/airship_att_control/airship_att_control_main.cpp +++ b/src/modules/airship_att_control/airship_att_control_main.cpp @@ -126,8 +126,8 @@ AirshipAttitudeControl::Run() if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { /* run the rate controller immediately after a gyro update */ - publishTorqueSetpoint(angular_velocity.timestamp_sample); publishThrustSetpoint(angular_velocity.timestamp_sample); + publishTorqueSetpoint(angular_velocity.timestamp_sample); /* check for updates in manual control topic */ _manual_control_setpoint_sub.update(&_manual_control_setpoint); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 32ac785cda..d7de4e4741 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -44,8 +44,8 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), _actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)), - _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_fw) : ORB_ID(vehicle_torque_setpoint)), _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_fw) : ORB_ID(vehicle_torque_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _handle_param_vt_fw_difthr_en = param_find("VT_FW_DIFTHR_EN"); diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index ec3b7e1c9e..2d87e86d8a 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -116,8 +116,8 @@ private: uORB::Publication _actuator_controls_status_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; - uORB::Publication _vehicle_torque_setpoint_pub; uORB::Publication _vehicle_thrust_setpoint_pub; + uORB::Publication _vehicle_torque_setpoint_pub; uORB::Publication _flaps_setpoint_pub{ORB_ID(flaps_setpoint)}; uORB::Publication _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)}; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 3931125289..a7a8ed4212 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -46,8 +46,8 @@ using math::radians; MulticopterRateControl::MulticopterRateControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_mc) : ORB_ID(vehicle_thrust_setpoint)), + _vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_mc) : ORB_ID(vehicle_torque_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 1a2427da18..76e488305a 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -107,8 +107,8 @@ private: uORB::Publication _actuator_controls_status_pub{ORB_ID(actuator_controls_status_0)}; uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; uORB::Publication _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _vehicle_torque_setpoint_pub; uORB::Publication _vehicle_thrust_setpoint_pub; + uORB::Publication _vehicle_torque_setpoint_pub; vehicle_control_mode_s _vehicle_control_mode{}; vehicle_status_s _vehicle_status{}; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 353d236ce6..081c5a8106 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -81,8 +81,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _spoilers_setpoint_pub.advertise(); _vtol_vehicle_status_pub.advertise(); _vehicle_thrust_setpoint0_pub.advertise(); - _vehicle_torque_setpoint0_pub.advertise(); _vehicle_thrust_setpoint1_pub.advertise(); + _vehicle_torque_setpoint0_pub.advertise(); _vehicle_torque_setpoint1_pub.advertise(); } @@ -449,10 +449,10 @@ VtolAttitudeControl::Run() _vtol_type->fill_actuator_outputs(); - _vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0); - _vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1); _vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0); _vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1); + _vehicle_torque_setpoint0_pub.publish(_torque_setpoint_0); + _vehicle_torque_setpoint1_pub.publish(_torque_setpoint_1); // Advertise/publish vtol vehicle status -- immediately if changed, otherwise at 1 Hz const bool vtol_vehicle_status_changed =