From 554719c78fe4e0e07e56bc7a57877340924d0d92 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 23:07:32 +0200 Subject: Harmonize preflight and prearm checks, run same code except for dynamic range check only on arming --- src/modules/commander/PreflightCheck.cpp | 78 +++++++++++++++++++++++--- src/modules/commander/PreflightCheck.h | 3 +- src/modules/commander/commander.cpp | 38 +++++++++++-- src/modules/commander/state_machine_helper.cpp | 69 +++-------------------- 4 files changed, 112 insertions(+), 76 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 70015cddd..6bb7e5c24 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -58,6 +58,9 @@ #include #include #include +#include + +#include #include @@ -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; } + } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 935f69969..66947a347 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c512c15c..bb1ed7f5d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3be329500..ccfc7a986 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,18 +52,17 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #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); } -- cgit v1.2.3