fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:33:33 +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/PreFlightCheck.cpp
+12 -7
View File
@@ -55,7 +55,7 @@ static constexpr unsigned max_mandatory_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 4;
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool report_failures, const bool prearm,
vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm,
const hrt_abstime &time_since_boot)
{
report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
@@ -235,16 +235,21 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
if (estimator_type == 2) {
bool ekf_healthy = false;
// don't report ekf failures for the first 10 seconds to allow time for the filter to start
bool report_ekf_fail = (time_since_boot > 10_s);
if (time_since_boot > 10_s) {
if (!ekf2Check(mavlink_log_pub, status, false, report_failures && report_ekf_fail, checkGNSS)) {
failed = true;
ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) &&
ekf2CheckSensorBias(mavlink_log_pub, report_failures);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
} else {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status);
}
if (!ekf2CheckSensorBias(mavlink_log_pub, report_failures && report_ekf_fail)) {
failed = true;
}
failed |= !ekf_healthy;
}
/* ---- Failure Detector ---- */
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+2 -2
View File
@@ -78,7 +78,7 @@ public:
* true if the system power should be checked
**/
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
vehicle_status_flags_s &status_flags, bool reportFailures, const bool prearm,
const hrt_abstime &time_since_boot);
struct arm_requirements_t {
@@ -109,7 +109,7 @@ private:
static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
const bool prearm);
static bool 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);
static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
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;
}