Skip to content
Snippets Groups Projects
Commit 2271b3f1 authored by Daniel Agar's avatar Daniel Agar
Browse files

commander preflight checks pass status and status_flags messages

parent f2104217
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/modules/commander/PreflightCheck.cpp
+ 82
− 13
View file @ 2271b3f1
......@@ -109,6 +109,7 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
break;
}
}
instance++;
}
......@@ -140,6 +141,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
......@@ -150,6 +152,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
......@@ -175,6 +178,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
......@@ -191,10 +195,12 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
......@@ -215,27 +221,32 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
// get the sensor preflight data
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
struct sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
// can happen if not advertised (yet)
return true;
}
orb_unsubscribe(sensors_sub);
// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
float test_limit;
param_get(param_find("COM_ARM_MAG"), &test_limit);
if (sensors.mag_inconsistency_ga > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
}
return false;
}
return true;
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic,
int &device_id, bool report_fail)
{
bool success = true;
......@@ -260,6 +271,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
......@@ -270,11 +282,13 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
}
success = false;
goto out;
}
#ifdef __PX4_NUTTX
if (dynamic) {
/* check measurement result range */
struct accel_report acc;
......@@ -288,19 +302,23 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
goto out;
}
} else {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
goto out;
}
}
#endif
out:
......@@ -333,6 +351,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
}
......@@ -343,6 +362,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
}
......@@ -403,6 +423,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
}
......@@ -412,6 +433,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
}
......@@ -426,6 +448,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK");
}
success = false;
goto out;
}
......@@ -439,6 +462,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
}
success = false;
goto out;
}
......@@ -454,8 +478,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
bool success = true;
if (!prearm) {
// Ignore power check after arming.
return true;
// Ignore power check after arming.
return true;
} else {
int system_power_sub = orb_subscribe(ORB_ID(system_power));
......@@ -515,66 +539,81 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF INTERNAL CHECKS");
}
success = false;
goto out;
}
// check vertical position innovation test ratio
param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
if (status.hgt_test_ratio > test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HGT ERROR");
}
success = false;
goto out;
}
// check velocity innovation test ratio
param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
if (status.vel_test_ratio > test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF VEL ERROR");
}
success = false;
goto out;
}
// check horizontal position innovation test ratio
param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
if (status.pos_test_ratio > test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HORIZ POS ERROR");
}
success = false;
goto out;
}
// check magnetometer innovation test ratio
param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
if (status.mag_test_ratio > test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF YAW ERROR");
}
success = false;
goto out;
}
// check accelerometer delta velocity bias estimates
param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) {
if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit
|| fabsf(status.states[15]) > test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS");
}
success = false;
goto out;
}
// check gyro delta angle bias estimates
param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) {
if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
|| fabsf(status.states[12]) > test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS");
}
success = false;
goto out;
}
......@@ -583,30 +622,36 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
if (enforce_gps_required) {
bool ekf_gps_fusion = status.control_mode_flags & (1 << 2);
bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
if (!ekf_gps_fusion) {
// The EKF is not using GPS
if (report_fail) {
if (ekf_gps_check_fail) {
// Poor GPS qulaity is the likely cause
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
} else {
// Likely cause unknown
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
}
}
success = false;
goto out;
} else {
// The EKF is using GPS so check for bad quality on key performance indicators
bool gps_quality_fail = ((status.gps_check_fail_flags & ((1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0);
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)
+ (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0);
if (gps_quality_fail) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
}
success = false;
goto out;
}
......@@ -618,8 +663,9 @@ out:
return success;
}
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot)
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
const hrt_abstime &time_since_boot)
{
if (time_since_boot < 2000000) {
......@@ -627,6 +673,23 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
reportFailures = false;
}
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
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);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) {
checkAirspeed = true;
}
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout);
#ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when
// all the sensors are supported
......@@ -673,6 +736,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
failed = true;
}
......@@ -696,7 +760,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures && !accel_fail_reported)) && required) {
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures
&& !accel_fail_reported)) && required) {
failed = true;
accel_fail_reported = true;
}
......@@ -711,6 +776,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
failed = true;
}
}
......@@ -743,6 +809,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
failed = true;
}
}
......@@ -776,6 +843,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
if (reportFailures && !failed) {
mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
}
failed = true;
}
}
......@@ -796,7 +864,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_log_pub, reportFailures, isVTOL) != OK) {
if (rc_calibration_check(mavlink_log_pub, reportFailures, status.is_vtol) != OK) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
}
......@@ -816,6 +884,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool check
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
int32_t estimator_type;
param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
if (estimator_type == 2) {
// 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 * 1000000);
......
src/modules/commander/PreflightCheck.h
+ 5
− 2
View file @ 2271b3f1
......@@ -40,6 +40,8 @@
*/
#include <drivers/drv_hrt.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#pragma once
......@@ -70,8 +72,9 @@ namespace Preflight
* @param checkPower
* true if the system power should be checked
**/
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool checkPower, bool isVTOL, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot);
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
const hrt_abstime &time_since_boot);
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_optional_gyro_count = 3;
......
src/modules/commander/commander.cpp
+ 1
− 18
View file @ 2271b3f1
......@@ -4032,26 +4032,9 @@ void Commander::mission_init()
bool Commander::preflight_check(bool report)
{
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
const bool checkSensors = !hil_enabled;
const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT);
const bool checkDynamic = !hil_enabled;
const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
const bool reportFailures = (report && status_flags.condition_system_hotplug_timeout);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status_flags.circuit_breaker_engaged_airspd_check && (!status.is_rotary_wing || status.is_vtol)) {
checkAirspeed = true;
}
bool success = Preflight::preflightCheck(&mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower,
status.is_vtol, reportFailures, false, hrt_elapsed_time(&commander_boot_timestamp));
bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, hrt_elapsed_time(&commander_boot_timestamp));
status_flags.condition_system_sensors_initialized = success;
......
src/modules/commander/state_machine_helper.cpp
+ 3
− 18
View file @ 2271b3f1
......@@ -75,7 +75,7 @@ static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MA
};
// You can index into the array with an arming_state_t in order to get its textual representation
const char * const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"INIT",
"STANDBY",
"ARMED",
......@@ -120,27 +120,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
bool preflight_check_ret = true;
bool prearm_check_ret = true;
const bool checkSensors = !hil_enabled;
const bool checkRC = (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT);
const bool checkDynamic = !hil_enabled;
const bool checkPower = (status_flags->condition_power_input_valid
&& !status_flags->circuit_breaker_engaged_power_check);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
checkAirspeed = true;
}
/* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !hil_enabled) {
preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, checkSensors, checkAirspeed, checkRC, checkGNSS,
checkDynamic, checkPower, status->is_vtol, true, true, time_since_boot);
preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot);
if (preflight_check_ret) {
status_flags->condition_system_sensors_initialized = true;
......@@ -156,8 +142,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const batt
if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, checkSensors,
checkAirspeed, checkRC, checkGNSS, checkDynamic, checkPower, status->is_vtol, false, false, time_since_boot);
status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, false, false, time_since_boot);
last_preflight_check = hrt_absolute_time();
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment