yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander: enable user override when GPS is lost

Browse Source 
When user override using the RC sticks is allowed, I would expect this
feature to also work in the case where GPS is lost and the vehicle goes
into a blind land/descent.

Without this commit, the vehicle would switch to Land mode and a pilot
could not take over control unless they switch to Altitude control in
a ground station.

With this commit, user override works as I would expect it and it will
switch to Altitude control allowing a pilot to recover in this
situation.
This commit is contained in:
Julian Oes
2021-06-15 09:35:46 +02:00
committed by Lorenz Meier
parent 749f213ca5
commit 8415692f2a
3 changed files with 16 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/commander/Commander.cpp
View File

@@ -2279,11 +2279,17 @@ Commander::run()
// abort autonomous mode and switch to position mode if sticks are moved significantly // abort autonomous mode and switch to position mode if sticks are moved significantly
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe && !_geofence_warning_action_on && !in_low_battery_failsafe && !_geofence_warning_action_on
&& _manual_control.wantsOverride(_vehicle_control_mode)) { && _manual_control.wantsOverride(_vehicle_control_mode, _status)) {
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
_internal_state) == TRANSITION_CHANGED) { _internal_state) == TRANSITION_CHANGED) {
tune_positive(true); tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks");
_status_changed = true;
} else if (main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags,
_internal_state) == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks");
_status_changed = true; _status_changed = true;
} }
} }

9
src/modules/commander/ManualControl.cpp
View File

@@ -60,7 +60,8 @@ bool ManualControl::update()
return updated && _rc_available; return updated && _rc_available;
} }
bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode) 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() & OverrideBits::OVERRIDE_AUTO_MODE_BIT)
&& vehicle_control_mode.flag_control_auto_enabled; && vehicle_control_mode.flag_control_auto_enabled;
@@ -68,7 +69,11 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT) const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
&& vehicle_control_mode.flag_control_offboard_enabled; && vehicle_control_mode.flag_control_offboard_enabled;
if (_rc_available && (override_auto_mode || override_offboard_mode)) { const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {
const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get(); 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) const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change)

2
src/modules/commander/ManualControl.hpp
View File

@@ -65,7 +65,7 @@ public:
bool update(); bool update();
bool isRCAvailable() const { return _rc_available; } bool isRCAvailable() const { return _rc_available; }
bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; } bool isMavlink() const { return _manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; }
bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode); bool wantsOverride(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, 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,