fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: extend reliant on opt flow to ALTCTL (degraded from POSCTL)

Browse Source 
If you're flying in manual position control mode and lose position the state machine will put you in altitude control mode. Extending the reliant on optical flow relaxed position validity thresholds allows you to potential get back into position control mode with flow alone.
This commit is contained in:
Daniel Agar 2021-03-05 10:07:36 -05:00 committed by GitHub
parent d0c9a5fc93
commit 077afdf9aa
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3899,8 +3899,10 @@ void Commander::UpdateEstimateValidity()
float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
// relax local position eph threshold in operator controlled position mode
// TODO: update to vehicle_control_mode (when available) - flag_control_manual_enabled && flag_control_position_enabled
if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) {
if (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
((_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
|| (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
// Set the allowable position uncertainty based on combination of flight and estimator state
// When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
const bool reliant_on_opt_flow = ((status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))