diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index e421d1d291..976d1cf60c 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -43,12 +43,6 @@ float32 r # yaw stick/twist position, -1..1 # in general corresponds to the righthand rotation around the vertical # (downwards) axis of the vehicle -# stick velocity -float32 vx -float32 vy -float32 vz -float32 vr - float32 flaps # flap position float32 aux1 diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 40c5743d2c..5278b0bbe6 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -36,10 +36,9 @@ using namespace time_literals; -enum OverrideBits { +enum class OverrideBits : int32_t { OVERRIDE_AUTO_MODE_BIT = (1 << 0), OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), - OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) }; bool ManualControl::update() @@ -63,10 +62,10 @@ bool ManualControl::update() bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status) { - const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT) + const bool override_auto_mode = (_param_rc_override.get() & static_cast(OverrideBits::OVERRIDE_AUTO_MODE_BIT)) && vehicle_control_mode.flag_control_auto_enabled; - const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT) + const bool override_offboard_mode = (_param_rc_override.get() & static_cast(OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)) && vehicle_control_mode.flag_control_offboard_enabled; // in Descend manual override is enbaled independently of COM_RC_OVERRIDE @@ -74,19 +73,8 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_ if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) { - const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get(); - const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change) - || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change) - || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change); - // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] - const bool throttle_moved = - (fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change); - const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); - - if (rpy_moved || (use_throttle && throttle_moved)) { - return true; - } + return _manual_control_setpoint.user_override; } return false; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0df0589228..8c89713d47 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -669,10 +669,9 @@ PARAM_DEFINE_INT32(COM_REARM_GRACE, 1); * override is always enabled. * * @min 0 - * @max 7 + * @max 3 * @bit 0 Enable override during auto modes (except for in critical battery reaction) * @bit 1 Enable override during offboard mode - * @bit 2 Ignore throttle stick * @group Commander */ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1); diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 67730a6a95..d6df252fe7 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -137,20 +137,23 @@ void ManualControl::Run() } // user wants override - //const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); + const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); - // FIXME: we need previous - //const bool rpy_moved = (fabsf(_selector.setpoint().x - _manual_control_input[i].x) > minimum_stick_change) - // || (fabsf(_selector.setpoint().y - _manual_control_input[i].y) > minimum_stick_change) - // || (fabsf(_selector.setpoint().r - _manual_control_input[i].r) > minimum_stick_change); + // TODO: look at least at 3 samples in a specific time + + const bool rpy_moved = (fabsf(_selector.setpoint().x - _previous_x) > minimum_stick_change) + || (fabsf(_selector.setpoint().y - _previous_y) > minimum_stick_change) + || (fabsf(_selector.setpoint().r - _previous_r) > minimum_stick_change); // Throttle change value doubled to achieve the same scaling even though the range is [0,1] instead of [-1,1] - //const bool throttle_moved = (fabsf(_selector.setpoint().z - _manual_control_input[i].z) * 2.f > minimum_stick_change); - //const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); + const bool throttle_moved = (fabsf(_selector.setpoint().z - _previous_z) * 2.f > minimum_stick_change); - //_selector.setpoint().user_override = rpy_moved || (use_throttle && throttle_moved); + _selector.setpoint().user_override = rpy_moved || throttle_moved; - // TODO: at least 3 samples in a time + _previous_x = _selector.setpoint().x; + _previous_y = _selector.setpoint().y; + _previous_z = _selector.setpoint().z; + _previous_r = _selector.setpoint().r; if (switches_updated) { // Only use switches if current source is RC as well. @@ -177,6 +180,11 @@ void ManualControl::Run() _published_invalid_once = true; _manual_control_setpoint_pub.publish(_selector.setpoint()); } + + _previous_x = NAN; + _previous_y = NAN; + _previous_z = NAN; + _previous_r = NAN; } // reschedule timeout diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 8302d6b0dd..e6f4943c31 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -76,13 +76,6 @@ public: private: static constexpr int MAX_MANUAL_INPUT_COUNT = 3; - - enum OverrideBits { - OVERRIDE_AUTO_MODE_BIT = (1 << 0), - OVERRIDE_OFFBOARD_MODE_BIT = (1 << 1), - OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) - }; - void Run() override; void send_arm_command(); @@ -108,13 +101,17 @@ private: bool _previous_arm_gesture{false}; bool _previous_disarm_gesture{false}; + float _previous_x{NAN}; + float _previous_y{NAN}; + float _previous_z{NAN}; + float _previous_r{NAN}; + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; DEFINE_PARAMETERS( (ParamInt) _param_com_rc_in_mode, (ParamFloat) _param_com_rc_loss_t, - (ParamInt) _param_rc_override, (ParamFloat) _param_com_rc_stick_ov, (ParamInt) _param_rc_arm_hyst )