Exemplo n.º 1
0
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);
		}
	}
}
Exemplo n.º 2
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;
}
Exemplo n.º 3
0
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;
}
Exemplo n.º 4
0
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;
}