diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fb5037607a..76e1a3ac4a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -427,14 +427,14 @@ 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) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied because throttle above center"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center"); tune_negative(true); return TRANSITION_DENIED; } if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied because of high throttle"); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle"); tune_negative(true); return TRANSITION_DENIED; } @@ -2230,7 +2230,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.getLastRCTimestamp(); + _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/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 1d62ecdcf3..60b5fa56c4 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -44,7 +44,7 @@ enum OverrideBits { bool ManualControl::update() { - bool ret = false; + bool updated = false; if (_manual_control_setpoint_sub.updated()) { manual_control_setpoint_s manual_control_setpoint; @@ -53,17 +53,17 @@ bool ManualControl::update() process(manual_control_setpoint); } - ret = true; + updated = true; } _rc_available = _rc_allowed && _manual_control_setpoint.timestamp != 0 && (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s)); - return ret && _rc_available; + return updated && _rc_available; } -void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint) +void ManualControl::process(const manual_control_setpoint_s &manual_control_setpoint) { _last_manual_control_setpoint = _manual_control_setpoint; _manual_control_setpoint = manual_control_setpoint; @@ -83,6 +83,7 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_ 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); @@ -101,11 +102,8 @@ bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mo { bool ret = false; - // no switch or button is mapped const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE; - // arm switch mapped and "switch is button" configured const bool use_button = !use_stick && _param_com_arm_swisbtn.get(); - // arm switch mapped and "switch_is_button" configured const bool use_switch = !use_stick && !use_button; const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); @@ -143,15 +141,12 @@ bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mo } bool ManualControl::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) + const manual_control_switches_s &manual_control_switches, const bool landed) { bool ret = false; - // no switch or button is mapped const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE; - // arm switch mapped and "switch is button" configured const bool use_button = !use_stick && _param_com_arm_swisbtn.get(); - // arm switch mapped and "switch_is_button" configured const bool use_switch = !use_stick && !use_button; const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index 80f49c09d1..ef87e8a22d 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -63,19 +63,19 @@ public: * @return true if there was new data */ bool update(); - bool isRCAvailable() { return _rc_available; } + bool isRCAvailable() const { return _rc_available; } bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode); bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status, 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); - 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; } + const manual_control_switches_s &manual_control_switches, const bool landed); + bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; } + bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; } + hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; } private: void updateParams() override; - void process(manual_control_setpoint_s &manual_control_setpoint); + void process(const 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{};