From 1c8f31f339dcb0e8e674ff48d7f735aff49d6b26 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Sat, 14 Oct 2023 16:03:49 +0100 Subject: [PATCH] [offboard] add thrust and torque control mode New thrust and torque control mode added which replaces the previous actuator mode, in this mode the rate controllers are disables, the control allocator is enabled and the used externally provices vehicle_thrust_setpoints and vehicle_torque_setpoints. New direct_actuator mode In direct_actuator mode the control allocator module does not publish actuator_motors and actuator_servos messages which must instead be provided extrernally by the user. Removed old direct mode. Signed-off-by: Beniamino Pozzan --- msg/OffboardControlMode.msg | 3 ++- .../checks/offboardCheck.cpp | 2 +- .../commander/ModeUtil/control_mode.cpp | 24 +++++++++++++++---- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/msg/OffboardControlMode.msg b/msg/OffboardControlMode.msg index 319ba1ac50..885164a652 100644 --- a/msg/OffboardControlMode.msg +++ b/msg/OffboardControlMode.msg @@ -7,4 +7,5 @@ bool velocity bool acceleration bool attitude bool body_rate -bool actuator +bool thrust_and_torque +bool direct_actuator diff --git a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp index c4d2583e7b..c2e2fb1598 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp @@ -48,7 +48,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter) bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate - || offboard_control_mode.actuator) && data_is_recent; + || offboard_control_mode.thrust_and_torque || offboard_control_mode.direct_actuator) && data_is_recent; if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) { offboard_available = false; diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 468fdc9f1b..9d5483c2ea 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -46,19 +46,20 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, const offboard_control_mode_s &offboard_control_mode, vehicle_control_mode_s &vehicle_control_mode) { - vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default switch (nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = stabilization_required(vehicle_type); vehicle_control_mode.flag_control_attitude_enabled = stabilization_required(vehicle_type); + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_STAB: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: @@ -67,6 +68,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_attitude_enabled = true; vehicle_control_mode.flag_control_altitude_enabled = true; vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: @@ -77,6 +79,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_climb_rate_enabled = true; vehicle_control_mode.flag_control_position_enabled = true; vehicle_control_mode.flag_control_velocity_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: @@ -93,11 +96,13 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_climb_rate_enabled = true; vehicle_control_mode.flag_control_position_enabled = true; vehicle_control_mode.flag_control_velocity_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ACRO: vehicle_control_mode.flag_control_manual_enabled = true; vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: @@ -105,6 +110,7 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; vehicle_control_mode.flag_control_climb_rate_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_TERMINATION: @@ -121,28 +127,36 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode.flag_control_altitude_enabled = true; vehicle_control_mode.flag_control_climb_rate_enabled = true; vehicle_control_mode.flag_control_acceleration_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; } else if (offboard_control_mode.velocity) { vehicle_control_mode.flag_control_velocity_enabled = true; vehicle_control_mode.flag_control_altitude_enabled = true; vehicle_control_mode.flag_control_climb_rate_enabled = true; vehicle_control_mode.flag_control_acceleration_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; } else if (offboard_control_mode.acceleration) { vehicle_control_mode.flag_control_acceleration_enabled = true; - vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; } else if (offboard_control_mode.attitude) { - vehicle_control_mode.flag_control_rates_enabled = true; vehicle_control_mode.flag_control_attitude_enabled = true; + vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; } else if (offboard_control_mode.body_rate) { vehicle_control_mode.flag_control_rates_enabled = true; + vehicle_control_mode.flag_control_allocation_enabled = true; + + } else if (offboard_control_mode.thrust_and_torque) { + vehicle_control_mode.flag_control_allocation_enabled = true; } break;