fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 15:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: report GNSS yaw fault to user

Browse Source
This commit is contained in:
bresch
2021-08-02 16:29:54 +02:00
committed by Mathieu Bresciani
parent 44219e9f45
commit 01d0b8800e
9 changed files with 20 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+8
View File
@@ -3786,6 +3786,8 @@ void Commander::estimator_check()
}
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));
// use primary estimator_status
if (_estimator_selector_status_sub.updated()) {
@@ -3803,12 +3805,18 @@ void Commander::estimator_check()
// 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) {
mavlink_log_critical(&_mavlink_log_pub, "Stopping compass use! Check calibration on landing");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
}
if (!gnss_heading_fault_prev && gnss_heading_fault) {
mavlink_log_critical(&_mavlink_log_pub, "Stopping GNSS heading use! Check configuration on landing");
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