diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index bcf59df685..6ab1d0b49f 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -63,21 +63,6 @@ */ #define RC_INPUT_RSSI_MAX 100 -/** - * Minimum value - */ -#define RC_INPUT_LOWEST_MIN_US 500 - -/** - * Maximum value - */ -#define RC_INPUT_HIGHEST_MAX_US 2500 - -/** - * Maximum deadzone value - */ -#define RC_INPUT_MAX_DEADZONE_US 500 - /** * Input signal type, value is a control position from zero to 100 * percent. diff --git a/src/lib/systemlib/mavlink_log.h b/src/lib/systemlib/mavlink_log.h index bb9d665355..0f5b1086bf 100644 --- a/src/lib/systemlib/mavlink_log.h +++ b/src/lib/systemlib/mavlink_log.h @@ -41,6 +41,7 @@ #pragma once +#include #include /** diff --git a/src/modules/commander/arm_auth.cpp b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp similarity index 99% rename from src/modules/commander/arm_auth.cpp rename to src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp index 0eb131933c..5f265a077b 100644 --- a/src/modules/commander/arm_auth.cpp +++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ -#include "arm_auth.h" +#include "ArmAuthorization.h" #include #include diff --git a/src/modules/commander/arm_auth.h b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h similarity index 100% rename from src/modules/commander/arm_auth.h rename to src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h diff --git a/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt b/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt new file mode 100644 index 0000000000..648d5ba337 --- /dev/null +++ b/src/modules/commander/Arming/ArmAuthorization/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +px4_add_library(ArmAuthorization + ArmAuthorization.cpp +) +target_include_directories(ArmAuthorization + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt new file mode 100644 index 0000000000..1d638ad3dd --- /dev/null +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +add_subdirectory(PreFlightCheck) +add_subdirectory(ArmAuthorization) +add_subdirectory(HealthFlags) diff --git a/src/modules/commander/Arming/HealthFlags/CMakeLists.txt b/src/modules/commander/Arming/HealthFlags/CMakeLists.txt new file mode 100644 index 0000000000..4b8dc20cef --- /dev/null +++ b/src/modules/commander/Arming/HealthFlags/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +px4_add_library(HealthFlags + HealthFlags.cpp +) +target_include_directories(HealthFlags PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/commander/health_flag_helper.cpp b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp similarity index 98% rename from src/modules/commander/health_flag_helper.cpp rename to src/modules/commander/Arming/HealthFlags/HealthFlags.cpp index 0351977680..892ee91257 100644 --- a/src/modules/commander/health_flag_helper.cpp +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp @@ -32,14 +32,14 @@ ****************************************************************************/ /** - * @file health_flag_helper.cpp + * @file HealthFlags.cpp * * Contains helper functions to efficiently set the system health flags from commander and preflight check. * * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch) */ -#include "health_flag_helper.h" +#include "HealthFlags.h" void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status) { diff --git a/src/modules/commander/health_flag_helper.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h similarity index 98% rename from src/modules/commander/health_flag_helper.h rename to src/modules/commander/Arming/HealthFlags/HealthFlags.h index 525410bfa0..978e1ca7fe 100644 --- a/src/modules/commander/health_flag_helper.h +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file health_flag_helper.h + * @file HealthFlags.h * * Contains helper functions to efficiently set the system health flags from commander and preflight check. * diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt new file mode 100644 index 0000000000..c409b7ad75 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +px4_add_library(PreFlightCheck + PreFlightCheck.cpp + checks/preArmCheck.cpp + checks/magnetometerCheck.cpp + checks/magConsistencyCheck.cpp + checks/accelerometerCheck.cpp + checks/gyroCheck.cpp + checks/baroCheck.cpp + checks/imuConsistencyCheck.cpp + checks/airspeedCheck.cpp + checks/rcCalibrationCheck.cpp + checks/powerCheck.cpp + checks/ekf2Check.cpp + checks/failureDetectorCheck.cpp +) +target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp new file mode 100644 index 0000000000..0e6acbe822 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -0,0 +1,373 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 + */ + +#include "PreFlightCheck.hpp" + +#include +#include +#include +#include +#include + +using namespace time_literals; + +static constexpr unsigned max_mandatory_gyro_count = 1; +static constexpr unsigned max_optional_gyro_count = 3; +static constexpr unsigned max_mandatory_accel_count = 1; +static constexpr unsigned max_optional_accel_count = 3; +static constexpr unsigned max_mandatory_mag_count = 1; +static constexpr unsigned max_optional_mag_count = 4; +static constexpr unsigned max_mandatory_baro_count = 1; +static constexpr unsigned max_optional_baro_count = 1; + +bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, + vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, + const hrt_abstime &time_since_boot) +{ + if (time_since_boot < 2_s) { + // 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); + const bool checkFailureDetector = true; + + 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.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) { + checkAirspeed = true; + } + + reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout + && !status_flags.condition_calibration_enabled); + + bool failed = false; + + /* ---- MAG ---- */ + if (checkSensors) { + bool prime_found = false; + + int32_t prime_id = -1; + 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 individually, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_mag_count; i++) { + const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1); + const bool report_fail = (reportFailures && !failed && !mag_fail_reported); + + int32_t device_id = -1; + + if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + + if ((prime_id > 0) && (device_id == prime_id)) { + prime_found = true; + } + + } else { + if (required) { + failed = true; + mag_fail_reported = true; + } + } + } + + if (sys_has_mag == 1) { + /* check if the primary device is present */ + if (!prime_found) { + if (reportFailures && !failed) { + mavlink_log_critical(mavlink_log_pub, "Primary compass not found"); + } + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status); + failed = true; + } + + /* mag consistency checks (need to be performed after the individual checks) */ + if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) { + failed = true; + } + } + } + + /* ---- ACCEL ---- */ + if (checkSensors) { + bool prime_found = false; + int32_t prime_id = -1; + param_get(param_find("CAL_ACC_PRIME"), &prime_id); + + bool accel_fail_reported = false; + + /* check all sensors individually, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_accel_count; i++) { + const bool required = (i < max_mandatory_accel_count); + const bool report_fail = (reportFailures && !failed && !accel_fail_reported); + + int32_t device_id = -1; + + if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) { + + if ((prime_id > 0) && (device_id == prime_id)) { + prime_found = true; + } + + } else { + if (required) { + failed = true; + accel_fail_reported = true; + } + } + } + + /* check if the primary device is present */ + if (!prime_found) { + if (reportFailures && !failed) { + mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found"); + } + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status); + failed = true; + } + } + + /* ---- GYRO ---- */ + if (checkSensors) { + bool prime_found = false; + int32_t prime_id = -1; + param_get(param_find("CAL_GYRO_PRIME"), &prime_id); + + bool gyro_fail_reported = false; + + /* check all sensors individually, but fail only for mandatory ones */ + for (unsigned i = 0; i < max_optional_gyro_count; i++) { + const bool required = (i < max_mandatory_gyro_count); + const bool report_fail = (reportFailures && !failed && !gyro_fail_reported); + + int32_t device_id = -1; + + if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + + if ((prime_id > 0) && (device_id == prime_id)) { + prime_found = true; + } + + } else { + if (required) { + failed = true; + gyro_fail_reported = true; + } + } + } + + /* check if the primary device is present */ + if (!prime_found) { + if (reportFailures && !failed) { + mavlink_log_critical(mavlink_log_pub, "Primary gyro not found"); + } + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status); + failed = true; + } + } + + /* ---- BARO ---- */ + if (checkSensors) { + //bool prime_found = false; + + int32_t prime_id = -1; + 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++) { + const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1); + const bool report_fail = (reportFailures && !failed && !baro_fail_reported); + + int32_t device_id = -1; + + if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + if ((prime_id > 0) && (device_id == prime_id)) { + //prime_found = true; + } + + } else { + if (required) { + failed = true; + baro_fail_reported = true; + } + } + } + + // TODO there is no logic in place to calibrate the primary baro yet + // // check if the primary device is present + // if (false) { + // if (reportFailures && !failed) { + // mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational"); + // } + + // set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status); + // failed = true; + // } + } + + /* ---- IMU CONSISTENCY ---- */ + // To be performed after the individual sensor checks have completed + if (checkSensors) { + if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) { + failed = true; + } + } + + /* ---- AIRSPEED ---- */ + if (checkAirspeed) { + int32_t optional = 0; + param_get(param_find("FW_ARSP_MODE"), &optional); + + if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) { + failed = true; + } + } + + /* ---- RC CALIBRATION ---- */ + if (checkRC) { + if (rcCalibrationCheck(mavlink_log_pub, reportFailures && !failed, status.is_vtol) != OK) { + if (reportFailures) { + mavlink_log_critical(mavlink_log_pub, "RC calibration check failed"); + } + + failed = true; + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status); + status_flags.rc_calibration_valid = false; + + } else { + // The calibration is fine, but only set the overall health state to true if the signal is not currently lost + status_flags.rc_calibration_valid = true; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, + !status.rc_signal_lost, status); + } + } + + /* ---- SYSTEM POWER ---- */ + if (checkPower) { + if (!powerCheck(mavlink_log_pub, status, (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 = -1; + + if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) { + param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type); + + } else { + // EKF2 is currently the only supported option for FW & VTOL + estimator_type = 2; + } + + 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_s); + + if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) { + failed = true; + } + } + + /* ---- Failure Detector ---- */ + if (checkFailureDetector) { + if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) { + failed = true; + } + } + + /* Report status */ + return !failed; +} + +bool PreFlightCheck::check_calibration(const char *param_template, const int32_t device_id) +{ + bool calibration_found = false; + + char s[20]; + int instance = 0; + + /* old style transition: check param values */ + while (!calibration_found) { + sprintf(s, param_template, instance); + const param_t parm = param_find_no_notification(s); + + /* if the calibration param is not present, abort */ + if (parm == PARAM_INVALID) { + break; + } + + /* if param get succeeds */ + int32_t calibration_devid = -1; + + if (param_get(parm, &calibration_devid) == PX4_OK) { + + /* if the devid matches, exit early */ + if (device_id == calibration_devid) { + calibration_found = true; + break; + } + } + + instance++; + } + + return calibration_found; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp new file mode 100644 index 0000000000..052aad9854 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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.hpp + * + * Check if flight is safely possible + * if not prevent it and inform the user. + */ + +#pragma once + +#include +#include + +#include +#include + +class PreFlightCheck +{ +public: + PreFlightCheck() = default; + ~PreFlightCheck() = default; + + /** + * 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_log_pub + * Mavlink output orb handle reference 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 checkAirspeed + * true if the airspeed sensor should be checked + * @param checkRC + * true if the Remote Controller should be checked + * @param checkGNSS + * true if the GNSS receiver should be checked + * @param checkPower + * true if the system power should be checked + **/ + static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, + vehicle_status_flags_s &status_flags, + const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot); + + static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const safety_s &safety, const uint8_t arm_requirements); + + typedef enum { + ARM_REQ_NONE = 0, + ARM_REQ_MISSION_BIT = (1 << 0), + ARM_REQ_ARM_AUTH_BIT = (1 << 1), + ARM_REQ_GPS_BIT = (1 << 2), + ARM_REQ_ESCS_CHECK_BIT = (1 << 3) + } arm_requirements_t; + +private: + static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, int32_t &device_id, const bool report_fail); + static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); + static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail); + static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, int32_t &device_id, const bool report_fail); + static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, int32_t &device_id, const bool report_fail); + static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); + static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, + const bool report_fail, const bool prearm); + static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL); + static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, + const bool prearm); + static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, + const bool report_fail, const bool enforce_gps_required); + static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, + const bool prearm); + static bool check_calibration(const char *param_template, const int32_t device_id); +}; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp new file mode 100644 index 0000000000..28e7631d91 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail) +{ + const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); + bool calibration_valid = false; + bool accel_valid = true; + + if (exists) { + + uORB::SubscriptionData accel{ORB_ID(sensor_accel), instance}; + + accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s); + + if (!accel_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance); + } + } + + device_id = accel.get().device_id; + + calibration_valid = check_calibration("CAL_ACC%u_ID", device_id); + + if (!calibration_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance); + } + + } else { + + if (dynamic) { + const float accel_magnitude = sqrtf(accel.get().x * accel.get().x + + accel.get().y * accel.get().y + + accel.get().z * accel.get().z); + + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming"); + } + + /* this is frickin' fatal */ + accel_valid = false; + } + } + } + + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor #%u missing", instance); + } + } + + const bool success = calibration_valid && accel_valid; + + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status); + + } else if (instance == 1) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status); + } + + return success; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp new file mode 100644 index 0000000000..bd646a9f43 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, + const bool report_fail, const bool prearm) +{ + bool present = true; + bool success = true; + + uORB::SubscriptionData airspeed_sub{ORB_ID(airspeed)}; + airspeed_sub.update(); + const airspeed_s &airspeed = airspeed_sub.get(); + + if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) { + if (report_fail && !optional) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing"); + } + + present = false; + success = false; + goto out; + } + + /* + * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise + * on the bench and trigger false positives of the voter. Therefore only fail this + * for a pre-arm check, as then the cover is off and the natural airflow in the field + * will ensure there is not zero noise. + */ + if (prearm && fabsf(airspeed.confidence) < 0.95f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck"); + } + + present = true; + success = false; + goto out; + } + + /** + * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying + * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover + * might have been removed. + */ + if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && !prearm) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot"); + } + + present = true; + success = false; + goto out; + } + +out: + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status); + + return success; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp new file mode 100644 index 0000000000..1b7542b531 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, int32_t &device_id, const bool report_fail) +{ + const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK); + bool baro_valid = false; + + if (exists) { + uORB::SubscriptionData baro{ORB_ID(sensor_baro), instance}; + + baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s); + + if (!baro_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro #%u", instance); + } + } + + + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor #%u missing", instance); + } + } + + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status); + } + + return baro_valid; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp new file mode 100644 index 0000000000..378b7b2563 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -0,0 +1,244 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include + +bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, + const bool report_fail, const bool enforce_gps_required) +{ + bool success = true; // start with a pass and change to a fail if any test fails + bool present = true; + float test_limit = 1.0f; // pass limit re-used for each test + + bool gps_success = true; + bool gps_present = true; + + // Get estimator status data if available and exit with a fail recorded if not + uORB::SubscriptionData status_sub{ORB_ID(estimator_status)}; + status_sub.update(); + const estimator_status_s &status = status_sub.get(); + + if (status.timestamp == 0) { + present = false; + goto out; + } + + // Check if preflight check performed by estimator has failed + if (status.pre_flt_fail_innov_heading || + status.pre_flt_fail_innov_vel_horiz || + status.pre_flt_fail_innov_vel_vert || + status.pre_flt_fail_innov_height) { + if (report_fail) { + if (status.pre_flt_fail_innov_heading) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable"); + + } else if (status.pre_flt_fail_innov_vel_horiz) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable"); + + } else if (status.pre_flt_fail_innov_vel_horiz) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable"); + + } else if (status.pre_flt_fail_innov_height) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable"); + } + } + + success = false; + goto out; + } + + // check vertical position innovation test ratio + param_get(param_find("COM_ARM_EKF_HGT"), &test_limit); + + if (status.hgt_test_ratio > test_limit) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error"); + } + + success = false; + goto out; + } + + // check velocity innovation test ratio + param_get(param_find("COM_ARM_EKF_VEL"), &test_limit); + + if (status.vel_test_ratio > test_limit) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error"); + } + + success = false; + goto out; + } + + // check horizontal position innovation test ratio + param_get(param_find("COM_ARM_EKF_POS"), &test_limit); + + if (status.pos_test_ratio > test_limit) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error"); + } + + success = false; + goto out; + } + + // check magnetometer innovation test ratio + param_get(param_find("COM_ARM_EKF_YAW"), &test_limit); + + if (status.mag_test_ratio > test_limit) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error"); + } + + success = false; + goto out; + } + + // check accelerometer delta velocity bias estimates + param_get(param_find("COM_ARM_EKF_AB"), &test_limit); + + for (uint8_t index = 13; index < 16; index++) { + // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives + // adjust test threshold by 3-sigma + float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f)); + + if (fabsf(status.states[index]) > test_limit + test_uncertainty) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); + } + + success = false; + goto out; + } + } + + // check gyro delta angle bias estimates + param_get(param_find("COM_ARM_EKF_GB"), &test_limit); + + if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit + || fabsf(status.states[12]) > test_limit) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias"); + } + + success = false; + goto out; + } + + // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing + if (enforce_gps_required || report_fail) { + const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS); + const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0; + + gps_success = ekf_gps_fusion; // default to success if gps data is fused + + if (ekf_gps_check_fail) { + if (report_fail) { + // Only report the first failure to avoid spamming + const char *message = nullptr; + + if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { + message = "Preflight%s: GPS fix too low"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { + message = "Preflight%s: not enough GPS Satellites"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) { + message = "Preflight%s: GPS GDoP too low"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { + message = "Preflight%s: GPS Horizontal Pos Error too high"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { + message = "Preflight%s: GPS Vertical Pos Error too high"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { + message = "Preflight%s: GPS Speed Accuracy too low"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { + message = "Preflight%s: GPS Horizontal Pos Drift too high"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { + message = "Preflight%s: GPS Vertical Pos Drift too high"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { + message = "Preflight%s: GPS Hor Speed Drift too high"; + + } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { + message = "Preflight%s: GPS Vert Speed Drift too high"; + + } else { + if (!ekf_gps_fusion) { + // Likely cause unknown + message = "Preflight%s: Estimator not using GPS"; + gps_present = false; + + } else { + // if we land here there was a new flag added and the code not updated. Show a generic message. + message = "Preflight%s: Poor GPS Quality"; + } + } + + if (message) { + if (enforce_gps_required) { + mavlink_log_critical(mavlink_log_pub, message, " Fail"); + + } else { + mavlink_log_warning(mavlink_log_pub, message, ""); + } + } + } + + gps_success = false; + + if (enforce_gps_required) { + success = false; + goto out; + } + } + } + +out: + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status); + + return success; +} diff --git a/src/modules/commander/rc_check.h b/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp similarity index 62% rename from src/modules/commander/rc_check.h rename to src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp index c878f9c130..05345ee5c9 100644 --- a/src/modules/commander/rc_check.h +++ b/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 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 @@ -31,19 +31,35 @@ * ****************************************************************************/ -/** - * @file rc_check.h - * - * RC calibration check - */ -#include +#include "../PreFlightCheck.hpp" -#pragma once +#include -/** - * Check the RC calibration - * - * @return 0 / OK if RC calibration is ok, index + 1 of the first - * channel that failed else (so 1 == first channel failed) - */ -int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL); +bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, + const bool report_fail, const bool prearm) +{ + // Ignore failure detector check after arming + if (!prearm) { + return true; + } + + if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) { + if (report_fail) { + if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected"); + } + + if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected"); + } + + if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected"); + } + } + + return false; + } + + return true; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp new file mode 100644 index 0000000000..50496e686a --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, int32_t &device_id, const bool report_fail) +{ + const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); + bool calibration_valid = false; + bool gyro_valid = false; + + if (exists) { + + uORB::SubscriptionData gyro{ORB_ID(sensor_gyro), instance}; + + gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s); + + if (!gyro_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro #%u", instance); + } + } + + device_id = gyro.get().device_id; + + calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id); + + if (!calibration_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u uncalibrated", instance); + } + } + + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor #%u missing", instance); + } + } + + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status); + + } else if (instance == 1) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status); + } + + return calibration_valid && gyro_valid; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp new file mode 100644 index 0000000000..987389c325 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include + +bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, + const bool report_status) +{ + float test_limit = 1.0f; // pass limit re-used for each test + + // Get sensor_preflight data if available and exit with a fail recorded if not + uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; + sensors_sub.update(); + const sensor_preflight_s &sensors = sensors_sub.get(); + + // Use the difference between IMU's to detect a bad calibration. + // If a single IMU is fitted, the value being checked will be zero so this check will always pass. + param_get(param_find("COM_ARM_IMU_ACC"), &test_limit); + + if (sensors.accel_inconsistency_m_s_s > test_limit) { + if (report_status) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal"); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status); + } + + return false; + + } else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) { + if (report_status) { + mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal"); + } + } + + // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec + param_get(param_find("COM_ARM_IMU_GYR"), &test_limit); + + if (sensors.gyro_inconsistency_rad_s > test_limit) { + if (report_status) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal"); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status); + } + + return false; + + } else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) { + if (report_status) { + mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal"); + } + } + + return true; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp new file mode 100644 index 0000000000..f0c1111932 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include + +// return false if the magnetomer measurements are inconsistent +bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, + const bool report_status) +{ + bool pass = false; // flag for result of checks + + // get the sensor preflight data + uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; + sensors_sub.update(); + const sensor_preflight_s &sensors = sensors_sub.get(); + + if (sensors.timestamp == 0) { + // can happen if not advertised (yet) + pass = true; + } + + // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. + // If a single sensor is fitted, the value being checked will be zero so this check will always pass. + int32_t angle_difference_limit_deg; + param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg); + + pass = pass || angle_difference_limit_deg < 0; // disabled, pass check + pass = pass || sensors.mag_inconsistency_angle < math::radians(angle_difference_limit_deg); + + if (!pass && report_status) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent", + static_cast(math::degrees(sensors.mag_inconsistency_angle))); + mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate"); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); + set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); + } + + return pass; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp new file mode 100644 index 0000000000..bf8b8e8729 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, + const bool optional, int32_t &device_id, const bool report_fail) +{ + const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); + bool calibration_valid = false; + bool mag_valid = false; + + if (exists) { + + uORB::SubscriptionData magnetometer{ORB_ID(sensor_mag), instance}; + + mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s); + + if (!mag_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance); + } + } + + device_id = magnetometer.get().device_id; + + calibration_valid = check_calibration("CAL_MAG%u_ID", device_id); + + if (!calibration_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance); + } + } + + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor #%u missing", instance); + } + } + + const bool success = calibration_valid && mag_valid; + + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status); + + } else if (instance == 1) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status); + } + + return success; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp new file mode 100644 index 0000000000..0bafb71967 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include +#include + +using namespace time_literals; + +bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, + const bool prearm) +{ + bool success = true; + + if (!prearm) { + // Ignore power check after arming. + return true; + + } else { + uORB::SubscriptionData system_power_sub{ORB_ID(system_power)}; + system_power_sub.update(); + const system_power_s &system_power = system_power_sub.get(); + + if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { + + /* copy avionics voltage */ + float avionics_power_rail_voltage = system_power.voltage5v_v; + + // avionics rail + // Check avionics rail voltages + if (avionics_power_rail_voltage < 4.5f) { + success = false; + + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", + (double)avionics_power_rail_voltage); + } + + } else if (avionics_power_rail_voltage < 4.9f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); + } + + } else if (avionics_power_rail_voltage > 5.4f) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); + } + } + } + } + + return success; +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp new file mode 100644 index 0000000000..d5030bae07 --- /dev/null +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "../PreFlightCheck.hpp" + +#include +#include +#include + +bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const safety_s &safety, const uint8_t arm_requirements) +{ + bool prearm_ok = true; + + // USB not connected + if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe"); + + prearm_ok = false; + } + + // battery and system power status + if (!status_flags.circuit_breaker_engaged_power_check) { + + // Fail transition if power is not good + if (!status_flags.condition_power_input_valid) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); + + prearm_ok = false; + } + + // main battery level + if (!status_flags.condition_battery_healthy) { + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); + } + + prearm_ok = false; + } + } + + // Arm Requirements: mission + if (arm_requirements & ARM_REQ_MISSION_BIT) { + + if (!status_flags.condition_auto_mission_available) { + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); + } + + prearm_ok = false; + } + + if (!status_flags.condition_global_position_valid) { + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); + } + + prearm_ok = false; + } + } + + // Arm Requirements: global position + if (arm_requirements & ARM_REQ_GPS_BIT) { + + if (!status_flags.condition_global_position_valid) { + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); + } + + prearm_ok = false; + } + } + + // safety button + if (safety.safety_switch_available && !safety.safety_off) { + // Fail transition if we need safety switch press + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); + } + + prearm_ok = false; + } + + if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) { + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); + } + + prearm_ok = false; + + } + + if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) { + if (prearm_ok) { + mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); + prearm_ok = false; + } + } + + // Arm Requirements: authorization + // check last, and only if everything else has passed + if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) { + if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) { + // feedback provided in arm_auth_check + prearm_ok = false; + } + } + + + return prearm_ok; +} diff --git a/src/modules/commander/rc_check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp similarity index 93% rename from src/modules/commander/rc_check.cpp rename to src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp index ace9176fac..26bee53d76 100644 --- a/src/modules/commander/rc_check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 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 @@ -31,30 +31,28 @@ * ****************************************************************************/ -/** - * @file rc_check.c - * - * RC calibration check - */ - -#include "rc_check.h" - -#include -#include - -#include -#include -#include - -#include +#include "../PreFlightCheck.hpp" #include #include -#include +#include -#define RC_INPUT_MAP_UNMAPPED 0 +/** + * Maximum deadzone value + */ +#define RC_INPUT_MAX_DEADZONE_US 500 -int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL) +/** + * Minimum value + */ +#define RC_INPUT_LOWEST_MIN_US 500 + +/** + * Maximum value + */ +#define RC_INPUT_HIGHEST_MAX_US 2500 + +int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL) { char nbuf[20]; param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, @@ -92,7 +90,6 @@ int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail, bool i } } - /* first check channel mappings */ while (rc_map_mandatory[j] != nullptr) { diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index d90db86817..2dbbbf5043 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -32,6 +32,7 @@ ############################################################################ add_subdirectory(failure_detector) +add_subdirectory(Arming) px4_add_module( MODULE modules__commander @@ -40,18 +41,14 @@ px4_add_module( SRCS accelerometer_calibration.cpp airspeed_calibration.cpp - arm_auth.cpp baro_calibration.cpp calibration_routines.cpp Commander.cpp commander_helper.cpp esc_calibration.cpp gyro_calibration.cpp - health_flag_helper.cpp mag_calibration.cpp - PreflightCheck.cpp rc_calibration.cpp - rc_check.cpp state_machine_helper.cpp DEPENDS circuit_breaker @@ -60,6 +57,9 @@ px4_add_module( git_ecl ecl_geo hysteresis + PreFlightCheck + ArmAuthorization + HealthFlags ) if(PX4_TESTING) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7a36d63307..3582db6f38 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -45,20 +45,20 @@ #include "Commander.hpp" /* commander module headers */ +#include "Arming/PreFlightCheck/PreFlightCheck.hpp" +#include "Arming/ArmAuthorization/ArmAuthorization.h" +#include "Arming/HealthFlags/HealthFlags.h" #include "accelerometer_calibration.h" #include "airspeed_calibration.h" -#include "arm_auth.h" #include "baro_calibration.h" #include "calibration_routines.h" #include "commander_helper.h" #include "esc_calibration.h" #include "gyro_calibration.h" #include "mag_calibration.h" -#include "PreflightCheck.h" #include "px4_custom_mode.h" #include "rc_calibration.h" #include "state_machine_helper.h" -#include "health_flag_helper.h" /* PX4 headers */ #include @@ -156,7 +156,7 @@ static struct vehicle_status_flags_s status_flags = {}; static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost -static uint8_t arm_requirements = ARM_REQ_NONE; +static uint8_t arm_requirements = PreFlightCheck::ARM_REQ_NONE; static bool _last_condition_local_altitude_valid = false; static bool _last_condition_local_position_valid = false; @@ -374,7 +374,7 @@ int commander_main(int argc, char *argv[]) bool preflight_check_res = Commander::preflight_check(true); PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); - bool prearm_check_res = prearm_check(&mavlink_log_pub, status_flags, safety, arm_requirements); + bool prearm_check_res = PreFlightCheck::preArmCheck(&mavlink_log_pub, status_flags, safety, arm_requirements); PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); return 0; @@ -1392,15 +1392,17 @@ Commander::run() int32_t arm_without_gps_param = 0; param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; + arm_requirements = (arm_without_gps_param == 1) ? PreFlightCheck::ARM_REQ_NONE : PreFlightCheck::ARM_REQ_GPS_BIT; int32_t arm_mission_required_param = 0; param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); + arm_requirements |= (arm_mission_required_param & + (PreFlightCheck::ARM_REQ_MISSION_BIT | PreFlightCheck::ARM_REQ_ARM_AUTH_BIT)); int32_t arm_escs_checks_required_param = 0; param_get(_param_escs_checks_required, &arm_escs_checks_required_param); - arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT; + arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : + PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT; status.rc_input_mode = rc_in_off; @@ -1528,11 +1530,13 @@ Commander::run() param_get(_param_arm_switch_is_button, &arm_switch_is_button); param_get(_param_arm_without_gps, &arm_without_gps_param); - arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT; + arm_requirements = (arm_without_gps_param == 1) ? PreFlightCheck::ARM_REQ_NONE : PreFlightCheck::ARM_REQ_GPS_BIT; param_get(_param_arm_mission_required, &arm_mission_required_param); - arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT)); + arm_requirements |= (arm_mission_required_param & + (PreFlightCheck::ARM_REQ_MISSION_BIT | PreFlightCheck::ARM_REQ_ARM_AUTH_BIT)); param_get(_param_escs_checks_required, &arm_escs_checks_required_param); - arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT; + arm_requirements |= (arm_escs_checks_required_param == 0) ? PreFlightCheck::ARM_REQ_NONE : + PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT; /* flight mode slots */ @@ -3755,9 +3759,9 @@ void Commander::mission_init() bool Commander::preflight_check(bool report) { - const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); + const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT); - const bool success = Preflight::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, + const bool success = PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, checkGNSS, report, false, hrt_elapsed_time(&commander_boot_timestamp)); if (success) { diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp deleted file mode 100644 index 9e92bafe24..0000000000 --- a/src/modules/commander/PreflightCheck.cpp +++ /dev/null @@ -1,990 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2012-2017 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 -*/ - -#include "PreflightCheck.h" -#include "health_flag_helper.h" -#include "rc_check.h" - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -namespace Preflight -{ - -static bool check_calibration(const char *param_template, const int32_t device_id) -{ - bool calibration_found = false; - - char s[20]; - int instance = 0; - - /* old style transition: check param values */ - while (!calibration_found) { - sprintf(s, param_template, instance); - const param_t parm = param_find_no_notification(s); - - /* if the calibration param is not present, abort */ - if (parm == PARAM_INVALID) { - break; - } - - /* if param get succeeds */ - int32_t calibration_devid = -1; - - if (param_get(parm, &calibration_devid) == PX4_OK) { - - /* if the devid matches, exit early */ - if (device_id == calibration_devid) { - calibration_found = true; - break; - } - } - - instance++; - } - - return calibration_found; -} - -static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); - bool calibration_valid = false; - bool mag_valid = false; - - if (exists) { - - uORB::SubscriptionData magnetometer{ORB_ID(sensor_mag), instance}; - - mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s); - - if (!mag_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass #%u", instance); - } - } - - device_id = magnetometer.get().device_id; - - calibration_valid = check_calibration("CAL_MAG%u_ID", device_id); - - if (!calibration_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u uncalibrated", instance); - } - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor #%u missing", instance); - } - } - - const bool success = calibration_valid && mag_valid; - - if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status); - - } else if (instance == 1) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status); - } - - return success; -} - -static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status) -{ - float test_limit = 1.0f; // pass limit re-used for each test - - // Get sensor_preflight data if available and exit with a fail recorded if not - uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; - sensors_sub.update(); - const sensor_preflight_s &sensors = sensors_sub.get(); - - // Use the difference between IMU's to detect a bad calibration. - // If a single IMU is fitted, the value being checked will be zero so this check will always pass. - param_get(param_find("COM_ARM_IMU_ACC"), &test_limit); - - if (sensors.accel_inconsistency_m_s_s > test_limit) { - if (report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal"); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status); - } - - return false; - - } else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) { - if (report_status) { - mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal"); - } - } - - // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec - param_get(param_find("COM_ARM_IMU_GYR"), &test_limit); - - if (sensors.gyro_inconsistency_rad_s > test_limit) { - if (report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal"); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status); - } - - return false; - - } else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) { - if (report_status) { - mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal"); - } - } - - return true; -} - -// return false if the magnetomer measurements are inconsistent -static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status) -{ - bool pass = false; // flag for result of checks - - // get the sensor preflight data - uORB::SubscriptionData sensors_sub{ORB_ID(sensor_preflight)}; - sensors_sub.update(); - const sensor_preflight_s &sensors = sensors_sub.get(); - - if (sensors.timestamp == 0) { - // can happen if not advertised (yet) - pass = true; - } - - // Use the difference between sensors to detect a bad calibration, orientation or magnetic interference. - // If a single sensor is fitted, the value being checked will be zero so this check will always pass. - int32_t angle_difference_limit_deg; - param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg); - - pass = pass || angle_difference_limit_deg < 0; // disabled, pass check - pass = pass || sensors.mag_inconsistency_angle < math::radians(angle_difference_limit_deg); - - if (!pass && report_status) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent", - static_cast(math::degrees(sensors.mag_inconsistency_angle))); - mavlink_log_critical(mavlink_log_pub, "Please check orientations and recalibrate"); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); - set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); - } - - return pass; -} - -static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); - bool calibration_valid = false; - bool accel_valid = true; - - if (exists) { - - uORB::SubscriptionData accel{ORB_ID(sensor_accel), instance}; - - accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s); - - if (!accel_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel #%u", instance); - } - } - - device_id = accel.get().device_id; - - calibration_valid = check_calibration("CAL_ACC%u_ID", device_id); - - if (!calibration_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance); - } - - } else { - - if (dynamic) { - const float accel_magnitude = sqrtf(accel.get().x * accel.get().x - + accel.get().y * accel.get().y - + accel.get().z * accel.get().z); - - if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming"); - } - - /* this is frickin' fatal */ - accel_valid = false; - } - } - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor #%u missing", instance); - } - } - - const bool success = calibration_valid && accel_valid; - - if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status); - - } else if (instance == 1) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status); - } - - return success; -} - -static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); - bool calibration_valid = false; - bool gyro_valid = false; - - if (exists) { - - uORB::SubscriptionData gyro{ORB_ID(sensor_gyro), instance}; - - gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s); - - if (!gyro_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro #%u", instance); - } - } - - device_id = gyro.get().device_id; - - calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id); - - if (!calibration_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u uncalibrated", instance); - } - } - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor #%u missing", instance); - } - } - - if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status); - - } else if (instance == 1) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status); - } - - return calibration_valid && gyro_valid; -} - -static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, - const bool optional, int32_t &device_id, const bool report_fail) -{ - const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK); - bool baro_valid = false; - - if (exists) { - uORB::SubscriptionData baro{ORB_ID(sensor_baro), instance}; - - baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s); - - if (!baro_valid) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro #%u", instance); - } - } - - - } else { - if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor #%u missing", instance); - } - } - - if (instance == 0) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status); - } - - return baro_valid; -} - -static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, - const bool report_fail, const bool prearm) -{ - bool present = true; - bool success = true; - - uORB::SubscriptionData airspeed_sub{ORB_ID(airspeed)}; - airspeed_sub.update(); - const airspeed_s &airspeed = airspeed_sub.get(); - - if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) { - if (report_fail && !optional) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing"); - } - - present = false; - success = false; - goto out; - } - - /* - * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise - * on the bench and trigger false positives of the voter. Therefore only fail this - * for a pre-arm check, as then the cover is off and the natural airflow in the field - * will ensure there is not zero noise. - */ - if (prearm && fabsf(airspeed.confidence) < 0.95f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck"); - } - - present = true; - success = false; - goto out; - } - - /** - * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying - * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover - * might have been removed. - */ - if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && !prearm) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot"); - } - - present = true; - success = false; - goto out; - } - -out: - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status); - - return success; -} - -static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, - const bool prearm) -{ - bool success = true; - - if (!prearm) { - // Ignore power check after arming. - return true; - - } else { - uORB::SubscriptionData system_power_sub{ORB_ID(system_power)}; - system_power_sub.update(); - const system_power_s &system_power = system_power_sub.get(); - - if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { - - /* copy avionics voltage */ - float avionics_power_rail_voltage = system_power.voltage5v_v; - - // avionics rail - // Check avionics rail voltages - if (avionics_power_rail_voltage < 4.5f) { - success = false; - - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", - (double)avionics_power_rail_voltage); - } - - } else if (avionics_power_rail_voltage < 4.9f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); - } - - } else if (avionics_power_rail_voltage > 5.4f) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); - } - } - } - } - - return success; -} - -static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, - const bool report_fail, const bool enforce_gps_required) -{ - bool success = true; // start with a pass and change to a fail if any test fails - bool present = true; - float test_limit = 1.0f; // pass limit re-used for each test - - bool gps_success = true; - bool gps_present = true; - - // Get estimator status data if available and exit with a fail recorded if not - uORB::SubscriptionData status_sub{ORB_ID(estimator_status)}; - status_sub.update(); - const estimator_status_s &status = status_sub.get(); - - if (status.timestamp == 0) { - present = false; - goto out; - } - - // Check if preflight check performed by estimator has failed - if (status.pre_flt_fail_innov_heading || - status.pre_flt_fail_innov_vel_horiz || - status.pre_flt_fail_innov_vel_vert || - status.pre_flt_fail_innov_height) { - if (report_fail) { - if (status.pre_flt_fail_innov_heading) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable"); - - } else if (status.pre_flt_fail_innov_vel_horiz) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable"); - - } else if (status.pre_flt_fail_innov_vel_horiz) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable"); - - } else if (status.pre_flt_fail_innov_height) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable"); - } - } - - success = false; - goto out; - } - - // check vertical position innovation test ratio - param_get(param_find("COM_ARM_EKF_HGT"), &test_limit); - - if (status.hgt_test_ratio > test_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error"); - } - - success = false; - goto out; - } - - // check velocity innovation test ratio - param_get(param_find("COM_ARM_EKF_VEL"), &test_limit); - - if (status.vel_test_ratio > test_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error"); - } - - success = false; - goto out; - } - - // check horizontal position innovation test ratio - param_get(param_find("COM_ARM_EKF_POS"), &test_limit); - - if (status.pos_test_ratio > test_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error"); - } - - success = false; - goto out; - } - - // check magnetometer innovation test ratio - param_get(param_find("COM_ARM_EKF_YAW"), &test_limit); - - if (status.mag_test_ratio > test_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error"); - } - - success = false; - goto out; - } - - // check accelerometer delta velocity bias estimates - param_get(param_find("COM_ARM_EKF_AB"), &test_limit); - - for (uint8_t index = 13; index < 16; index++) { - // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives - // adjust test threshold by 3-sigma - float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f)); - - if (fabsf(status.states[index]) > test_limit + test_uncertainty) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); - } - - success = false; - goto out; - } - } - - // check gyro delta angle bias estimates - param_get(param_find("COM_ARM_EKF_GB"), &test_limit); - - if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit - || fabsf(status.states[12]) > test_limit) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias"); - } - - success = false; - goto out; - } - - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (enforce_gps_required || report_fail) { - const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS); - const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0; - - gps_success = ekf_gps_fusion; // default to success if gps data is fused - - if (ekf_gps_check_fail) { - if (report_fail) { - // Only report the first failure to avoid spamming - const char *message = nullptr; - - if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { - message = "Preflight%s: GPS fix too low"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { - message = "Preflight%s: not enough GPS Satellites"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) { - message = "Preflight%s: GPS GDoP too low"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { - message = "Preflight%s: GPS Horizontal Pos Error too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { - message = "Preflight%s: GPS Vertical Pos Error too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { - message = "Preflight%s: GPS Speed Accuracy too low"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { - message = "Preflight%s: GPS Horizontal Pos Drift too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { - message = "Preflight%s: GPS Vertical Pos Drift too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { - message = "Preflight%s: GPS Hor Speed Drift too high"; - - } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { - message = "Preflight%s: GPS Vert Speed Drift too high"; - - } else { - if (!ekf_gps_fusion) { - // Likely cause unknown - message = "Preflight%s: Estimator not using GPS"; - gps_present = false; - - } else { - // if we land here there was a new flag added and the code not updated. Show a generic message. - message = "Preflight%s: Poor GPS Quality"; - } - } - - if (message) { - if (enforce_gps_required) { - mavlink_log_critical(mavlink_log_pub, message, " Fail"); - - } else { - mavlink_log_warning(mavlink_log_pub, message, ""); - } - } - } - - gps_success = false; - - if (enforce_gps_required) { - success = false; - goto out; - } - } - } - -out: - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status); - - return success; -} - -static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, - const bool prearm) -{ - // Ignore failure detector check after arming - if (!prearm) { - return true; - } - - if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) { - if (report_fail) { - if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected"); - } - } - - return false; - } - - return true; -} - -bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, - const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot) -{ - if (time_since_boot < 2_s) { - // 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); - const bool checkFailureDetector = true; - - 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.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || status.is_vtol)) { - checkAirspeed = true; - } - - reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout - && !status_flags.condition_calibration_enabled); - - bool failed = false; - - /* ---- MAG ---- */ - if (checkSensors) { - bool prime_found = false; - - int32_t prime_id = -1; - 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 individually, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_mag_count; i++) { - const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1); - const bool report_fail = (reportFailures && !failed && !mag_fail_reported); - - int32_t device_id = -1; - - if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { - - if ((prime_id > 0) && (device_id == prime_id)) { - prime_found = true; - } - - } else { - if (required) { - failed = true; - mag_fail_reported = true; - } - } - } - - if (sys_has_mag == 1) { - /* check if the primary device is present */ - if (!prime_found) { - if (reportFailures && !failed) { - mavlink_log_critical(mavlink_log_pub, "Primary compass not found"); - } - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status); - failed = true; - } - - /* mag consistency checks (need to be performed after the individual checks) */ - if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) { - failed = true; - } - } - } - - /* ---- ACCEL ---- */ - if (checkSensors) { - bool prime_found = false; - int32_t prime_id = -1; - param_get(param_find("CAL_ACC_PRIME"), &prime_id); - - bool accel_fail_reported = false; - - /* check all sensors individually, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_accel_count; i++) { - const bool required = (i < max_mandatory_accel_count); - const bool report_fail = (reportFailures && !failed && !accel_fail_reported); - - int32_t device_id = -1; - - if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) { - - if ((prime_id > 0) && (device_id == prime_id)) { - prime_found = true; - } - - } else { - if (required) { - failed = true; - accel_fail_reported = true; - } - } - } - - /* check if the primary device is present */ - if (!prime_found) { - if (reportFailures && !failed) { - mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found"); - } - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status); - failed = true; - } - } - - /* ---- GYRO ---- */ - if (checkSensors) { - bool prime_found = false; - int32_t prime_id = -1; - param_get(param_find("CAL_GYRO_PRIME"), &prime_id); - - bool gyro_fail_reported = false; - - /* check all sensors individually, but fail only for mandatory ones */ - for (unsigned i = 0; i < max_optional_gyro_count; i++) { - const bool required = (i < max_mandatory_gyro_count); - const bool report_fail = (reportFailures && !failed && !gyro_fail_reported); - - int32_t device_id = -1; - - if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { - - if ((prime_id > 0) && (device_id == prime_id)) { - prime_found = true; - } - - } else { - if (required) { - failed = true; - gyro_fail_reported = true; - } - } - } - - /* check if the primary device is present */ - if (!prime_found) { - if (reportFailures && !failed) { - mavlink_log_critical(mavlink_log_pub, "Primary gyro not found"); - } - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status); - failed = true; - } - } - - /* ---- BARO ---- */ - if (checkSensors) { - //bool prime_found = false; - - int32_t prime_id = -1; - 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++) { - const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1); - const bool report_fail = (reportFailures && !failed && !baro_fail_reported); - - int32_t device_id = -1; - - if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { - if ((prime_id > 0) && (device_id == prime_id)) { - //prime_found = true; - } - - } else { - if (required) { - failed = true; - baro_fail_reported = true; - } - } - } - - // TODO there is no logic in place to calibrate the primary baro yet - // // check if the primary device is present - // if (false) { - // if (reportFailures && !failed) { - // mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational"); - // } - - // set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status); - // failed = true; - // } - } - - /* ---- IMU CONSISTENCY ---- */ - // To be performed after the individual sensor checks have completed - if (checkSensors) { - if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) { - failed = true; - } - } - - /* ---- AIRSPEED ---- */ - if (checkAirspeed) { - int32_t optional = 0; - param_get(param_find("FW_ARSP_MODE"), &optional); - - if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) { - 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; - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status); - status_flags.rc_calibration_valid = false; - - } else { - // The calibration is fine, but only set the overall health state to true if the signal is not currently lost - status_flags.rc_calibration_valid = true; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, - !status.rc_signal_lost, status); - } - } - - /* ---- SYSTEM POWER ---- */ - if (checkPower) { - if (!powerCheck(mavlink_log_pub, status, (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 = -1; - - if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !status.is_vtol) { - param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type); - - } else { - // EKF2 is currently the only supported option for FW & VTOL - estimator_type = 2; - } - - 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_s); - - if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) { - failed = true; - } - } - - /* ---- Failure Detector ---- */ - if (checkFailureDetector) { - if (!failureDetectorCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) { - failed = true; - } - } - - /* Report status */ - return !failed; -} - -} diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h deleted file mode 100644 index 9962ee98d9..0000000000 --- a/src/modules/commander/PreflightCheck.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * 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.h - * - * Preflight check for main system components - * - * @author Johan Jansen - */ - -#include -#include -#include - -#pragma once - -namespace Preflight -{ -/** -* 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_log_pub -* Mavlink output orb handle reference 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 checkAirspeed -* true if the airspeed sensor should be checked -* @param checkRC -* true if the Remote Controller should be checked -* @param checkGNSS -* true if the GNSS receiver should be checked -* @param checkPower -* true if the system power should be checked -**/ -bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, - const hrt_abstime &time_since_boot); - -static constexpr unsigned max_mandatory_gyro_count = 1; -static constexpr unsigned max_optional_gyro_count = 3; - -static constexpr unsigned max_mandatory_accel_count = 1; -static constexpr unsigned max_optional_accel_count = 3; - -static constexpr unsigned max_mandatory_mag_count = 1; -static constexpr unsigned max_optional_mag_count = 4; - -static constexpr unsigned max_mandatory_baro_count = 1; -static constexpr unsigned max_optional_baro_count = 1; - -} diff --git a/src/modules/commander/commander_tests/CMakeLists.txt b/src/modules/commander/commander_tests/CMakeLists.txt index 692b95f106..64df7b6076 100644 --- a/src/modules/commander/commander_tests/CMakeLists.txt +++ b/src/modules/commander/commander_tests/CMakeLists.txt @@ -37,6 +37,5 @@ px4_add_module( commander_tests.cpp state_machine_helper_test.cpp ../state_machine_helper.cpp - ../PreflightCheck.cpp DEPENDS ) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a40854cc2..73c04af69e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -42,6 +42,7 @@ #include "../state_machine_helper.h" #include +#include "../Arming/PreFlightCheck/PreFlightCheck.hpp" class StateMachineHelperTest : public UnitTest { @@ -304,7 +305,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() true /* enable pre-arm checks */, nullptr /* no mavlink_log_pub */, &status_flags, - (check_gps ? ARM_REQ_GPS_BIT : 0), + (check_gps ? PreFlightCheck::ARM_REQ_GPS_BIT : 0), 2e6 /* 2 seconds after boot, everything should be checked */ ); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fd20ad94ed..219085789b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,11 +46,9 @@ #include #include - +#include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "state_machine_helper.h" #include "commander_helper.h" -#include "PreflightCheck.h" -#include "arm_auth.h" static constexpr const char reason_no_rc[] = "no RC"; static constexpr const char reason_no_offboard[] = "no offboard"; @@ -132,13 +130,13 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe */ bool preflight_check_ret = true; - const bool checkGNSS = (arm_requirements & ARM_REQ_GPS_BIT); + const bool checkGNSS = (arm_requirements & PreFlightCheck::ARM_REQ_GPS_BIT); /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !hil_enabled) { - preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, + preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot); if (preflight_check_ret) { @@ -157,7 +155,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { - status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, + status_flags->condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, + *status_flags, checkGNSS, false, false, time_since_boot); last_preflight_check = hrt_absolute_time(); @@ -179,7 +178,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe if (fRunPreArmChecks && preflight_check_ret) { // only bother running prearm if preflight was successful - prearm_check_ret = prearm_check(mavlink_log_pub, *status_flags, safety, arm_requirements); + prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, *status_flags, safety, arm_requirements); } if (!preflight_check_ret || !prearm_check_ret) { @@ -1009,115 +1008,8 @@ void reset_offboard_loss_globals(actuator_armed_s *armed, const bool old_failsaf } } -bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, - const uint8_t arm_requirements) -{ - bool reportFailures = true; - bool prearm_ok = true; - // USB not connected - if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { - if (reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe"); - } - - prearm_ok = false; - } - - // battery and system power status - if (!status_flags.circuit_breaker_engaged_power_check) { - - // Fail transition if power is not good - if (!status_flags.condition_power_input_valid) { - if (reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); - } - - prearm_ok = false; - } - - // main battery level - if (!status_flags.condition_battery_healthy) { - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); - } - - prearm_ok = false; - } - } - - // Arm Requirements: mission - if (arm_requirements & ARM_REQ_MISSION_BIT) { - - if (!status_flags.condition_auto_mission_available) { - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); - } - - prearm_ok = false; - } - - if (!status_flags.condition_global_position_valid) { - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); - } - - prearm_ok = false; - } - } - - // Arm Requirements: global position - if (arm_requirements & ARM_REQ_GPS_BIT) { - - if (!status_flags.condition_global_position_valid) { - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); - } - - prearm_ok = false; - } - } - - // safety button - if (safety.safety_switch_available && !safety.safety_off) { - // Fail transition if we need safety switch press - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); - } - - prearm_ok = false; - } - - if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) { - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); - } - - prearm_ok = false; - - } - - if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) { - if (prearm_ok && reportFailures) { - mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); - prearm_ok = false; - } - } - - // Arm Requirements: authorization - // check last, and only if everything else has passed - if ((arm_requirements & ARM_REQ_ARM_AUTH_BIT) && prearm_ok) { - if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) { - // feedback provided in arm_auth_check - prearm_ok = false; - } - } - - - return prearm_ok; -} - void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning, diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 3ea6711b91..071afba6d3 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -94,14 +94,6 @@ enum class position_nav_loss_actions_t { LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. }; -typedef enum { - ARM_REQ_NONE = 0, - ARM_REQ_MISSION_BIT = (1 << 0), - ARM_REQ_ARM_AUTH_BIT = (1 << 1), - ARM_REQ_GPS_BIT = (1 << 2), - ARM_REQ_ESCS_CHECK_BIT = (1 << 3) -} arm_requirements_t; - extern const char *const arming_state_names[]; bool is_safe(const safety_s &safety, const actuator_armed_s &armed); @@ -125,15 +117,12 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ const position_nav_loss_actions_t posctl_nav_loss_act); /* - * Checks the validty of position data aaainst the requirements of the current navigation + * Checks the validty of position data against the requirements of the current navigation * mode and switches mode if position data required is not available. */ bool check_invalid_pos_nav_state(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const bool use_rc, const bool using_global_pos); -bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, - const uint8_t arm_requirements); - // COM_LOW_BAT_ACT parameter values typedef enum LOW_BAT_ACTION {