Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

commander: report estimator mag fault detection to ground station

Browse Source
This commit is contained in:
bresch
2021-03-19 14:33:41 +01:00
committed by Daniel Agar
parent 2c5342acd4
commit ace6f81c93
2 changed files with 19 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
+18 -1
View File
@@ -39,6 +39,7 @@
#include <lib/sensor_calibration/Utilities.hpp> #include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h> #include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_mag.h> #include <uORB/topics/sensor_mag.h>
using namespace time_literals; using namespace time_literals;
@@ -49,6 +50,7 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
bool calibration_valid = false; bool calibration_valid = false;
bool valid = false; bool valid = false;
bool is_mag_fault = false;
if (exists) { if (exists) {
@@ -72,13 +74,28 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
} }
} }
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (estimator_status_sub.get().mag_device_id == static_cast<uint32_t>(device_id)) {
if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) {
is_mag_fault = true;
break;
}
}
}
if (is_mag_fault && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
}
} else { } else {
if (!optional && report_fail) { if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance); mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
} }
} }
const bool success = calibration_valid && valid; const bool success = calibration_valid && valid && !is_mag_fault;
if (instance == 0) { if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
src/modules/commander/Commander.cpp
+1
View File
@@ -3809,6 +3809,7 @@ void Commander::estimator_check()
if (!mag_fault_prev && mag_fault) { if (!mag_fault_prev && mag_fault) {
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_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _status);
} }
/* 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