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) {
// 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;
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 && !status_flags.condition_calibration_enabled);
#ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when
// all the sensors are supported
PX4_WARN("Preflight checks always pass on Snapdragon.");
checkSensors = false;
#elif defined(__PX4_POSIX_RPI)
PX4_WARN("Preflight checks for mag, acc, gyro always pass on RPI");
checkSensors = false;
#elif defined(__PX4_POSIX_BEBOP)
PX4_WARN("Preflight checks always pass on Bebop.");
checkSensors = false;
#elif defined(__PX4_POSIX_OCPOC)
PX4_WARN("Preflight checks always pass on OcPoC.");
checkSensors = false;
#endif
bool failed = false;
/* ---- MAG ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
int32_t sys_has_mag = 1;
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
bool mag_fail_reported = false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count) && sys_has_mag == 1;
int device_id = -1;
if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, (reportFailures && !failed && !mag_fail_reported)) && required) {
failed = true;
mag_fail_reported = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
failed = true;
}
/* fail if mag sensors are inconsistent */
if (!magConsistencyCheck(mavlink_log_pub, (reportFailures && !failed))) {
failed = true;
}
}
/* ---- ACCEL ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
bool accel_fail_reported = false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures && !failed && !accel_fail_reported)) && required) {
failed = true;
accel_fail_reported = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
failed = true;
}
}
/* ---- GYRO ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
bool gyro_fail_reported = false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_log_pub, i, !required, device_id, (reportFailures && !failed && !gyro_fail_reported)) && required) {
failed = true;
gyro_fail_reported = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
failed = true;
}
}
/* ---- BARO ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_BARO_PRIME"), &prime_id);
int32_t sys_has_baro = 1;
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
bool baro_fail_reported = false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count) && sys_has_baro == 1;
int device_id = -1;
if (!baroCheck(mavlink_log_pub, i, !required, device_id, (reportFailures && !failed && !baro_fail_reported)) && required) {
failed = true;
baro_fail_reported = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
if (reportFailures && !failed) {
mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
}
failed = true;
}
}
/* ---- IMU CONSISTENCY ---- */
if (checkSensors) {
if (!imuConsistencyCheck(mavlink_log_pub, (reportFailures && !failed))) {
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures && !failed, prearm)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
}
failed = true;
}
}
/* ---- SYSTEM POWER ---- */
if (checkPower) {
if (!powerCheck(mavlink_log_pub, (reportFailures && !failed), prearm)) {
failed = true;
}
}
/* ---- Navigation EKF ---- */
// 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);
if (!ekf2Check(mavlink_log_pub, true, (reportFailures && report_ekf_fail && !failed), checkGNSS)) {
failed = true;
}
}
/* Report status */
return !failed;
}
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
/* ---- MAG ---- */
if (checkMag) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary compass not found");
}
failed = true;
}
}
/* ---- ACCEL ---- */
if (checkAcc) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found");
}
failed = true;
}
}
/* ---- GYRO ---- */
if (checkGyro) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary gyro not found");
}
failed = true;
}
}
/* ---- BARO ---- */
if (checkBaro) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_BARO_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "warning: primary barometer not operational");
}
failed = true;
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_log_pub, reportFailures) != OK) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_log_pub, "RC calibration check failed");
}
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if (!gnssCheck(mavlink_log_pub, reportFailures)) {
failed = true;
}
}
#ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when
// all the sensors are supported
PX4_WARN("WARNING: Preflight checks PASS always.");
failed = false;
#endif
/* Report status */
return !failed;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
{
bool failed = false;
/* ---- MAG ---- */
if (checkMag) {
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count);
if (!magnometerCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
/* ---- ACCEL ---- */
if (checkAcc) {
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count);
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
failed = true;
}
}
}
/* ---- GYRO ---- */
if (checkGyro) {
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count);
if (!gyroCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
/* ---- BARO ---- */
if (checkBaro) {
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count);
if (!baroCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
failed = true;
}
}
/* Report status */
return !failed;
}