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

commander: preflightcheck only report failure once per sensor type 

- either way the user action is to recalibrate, so we might as well try
to minimize the noise/annoyance
This commit is contained in:
Daniel Agar 2020-11-16 11:41:37 -05:00
parent edaf9f2bb6
commit 8f5f564c05
1 changed files with 31 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -55,11 +55,11 @@ 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 reportFailures, const bool prearm,
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool report_failures, const bool prearm,
const hrt_abstime &time_since_boot)
{
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
bool failed = false;
@ -75,7 +75,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
const bool report_fail = (reportFailures);
bool report_fail = report_failures;
int32_t device_id = -1;
@ -83,13 +83,15 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (required) {
failed = true;
}
report_fail = false; // only report the first failure
}
}
// TODO: highest priority mag
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) {
if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
}
@ -100,7 +102,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
const bool required = (i < max_mandatory_accel_count);
const bool report_fail = (reportFailures);
bool report_fail = report_failures;
int32_t device_id = -1;
@ -108,6 +110,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (required) {
failed = true;
}
report_fail = false; // only report the first failure
}
}
@ -119,12 +123,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
const bool required = (i < max_mandatory_gyro_count);
bool report_fail = report_failures;
int32_t device_id = -1;
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
report_fail = false; // only report the first failure
}
}
@ -141,7 +149,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1);
const bool report_fail = (reportFailures && !baro_fail_reported);
bool report_fail = (report_failures && !baro_fail_reported);
int32_t device_id = -1;
@ -149,6 +157,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (required) {
baro_fail_reported = true;
}
report_fail = false; // only report the first failure
}
}
}
@ -156,7 +166,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- IMU CONSISTENCY ---- */
// To be performed after the individual sensor checks have completed
{
if (!imuConsistencyCheck(mavlink_log_pub, status, reportFailures)) {
if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
}
@ -166,18 +176,19 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (!status_flags.circuit_breaker_engaged_airspd_check &&
(status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) {
int32_t optional = 0;
param_get(param_find("FW_ARSP_MODE"), &optional);
int32_t airspeed_mode = 0;
param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
const bool optional = (airspeed_mode == 1);
if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures, prearm) && !(bool)optional) {
if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm) && !optional) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) {
if (rcCalibrationCheck(mavlink_log_pub, reportFailures, status.is_vtol) != OK) {
if (reportFailures) {
if (rcCalibrationCheck(mavlink_log_pub, report_failures, status.is_vtol) != OK) {
if (report_failures) {
mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
}
@ -196,7 +207,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- SYSTEM POWER ---- */
if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
if (!powerCheck(mavlink_log_pub, status, reportFailures, prearm)) {
if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) {
failed = true;
}
}
@ -217,22 +228,22 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
// 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 (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail, checkGNSS)) {
if (!ekf2Check(mavlink_log_pub, status, false, report_failures && report_ekf_fail, checkGNSS)) {
failed = true;
}
if (!ekf2CheckStates(mavlink_log_pub, reportFailures && report_ekf_fail)) {
if (!ekf2CheckStates(mavlink_log_pub, report_failures && report_ekf_fail)) {
failed = true;
}
}
/* ---- Failure Detector ---- */
if (!failureDetectorCheck(mavlink_log_pub, status, reportFailures, prearm)) {
if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) {
failed = true;
}
failed = failed || !manualControlCheck(mavlink_log_pub, reportFailures);
failed = failed || !cpuResourceCheck(mavlink_log_pub, reportFailures);
failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
/* Report status */
return !failed;