int preflight_check_main(int argc, char *argv[]) { bool fail_on_error = false; if (argc > 1 && !strcmp(argv[1], "--help")) { warnx("usage: preflight_check [--fail-on-error]\n\tif fail on error is enabled, will return 1 on error"); exit(1); } if (argc > 1 && !strcmp(argv[1], "--fail-on-error")) { fail_on_error = true; } bool system_ok = true; int fd; /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; /* give the system some time to sample the sensors in the background */ usleep(150000); /* ---- MAG ---- */ close(fd); fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG"); system_ok = false; goto system_eval; } ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- ACCEL ---- */ close(fd); fd = open(ACCEL_DEVICE_PATH, 0); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- GYRO ---- */ close(fd); fd = open(GYRO_DEVICE_PATH, 0); ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); system_ok = false; goto system_eval; } /* ---- BARO ---- */ close(fd); fd = open(BARO_DEVICE_PATH, 0); close(fd); /* ---- RC CALIBRATION ---- */ bool rc_ok = (OK == rc_calibration_check()); /* warn */ if (!rc_ok) warnx("rc calibration test failed"); /* require RC ok to keep system_ok */ system_ok &= rc_ok; system_eval: if (system_ok) { /* all good, exit silently */ exit(0); } else { fflush(stdout); warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); fflush(stderr); int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { close(buzzer); errx(1, "failed to open leds, aborting"); } /* flip blue led into alternating amber */ led_off(leds, LED_BLUE); led_off(leds, LED_AMBER); led_toggle(leds, LED_BLUE); /* display and sound error */ for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); if (i % 10 == 0) { ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } else if (i % 5 == 0) { ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); } usleep(100000); } /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); /* switch on leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); if (fail_on_error) { /* exit with error message */ exit(1); } else { /* do not emit an error code to make sure system still boots */ exit(0); } } }
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; }