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

commander: rework nav failure check 

Allows to recover from a failed test with a stricter test
This commit is contained in:
bresch 2021-02-09 14:02:10 +01:00 committed by Daniel Agar
parent 80b8e6a48f
commit b1b032d6e1
2 changed files with 26 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -4001,39 +4001,44 @@ void Commander::estimator_check()
}
/* 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,
* but rotary wing vehicles cannot so the position and velocity validity needs to be latched
* to false after failure to prevent flyaway crashes */
* 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) {
if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
// reset flags
_nav_test_failed = false;
_nav_test_passed = false;
} else {
// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
const bool sufficient_speed = (lpos.vx * lpos.vx + lpos.vy * lpos.vy > 25.0f);
if (!_nav_test_passed) {
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.0f) && (estimator_status.pos_test_ratio < 1.0f);
bool innovation_pass = estimator_status.vel_test_ratio < 1.0f && estimator_status.pos_test_ratio < 1.0f;
if (innovation_pass) {
_time_last_innov_pass = hrt_absolute_time();
if (!_nav_test_failed) {
if (!_nav_test_passed) {
// pass if sufficient time or speed
if (sufficient_time || sufficient_speed) {
// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
const bool sufficient_time = (_status.takeoff_time != 0) && (hrt_elapsed_time(&_status.takeoff_time) > 30_s);
const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
&& (sufficient_time || sufficient_speed)) {
_nav_test_passed = true;
_nav_test_failed = false;
}
// record the last time the innovation check passed
if (innovation_pass) {
_time_last_innov_pass = hrt_absolute_time();
}
} else {
_time_last_innov_fail = hrt_absolute_time();
// if the innovation test has failed continuously, declare the nav as failed
if (hrt_elapsed_time(&_time_last_innov_pass) > 1_s) {
if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 1_s) {
// if the innovation test has failed continuously, declare the nav as failed
_nav_test_failed = true;
mavlink_log_emergency(&_mavlink_log_pub, "Critical navigation failure! Check sensor calibration");
mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Recalibrate sensors");
}
}
}

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

@ -319,7 +319,8 @@ private:
hrt_abstime _lvel_probation_time_us = POSVEL_PROBATION_MIN;
/* class variables used to check for navigation failure after takeoff */
hrt_abstime _time_last_innov_pass{0}; /**< last time velocity or position innovations passed */
hrt_abstime _time_last_innov_pass{0}; /**< last time velocity and position innovations passed */
hrt_abstime _time_last_innov_fail{0}; /**< last time velocity and position innovations failed */
bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */
bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */