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

ekf2checks: split GPS checks from AHRS checks - minor cleanup

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2021-02-01 09:02:19 +01:00
committed by Beat Küng
parent 78dff6adcb
commit c2154df2f6
5 changed files with 31 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+9 -10
View File
@@ -45,10 +45,9 @@
using namespace time_literals;
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
const bool report_fail, const bool enforce_gps_required)
const bool report_fail)
{
bool success = true; // start with a pass and change to a fail if any test fails
bool ahrs_present = true;
int32_t mag_strength_check_enabled = 1;
param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
@@ -65,6 +64,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
float mag_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
int32_t arm_without_gps = 0;
param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
bool gps_success = true;
bool gps_present = true;
@@ -74,7 +76,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
const estimator_status_s &status = status_sub.get();
if (status.timestamp == 0) {
ahrs_present = false;
success = false;
goto out;
}
@@ -152,7 +154,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
if (enforce_gps_required || report_fail) {
{
const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
@@ -206,7 +208,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
if (message) {
if (enforce_gps_required) {
if (!arm_without_gps) {
mavlink_log_critical(mavlink_log_pub, message, " Fail");
} else {
@@ -217,7 +219,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
gps_success = false;
if (enforce_gps_required) {
if (!arm_without_gps) {
success = false;
goto out;
}
@@ -225,10 +227,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
out:
//PX4_INFO("AHRS CHECK: %s", (success && ahrs_present) ? "OK" : "FAIL");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, ahrs_present, true, success && ahrs_present, vehicle_status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status);
return success;
}