dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: POSCTL optical flow aiding continue updating GPOS validity 

- updating the global position validity should happen regardless of
being in manual position control mode with only optical flow
 - condition_global_position_valid needs to be accurate to adjudicate
main state changes (eg going into AUTO MISSION)
This commit is contained in:
Daniel Agar 2020-12-10 10:23:21 -05:00
parent 6bda8af97c
commit ae2b1a265c
2 changed files with 23 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -3981,23 +3981,6 @@ void Commander::estimator_check()
mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing"); mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing");
} }
// 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 = ((estimator_status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))
&& !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS))
&& !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS)));
const bool operator_controlled_position = (_internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL);
_skip_pos_accuracy_check = reliant_on_opt_flow && operator_controlled_position;
if (_skip_pos_accuracy_check) {
_eph_threshold_adj = INFINITY;
} else {
_eph_threshold_adj = _param_com_pos_fs_eph.get();
}
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure /* Check estimator status for signs of bad yaw induced post takeoff navigation failure
* for a short time interval after takeoff. Fixed wing vehicles can recover using GPS heading, * for a short time interval after takeoff. Fixed wing vehicles can recover using GPS heading,
* but rotary wing vehicles cannot so the position and velocity validity needs to be latched * but rotary wing vehicles cannot so the position and velocity validity needs to be latched
@ -4052,16 +4035,34 @@ void Commander::estimator_check()
void Commander::UpdateEstimateValidity() void Commander::UpdateEstimateValidity()
{ {
const vehicle_local_position_s &lpos = _local_position_sub.get(); const vehicle_local_position_s &lpos = _local_position_sub.get();
if (!_skip_pos_accuracy_check) {
const vehicle_global_position_s &gpos = _global_position_sub.get(); const vehicle_global_position_s &gpos = _global_position_sub.get();
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _eph_threshold_adj, gpos.timestamp, const estimator_status_s &status = _estimator_status_sub.get();
&_last_gpos_fail_time_us, &_gpos_probation_time_us, &_status_flags.condition_global_position_valid);
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) {
// 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))
&& !(status.control_mode_flags & (1 << estimator_status_s::CS_GPS))
&& !(status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS)));
if (reliant_on_opt_flow) {
lpos_eph_threshold_adj = INFINITY;
}
} }
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, _eph_threshold_adj, lpos.timestamp, // condition_global_position_valid
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
&_last_gpos_fail_time_us, &_gpos_probation_time_us, &_status_flags.condition_global_position_valid);
// condition_local_position_valid
check_posvel_validity(lpos.xy_valid && !_nav_test_failed, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
&_last_lpos_fail_time_us, &_lpos_probation_time_us, &_status_flags.condition_local_position_valid); &_last_lpos_fail_time_us, &_lpos_probation_time_us, &_status_flags.condition_local_position_valid);
// condition_local_velocity_valid
check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, check_posvel_validity(lpos.v_xy_valid && !_nav_test_failed, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
&_last_lvel_fail_time_us, &_lvel_probation_time_us, &_status_flags.condition_local_velocity_valid); &_last_lvel_fail_time_us, &_lvel_probation_time_us, &_status_flags.condition_local_velocity_valid);
} }

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

@ -347,8 +347,6 @@ private:
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
float _eph_threshold_adj{INFINITY}; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition
bool _skip_pos_accuracy_check{false};
bool _last_condition_local_altitude_valid{false}; bool _last_condition_local_altitude_valid{false};
bool _last_condition_local_position_valid{false}; bool _last_condition_local_position_valid{false};
bool _last_condition_global_position_valid{false}; bool _last_condition_global_position_valid{false};