diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 75659a344f..def762663e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -426,18 +426,15 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (run_preflight_checks) { if (_vehicle_control_mode.flag_control_manual_enabled) { - const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f); - const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f); - - if (_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_center) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered"); + if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied because throttle above center"); tune_negative(true); return TRANSITION_DENIED; } - if (!_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_low) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero"); + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied because of high throttle"); tune_negative(true); return TRANSITION_DENIED; } @@ -2099,7 +2096,6 @@ Commander::run() _manual_control.setRCAllowed(!_status_flags.rc_input_blocked); _manual_control.update(); - _manual_control_setpoint = _manual_control._manual_control_setpoint; // abort autonomous mode and switch to position mode if sticks are moved significantly if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) @@ -2245,7 +2241,7 @@ Commander::run() if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) { mavlink_log_critical(&_mavlink_log_pub, "Manual control lost"); _status.rc_signal_lost = true; - _rc_signal_lost_timestamp = _manual_control_setpoint.timestamp; + _rc_signal_lost_timestamp = _manual_control.getLastRCTimestamp(); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status); _status_changed = true; } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e57f05c0e3..557580e335 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -349,7 +349,6 @@ private: unsigned int _leds_counter{0}; - manual_control_setpoint_s _manual_control_setpoint{}; manual_control_switches_s _manual_control_switches{}; manual_control_switches_s _last_manual_control_switches{}; ManualControl _manual_control{this}; diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 787801e12f..ad2ed637be 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -101,7 +101,7 @@ bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mo const bool arm_button_pressed = _param_arm_switch_is_button.get() && (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON); const bool stick_in_lower_left = _manual_control_setpoint.r < -.9f - && (_manual_control_setpoint.z < .1f) + && isThrottleLow() && !arm_switch_or_button_mapped; const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() && (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON) @@ -142,7 +142,7 @@ bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const bool arm_button_pressed = _param_arm_switch_is_button.get() && (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON); const bool stick_in_lower_right = _manual_control_setpoint.r > .9f - && _manual_control_setpoint.z < 0.1f + && isThrottleLow() && !arm_switch_or_button_mapped; const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index 5edec1c3c0..d4f9810c99 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -64,14 +64,16 @@ public: manual_control_switches_s &manual_control_switches, const bool landed); bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, manual_control_switches_s &manual_control_switches, const bool landed); - - manual_control_setpoint_s _manual_control_setpoint{}; + bool isThrottleLow() { return _last_manual_control_setpoint.z < 0.1f; } + bool isThrottleAboveCenter() { return _last_manual_control_setpoint.z > 0.6f; } + hrt_abstime getLastRCTimestamp() { return _last_manual_control_setpoint.timestamp; } private: void updateParams() override; void process(manual_control_setpoint_s &manual_control_setpoint); uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + manual_control_setpoint_s _manual_control_setpoint{}; manual_control_setpoint_s _last_manual_control_setpoint{}; // Availability