fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 21:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: monitor GPS validity and EKF2 dead reckoning

Browse Source 
- ekf2: expose dead reckoning as control status flags
 - commander:
    - add GPS validity check
    - in AUTO MISSION if dependent on GPS then a loss of GPS will
This commit is contained in:
Daniel Agar
2021-12-03 11:42:05 -05:00
parent 6021b8efb3
commit 1d7791dad6
14 changed files with 121 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+76 -26
View File
@@ -811,6 +811,9 @@ Commander::Commander() :
// default for vtol is rotary wing
_vtol_status.vtol_in_rw_mode = true;
_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
}
bool
@@ -3983,9 +3986,8 @@ void Commander::estimator_check()
_heading_reset_counter = lpos.heading_reset_counter;
}
const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
const bool gnss_heading_fault_prev = (_estimator_status_sub.get().control_mode_flags &
(1 << estimator_status_s::CS_GPS_YAW_FAULT));
const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault;
const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;
// use primary estimator_status
if (_estimator_selector_status_sub.updated()) {
@@ -3994,41 +3996,58 @@ void Commander::estimator_check()
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
_estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
}
}
}
if (_estimator_status_sub.update()) {
const estimator_status_s &estimator_status = _estimator_status_sub.get();
if (_estimator_status_flags_sub.update()) {
const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get();
_status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
|| estimator_status_flags.cs_inertial_dead_reckoning;
if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) {
// position requirements (update if not dead reckoning)
bool gps = estimator_status_flags.cs_gps;
bool optical_flow = estimator_status_flags.cs_opt_flow;
bool vision_position = estimator_status_flags.cs_ev_pos;
_status_flags.position_reliant_on_gps = gps && !optical_flow && !vision_position;
_status_flags.position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
_status_flags.position_reliant_on_vision_position = !gps && !optical_flow && vision_position;
}
// Check for a magnetomer fault and notify the user
const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
const bool gnss_heading_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS_YAW_FAULT));
if (!mag_fault_prev && mag_fault) {
if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) {
mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t");
events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical,
"Stopping compass use! Land now and calibrate the compass");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
}
if (!gnss_heading_fault_prev && gnss_heading_fault) {
if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) {
mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t");
events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical,
"GNSS heading not reliable. Land now!");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _status);
}
}
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
* for a short time interval after takeoff.
* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
* if this does not fix the issue we need to stop using a position controlled
* mode to prevent flyaway crashes.
*/
if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
* for a short time interval after takeoff.
* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
* if this does not fix the issue we need to stop using a position controlled
* mode to prevent flyaway crashes.
*/
if (_estimator_status_sub.updated() && run_quality_checks
&& _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
estimator_status_s estimator_status;
if (_estimator_status_sub.copy(&estimator_status)) {
if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
_nav_test_failed = false;
_nav_test_passed = false;
@@ -4036,9 +4055,10 @@ void Commander::estimator_check()
} else {
if (!_nav_test_passed) {
// Both test ratios need to pass/fail together to change the nav test status
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f)
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
&& (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.0f) && (estimator_status.pos_test_ratio >= 1.0f);
const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
if (innovation_pass) {
_time_last_innov_pass = hrt_absolute_time();
@@ -4122,13 +4142,47 @@ void Commander::estimator_check()
}
_status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid;
// gps
const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid;
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s vehicle_gps_position;
if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s);
bool fix = vehicle_gps_position.fix_type >= 2;
bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time());
_status_flags.condition_gps_position_valid = _vehicle_gps_position_valid.get_state();
_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
}
} else {
const hrt_abstime now_us = hrt_absolute_time();
if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
_vehicle_gps_position_valid.set_state_and_update(false, now_us);
_status_flags.condition_gps_position_valid = false;
}
}
if (condition_gps_position_was_valid && !_status_flags.condition_gps_position_valid) {
PX4_WARN("GPS no longer valid");
}
}
void Commander::UpdateEstimateValidity()
{
const vehicle_local_position_s &lpos = _local_position_sub.get();
const vehicle_global_position_s &gpos = _global_position_sub.get();
const estimator_status_s &status = _estimator_status_sub.get();
float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
@@ -4139,11 +4193,7 @@ void Commander::UpdateEstimateValidity()
// 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) {
if (_status_flags.position_reliant_on_optical_flow) {
lpos_eph_threshold_adj = INFINITY;
}
}