From 514845592b12e7db82ffedafbbbf8484769cc553 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 9 Jun 2021 16:03:52 +0200 Subject: [PATCH] en-/disable mc position controller using explicit control mode flag --- msg/vehicle_control_mode.msg | 1 + src/modules/commander/Commander.cpp | 8 ++++++++ src/modules/mc_pos_control/MulticopterPositionControl.cpp | 2 +- 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 9592a694ca..85ee40229d 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -2,6 +2,7 @@ uint64 timestamp # time since system start (microseconds) bool flag_armed # synonym for actuator_armed.armed bool flag_external_manual_override_ok # external override non-fatal for system. Only true for fixed wing +bool flag_multicopter_position_control_enabled bool flag_control_manual_enabled # true if manual input is mixed in bool flag_control_auto_enabled # true if onboard autopilot should act diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f0e040f749..2f8189d722 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3328,6 +3328,14 @@ Commander::update_control_mode() break; } + _vehicle_control_mode.flag_multicopter_position_control_enabled = + (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_vehicle_control_mode.flag_control_altitude_enabled + || _vehicle_control_mode.flag_control_climb_rate_enabled + || _vehicle_control_mode.flag_control_position_enabled + || _vehicle_control_mode.flag_control_velocity_enabled + || _vehicle_control_mode.flag_control_acceleration_enabled); + _vehicle_control_mode.timestamp = hrt_absolute_time(); _control_mode_pub.publish(_vehicle_control_mode); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index e55476a707..453c37b437 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -289,7 +289,7 @@ void MulticopterPositionControl::Run() PositionControlStates states{set_vehicle_states(local_pos)}; - if (_control_mode.flag_control_acceleration_enabled || _control_mode.flag_control_climb_rate_enabled) { + if (_control_mode.flag_multicopter_position_control_enabled) { _trajectory_setpoint_sub.update(&_setpoint);