From 7dbb6c4fa80039bd9c42a83161f18da9033ac40a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 19 Apr 2015 13:57:07 +0200 Subject: [PATCH] Commander: Improved preflight check routines. Running checks on all connected sensors. Re-run checks once GCS is connected. --- src/modules/commander/PreflightCheck.cpp | 445 ++++++++++-------- src/modules/commander/PreflightCheck.h | 50 +- src/modules/commander/commander.cpp | 3 +- .../commander/state_machine_helper.cpp | 8 +- 4 files changed, 292 insertions(+), 214 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 873f2c2890..70015cddd6 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -1,44 +1,44 @@ /**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ +* +* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ /** - * @file PreflightCheck.cpp - * - * Preflight check for main system components - * - * @author Lorenz Meier - * @author Johan Jansen - */ +* @file PreflightCheck.cpp +* +* Preflight check for main system components +* +* @author Lorenz Meier +* @author Johan Jansen +*/ #include #include @@ -65,158 +65,219 @@ namespace Commander { - static bool magnometerCheck(int mavlink_fd) - { - int fd = open(MAG0_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"); - return false; - } - - int calibration_devid; - int devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_MAG0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("magnetometer calibration is for a different device - calibrate magnetometer first (dev: %d vs cal: %d)", devid, calibration_devid); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); - return false; - } - - int 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"); - return false; - } - close(fd); - return true; - } - - static bool accelerometerCheck(int mavlink_fd) - { - int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY); - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); - - int calibration_devid; - int devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_ACC0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); - return false; - } - - if (ret != OK) { - warnx("accel self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); - return false; - } - - // 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); - - // evaluate values - if (accel_magnitude > 30.0f) { //m/s^2 - warnx("accel with spurious values"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); - //this is frickin' fatal - return false; - } - } else { - warnx("accel read failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); - //this is frickin' fatal - return false; - } - - close(fd); - return true; - } - - static bool gyroCheck(int mavlink_fd) - { - int fd = open(GYRO0_DEVICE_PATH, 0); - - int calibration_devid; - int devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid)); - if (devid != calibration_devid){ - warnx("gyro calibration is for a different device - calibrate gyro first"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); - return false; - } - - int ret = ioctl(fd, GYROIOCSELFTEST, 0); - - if (ret != OK) { - warnx("gyro self test failed"); - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); - return false; - } - - close(fd); - return true; - } - - static bool baroCheck(int mavlink_fd) - { - int fd = open(BARO0_DEVICE_PATH, 0); - if(fd < 0) { - mavlink_log_critical(mavlink_fd, "SENSOR FAIL: Barometer"); - return false; - } - - close(fd); - return true; - } - - bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC) - { - //give the system some time to sample the sensors in the background - usleep(150000); - - //Magnetometer - if(checkMag) { - if(!magnometerCheck(mavlink_fd)) { - return false; - } - } - - //Accelerometer - if(checkAcc) { - if(!accelerometerCheck(mavlink_fd)) { - return false; - } - } - - // ---- GYRO ---- - if(checkGyro) { - if(!gyroCheck(mavlink_fd)) { - return false; - } - } - - // ---- BARO ---- - if(checkBaro) { - if(!baroCheck(mavlink_fd)) { - return false; - } - } - - // ---- RC CALIBRATION ---- - if(checkRC) { - if(rc_calibration_check(mavlink_fd) != OK) { - return false; - } - } - - //All is good! - return true; - } +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_MAG%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, MAGIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); + int fd = open(s, O_RDONLY); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_ACC%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } + + return false; + } + + int calibration_devid; + int ret; + int devid = ioctl(fd, DEVIOCGDEVICEID, 0); + sprintf(s, "CAL_GYRO%u_ID", instance); + param_get(param_find(s), &(calibration_devid)); + + if (devid != calibration_devid) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance); + success = false; + goto out; + } + + ret = ioctl(fd, GYROIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance); + success = false; + goto out; + } + +out: + close(fd); + return success; +} + +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +{ + bool success = true; + + char s[30]; + sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); + int fd = open(s, 0); + + if (fd < 0) { + if (!optional) { + mavlink_and_console_log_critical(mavlink_fd, + "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } + + return false; + } + + close(fd); + return success; +} + +bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC) +{ + bool failed = false; + +//Magnetometer + 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; + } + } + } + +//Accelerometer + 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) { + 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; + } + } + } + +// ---- RC CALIBRATION ---- + if (checkRC) { + if (rc_calibration_check(mavlink_fd) != OK) { + failed = true; + } + } + +// Report status + return !failed; +} } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index e29357ec91..935f699690 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -43,21 +43,37 @@ namespace Commander { - /** - * @brief - * Runs a preflight check on all sensors to see if they are properly calibrated and healthy - * @param mavlink_fd - * Mavlink output file descriptor for feedback when a sensor fails - * @param checkMag - * true if the magneteometer should be checked - * @param checkAcc - * true if the accelerometers should be checked - * @param checkGyro - * true if the gyroscopes should be checked - * @param checkBaro - * true if the barometer should be checked - * @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); +/** +* Runs a preflight check on all sensors to see if they are properly calibrated and healthy +* +* The function won't fail the test if optional sensors are not found, however, +* it will fail the test if optional sensors are found but not in working condition. +* +* @param mavlink_fd +* Mavlink output file descriptor for feedback when a sensor fails +* @param checkMag +* true if the magneteometer should be checked +* @param checkAcc +* true if the accelerometers should be checked +* @param checkGyro +* true if the gyroscopes should be checked +* @param checkBaro +* true if the barometer should be checked +* @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); + +const unsigned max_mandatory_gyro_count = 1; +const unsigned max_optional_gyro_count = 3; + +const unsigned max_mandatory_accel_count = 1; +const unsigned max_optional_accel_count = 3; + +const unsigned max_mandatory_mag_count = 1; +const unsigned max_optional_mag_count = 3; + +const unsigned max_mandatory_baro_count = 1; +const unsigned max_optional_baro_count = 1; + } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2a143f7f2..5c512c15c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1302,7 +1302,8 @@ int commander_thread_main(int argc, char *argv[]) telemetry.heartbeat_time > 0 && hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { - (void)rc_calibration_check(mavlink_fd); + /* provide RC and sensor status feedback to the user */ + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 58a123adfd..3be3295002 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -77,8 +77,8 @@ static const int ERROR = -1; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false }, { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, { /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, @@ -212,10 +212,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* Sensors need to be initialized for STANDBY state */ if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational."); + mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational."); feedback_provided = true; valid_transition = false; - new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition