forked from Archive/PX4-Autopilot
Harmonize preflight and prearm checks, run same code except for dynamic range check only on arming
This commit is contained in:
parent
4f0896b105
commit
554719c78f
|
@ -58,6 +58,9 @@
|
|||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/drv_airspeed.h>
|
||||
|
||||
#include <uORB/topics/airspeed.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
|
@ -109,7 +112,7 @@ out:
|
|||
return success;
|
||||
}
|
||||
|
||||
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional)
|
||||
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
|
@ -148,6 +151,29 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional)
|
|||
goto out;
|
||||
}
|
||||
|
||||
if (dynamic) {
|
||||
/* check measurement result range */
|
||||
struct accel_report acc;
|
||||
ret = read(fd, &acc, sizeof(acc));
|
||||
|
||||
if (ret == sizeof(acc)) {
|
||||
/* evaluate values */
|
||||
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||||
|
||||
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
|
||||
/* this is frickin' fatal */
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
|
||||
/* this is frickin' fatal */
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
out:
|
||||
close(fd);
|
||||
return success;
|
||||
|
@ -218,11 +244,37 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
|
|||
return success;
|
||||
}
|
||||
|
||||
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC)
|
||||
static bool airspeedCheck(int mavlink_fd, bool optional)
|
||||
{
|
||||
bool success = true;
|
||||
int ret;
|
||||
int fd = orb_subscribe(ORB_ID(airspeed));
|
||||
|
||||
struct airspeed_s airspeed;
|
||||
|
||||
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
|
||||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
|
||||
mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
|
||||
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
|
||||
// XXX do not make this fatal yet
|
||||
}
|
||||
|
||||
out:
|
||||
close(fd);
|
||||
return success;
|
||||
}
|
||||
|
||||
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
|
||||
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
|
||||
{
|
||||
bool failed = false;
|
||||
|
||||
//Magnetometer
|
||||
/* ---- MAG ---- */
|
||||
if (checkMag) {
|
||||
/* check all sensors, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_mag_count; i++) {
|
||||
|
@ -234,19 +286,19 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
|
|||
}
|
||||
}
|
||||
|
||||
//Accelerometer
|
||||
/* ---- 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) && required) {
|
||||
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---- GYRO ----
|
||||
/* ---- GYRO ---- */
|
||||
if (checkGyro) {
|
||||
/* check all sensors, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
|
||||
|
@ -258,7 +310,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
|
|||
}
|
||||
}
|
||||
|
||||
// ---- BARO ----
|
||||
/* ---- BARO ---- */
|
||||
if (checkBaro) {
|
||||
/* check all sensors, but fail only for mandatory ones */
|
||||
for (unsigned i = 0; i < max_optional_baro_count; i++) {
|
||||
|
@ -270,14 +322,22 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
|
|||
}
|
||||
}
|
||||
|
||||
// ---- RC CALIBRATION ----
|
||||
/* ---- 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
|
||||
/* Report status */
|
||||
return !failed;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
@ -62,7 +62,8 @@ namespace Commander
|
|||
* @param checkRC
|
||||
* true if the Remote Controller should be checked
|
||||
**/
|
||||
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC);
|
||||
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
|
||||
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
|
||||
|
||||
const unsigned max_mandatory_gyro_count = 1;
|
||||
const unsigned max_optional_gyro_count = 3;
|
||||
|
|
|
@ -1125,8 +1125,15 @@ int commander_thread_main(int argc, char *argv[])
|
|||
commander_initialized = true;
|
||||
thread_running = true;
|
||||
|
||||
bool checkAirspeed = false;
|
||||
/* Perform airspeed check only if circuit breaker is not
|
||||
* engaged and it's not a rotary wing */
|
||||
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
|
||||
checkAirspeed = true;
|
||||
}
|
||||
|
||||
//Run preflight check
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
|
||||
if(!status.condition_system_sensors_initialized) {
|
||||
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
|
||||
}
|
||||
|
@ -1302,8 +1309,15 @@ int commander_thread_main(int argc, char *argv[])
|
|||
telemetry.heartbeat_time > 0 &&
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
|
||||
|
||||
bool chAirspeed = false;
|
||||
/* Perform airspeed check only if circuit breaker is not
|
||||
* engaged and it's not a rotary wing */
|
||||
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
|
||||
chAirspeed = true;
|
||||
}
|
||||
|
||||
/* provide RC and sensor status feedback to the user */
|
||||
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
|
||||
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
|
||||
}
|
||||
|
||||
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
|
||||
|
@ -2711,7 +2725,15 @@ void *commander_low_prio_loop(void *arg)
|
|||
// we do not set the calibration return value based on it because the calibration
|
||||
// might have worked just fine, but the preflight check fails for a different reason,
|
||||
// so this would be prone to false negatives.
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
|
||||
|
||||
bool checkAirspeed = false;
|
||||
/* Perform airspeed check only if circuit breaker is not
|
||||
* engaged and it's not a rotary wing */
|
||||
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
|
||||
checkAirspeed = true;
|
||||
}
|
||||
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
|
||||
|
||||
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
|
||||
|
@ -2719,8 +2741,16 @@ void *commander_low_prio_loop(void *arg)
|
|||
}
|
||||
|
||||
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
|
||||
|
||||
bool checkAirspeed = false;
|
||||
/* Perform airspeed check only if circuit breaker is not
|
||||
* engaged and it's not a rotary wing */
|
||||
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
|
||||
checkAirspeed = true;
|
||||
}
|
||||
|
||||
// Update preflight check status
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
|
||||
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
|
||||
|
||||
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
|
||||
|
||||
|
|
|
@ -52,18 +52,17 @@
|
|||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_airspeed.h>
|
||||
#include <drivers/drv_device.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "state_machine_helper.h"
|
||||
#include "commander_helper.h"
|
||||
#include "PreflightCheck.h"
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
|
@ -650,69 +649,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
|
|||
|
||||
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
||||
{
|
||||
int ret;
|
||||
bool failed = false;
|
||||
|
||||
int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
|
||||
|
||||
if (ret != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
/* check measurement result range */
|
||||
struct accel_report acc;
|
||||
ret = read(fd, &acc, sizeof(acc));
|
||||
|
||||
if (ret == sizeof(acc)) {
|
||||
/* evaluate values */
|
||||
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||||
|
||||
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
|
||||
/* this is frickin' fatal */
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
|
||||
/* this is frickin' fatal */
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
/* at this point this equals the preflight check, but might add additional
|
||||
* quantities later.
|
||||
*/
|
||||
bool checkAirspeed = false;
|
||||
/* Perform airspeed check only if circuit breaker is not
|
||||
* engaged and it's not a rotary wing */
|
||||
if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
|
||||
/* accel done, close it */
|
||||
close(fd);
|
||||
fd = orb_subscribe(ORB_ID(airspeed));
|
||||
|
||||
struct airspeed_s airspeed;
|
||||
|
||||
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
|
||||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
|
||||
mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
|
||||
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
|
||||
// XXX do not make this fatal yet
|
||||
}
|
||||
checkAirspeed = true;
|
||||
}
|
||||
|
||||
system_eval:
|
||||
close(fd);
|
||||
return (failed);
|
||||
return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue