fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 10:37:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Replace manual_control_setpoint use

Browse Source
This commit is contained in:
Matthias Grob
2021-02-17 16:26:32 +01:00
parent 49c240f49e
commit c16b48fd2c
4 changed files with 11 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+5 -9
View File
@@ -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;
}
@@ -2089,7 +2086,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)
@@ -2235,7 +2231,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;
}
src/modules/commander/Commander.hpp
-1
View File
@@ -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};
src/modules/commander/ManualControl.cpp
+2 -2
View File
@@ -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()
src/modules/commander/ManualControl.hpp
+4 -2
View File
@@ -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