fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 17:18:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Streamline preflight and arming checks

Browse Source 
The checks did previously only report the first failure (to not overload the radio link). As we are moving to buffered messages and higher bandwidth links this design choice is not any more in the best interest of the drone operator. We are now reporting all detected failures. To ensure architectural consistency more checks have been moved from the commander mainloop into the respective classes.
This commit is contained in:
Lorenz Meier
2020-03-17 00:17:11 +01:00
parent bf3b55cac7
commit aea68f06f9
7 changed files with 37 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/HealthFlags/HealthFlags.h
+1
View File
@@ -43,6 +43,7 @@
#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/subsystem_info.h>
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+23 -43
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -59,18 +59,15 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm,
const hrt_abstime &time_since_boot)
{
if (time_since_boot < 2_s) {
// the airspeed driver filter doesn't deliver the actual value yet
reportFailures = false;
}
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
bool checkSensors = !hil_enabled;
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
const bool checkSensors = !hil_enabled;
const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
const bool checkDynamic = !hil_enabled;
const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
const bool checkFailureDetector = true;
bool checkAirspeed = false;
@@ -81,9 +78,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
checkAirspeed = true;
}
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
bool failed = false;
/* ---- MAG ---- */
@@ -98,12 +92,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t prime_id = -1;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
bool mag_fail_reported = false;
/* 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 && !failed && !mag_fail_reported);
const bool report_fail = (reportFailures);
int32_t device_id = -1;
@@ -116,15 +108,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
} else {
if (required) {
failed = true;
mag_fail_reported = true;
}
}
}
/* check if the primary device is present */
if (!prime_found) {
if (reportFailures && !failed) {
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
@@ -133,7 +124,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) {
failed = true;
}
}
@@ -145,12 +136,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t prime_id = -1;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
bool accel_fail_reported = false;
/* 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 && !failed && !accel_fail_reported);
const bool report_fail = (reportFailures);
int32_t device_id = -1;
@@ -163,14 +152,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
} else {
if (required) {
failed = true;
accel_fail_reported = true;
}
}
}
/* check if the primary device is present */
if (!prime_found) {
if (reportFailures && !failed) {
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
@@ -185,16 +173,12 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t prime_id = -1;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
bool gyro_fail_reported = false;
/* 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);
const bool report_fail = (reportFailures && !failed && !gyro_fail_reported);
int32_t device_id = -1;
if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
@@ -203,14 +187,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
} else {
if (required) {
failed = true;
gyro_fail_reported = true;
}
}
}
/* check if the primary device is present */
if (!prime_found) {
if (reportFailures && !failed) {
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
@@ -229,13 +212,12 @@ 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 && !failed && !baro_fail_reported);
const bool report_fail = (reportFailures && !baro_fail_reported);
int32_t device_id = -1;
if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
baro_fail_reported = true;
}
}
@@ -245,7 +227,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 (checkSensors) {
if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
if (!imuConsistencyCheck(mavlink_log_pub, status, reportFailures)) {
failed = true;
}
}
@@ -255,14 +237,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t optional = 0;
param_get(param_find("FW_ARSP_MODE"), &optional);
if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures, prearm) && !(bool)optional) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rcCalibrationCheck(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) {
if (rcCalibrationCheck(mavlink_log_pub, reportFailures, status.is_vtol) != OK) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
}
@@ -282,7 +264,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- SYSTEM POWER ---- */
if (checkPower) {
if (!powerCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
if (!powerCheck(mavlink_log_pub, status, reportFailures, prearm)) {
failed = true;
}
}
@@ -303,16 +285,14 @@ 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 && !failed, checkGNSS)) {
if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail, checkGNSS)) {
failed = true;
}
}
/* ---- Failure Detector ---- */
if (checkFailureDetector) {
if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
failed = true;
}
if (!failureDetectorCheck(mavlink_log_pub, status, reportFailures, prearm)) {
failed = true;
}
/* Report status */
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+1 -1
View File
@@ -89,7 +89,7 @@ public:
};
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status,
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status,
bool report_fail = true);
private:
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+4 -3
View File
@@ -45,7 +45,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
const bool report_fail, const bool enforce_gps_required)
{
bool success = true; // start with a pass and change to a fail if any test fails
bool present = true;
bool ahrs_present = true;
float test_limit = 1.0f; // pass limit re-used for each test
int32_t mag_strength_check_enabled = 1;
@@ -60,7 +60,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) {
present = false;
ahrs_present = false;
goto out;
}
@@ -253,7 +253,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
out:
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status);
//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);
return success;
src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp
+1 -1
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
+6 -2
View File
@@ -36,9 +36,10 @@
#include <ArmAuthorization.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <HealthFlags.h>
bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status, bool report_fail)
const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail)
{
bool prearm_ok = true;
@@ -60,6 +61,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
}
// main battery level
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
status_flags.condition_battery_healthy, status);
// Only arm if healthy
if (!status_flags.condition_battery_healthy) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); }
@@ -89,7 +94,6 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
}
}
// Arm Requirements: global position
if (arm_requirements.global_position) {
if (!status_flags.condition_global_position_valid) {
src/modules/commander/Commander.cpp
+1 -11
View File
@@ -3833,13 +3833,10 @@ void Commander::battery_status_check()
// All connected batteries are regularly being published
(hrt_elapsed_time(&oldest_update) < 5_s)
// There is at least one connected battery (in any slot)
&& num_connected_batteries > 0
&& (num_connected_batteries > 0)
// No currently-connected batteries have any warning
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
(_battery_warning == battery_status_s::BATTERY_WARNING_NONE), status);
// execute battery failsafe if the state has gotten worse while we are armed
if (battery_warning_level_increased_while_armed) {
battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning,
@@ -3975,13 +3972,6 @@ void Commander::estimator_check()
}
}
if ((_last_condition_global_position_valid != status_flags.condition_global_position_valid)
&& status_flags.condition_global_position_valid) {
// If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa.
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, status);
set_health_flags_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, status);
}
check_valid(lpos.timestamp, _param_com_pos_fs_delay.get() * 1_s, lpos.z_valid,
&(status_flags.condition_local_altitude_valid), &_status_changed);
}