mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-02 03:49:12 +08:00
ManualControl: name, message, comment, const qualifier improvements
addressing review from @bresch, @julianoes and @JonasVautherin
This commit is contained in:
@@ -427,14 +427,14 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
|||||||
if (run_preflight_checks) {
|
if (run_preflight_checks) {
|
||||||
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
||||||
if (_vehicle_control_mode.flag_control_climb_rate_enabled && _manual_control.isThrottleAboveCenter()) {
|
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);
|
tune_negative(true);
|
||||||
return TRANSITION_DENIED;
|
return TRANSITION_DENIED;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_manual_control.isThrottleLow()) {
|
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);
|
tune_negative(true);
|
||||||
return TRANSITION_DENIED;
|
return TRANSITION_DENIED;
|
||||||
}
|
}
|
||||||
@@ -2230,7 +2230,7 @@ Commander::run()
|
|||||||
if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
|
if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
|
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
|
||||||
_status.rc_signal_lost = true;
|
_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);
|
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status);
|
||||||
_status_changed = true;
|
_status_changed = true;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -44,7 +44,7 @@ enum OverrideBits {
|
|||||||
|
|
||||||
bool ManualControl::update()
|
bool ManualControl::update()
|
||||||
{
|
{
|
||||||
bool ret = false;
|
bool updated = false;
|
||||||
|
|
||||||
if (_manual_control_setpoint_sub.updated()) {
|
if (_manual_control_setpoint_sub.updated()) {
|
||||||
manual_control_setpoint_s manual_control_setpoint;
|
manual_control_setpoint_s manual_control_setpoint;
|
||||||
@@ -53,17 +53,17 @@ bool ManualControl::update()
|
|||||||
process(manual_control_setpoint);
|
process(manual_control_setpoint);
|
||||||
}
|
}
|
||||||
|
|
||||||
ret = true;
|
updated = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
_rc_available = _rc_allowed
|
_rc_available = _rc_allowed
|
||||||
&& _manual_control_setpoint.timestamp != 0
|
&& _manual_control_setpoint.timestamp != 0
|
||||||
&& (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s));
|
&& (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;
|
_last_manual_control_setpoint = _manual_control_setpoint;
|
||||||
_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)
|
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.y - _last_manual_control_setpoint.y) > minimum_stick_change)
|
||||||
|| (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > 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 =
|
const bool throttle_moved =
|
||||||
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change);
|
(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);
|
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;
|
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;
|
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();
|
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 use_switch = !use_stick && !use_button;
|
||||||
|
|
||||||
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
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,
|
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;
|
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;
|
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();
|
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 use_switch = !use_stick && !use_button;
|
||||||
|
|
||||||
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||||
|
|||||||
@@ -63,19 +63,19 @@ public:
|
|||||||
* @return true if there was new data
|
* @return true if there was new data
|
||||||
*/
|
*/
|
||||||
bool update();
|
bool update();
|
||||||
bool isRCAvailable() { return _rc_available; }
|
bool isRCAvailable() const { return _rc_available; }
|
||||||
bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode);
|
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,
|
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);
|
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,
|
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);
|
const manual_control_switches_s &manual_control_switches, const bool landed);
|
||||||
bool isThrottleLow() { return _last_manual_control_setpoint.z < 0.1f; }
|
bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; }
|
||||||
bool isThrottleAboveCenter() { return _last_manual_control_setpoint.z > 0.6f; }
|
bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; }
|
||||||
hrt_abstime getLastRCTimestamp() { return _last_manual_control_setpoint.timestamp; }
|
hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void updateParams() override;
|
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)};
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
manual_control_setpoint_s _manual_control_setpoint{};
|
manual_control_setpoint_s _manual_control_setpoint{};
|
||||||
|
|||||||
Reference in New Issue
Block a user