fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 11:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: rework arming checks to use the events interface

Browse Source
This commit is contained in:
Beat Küng
2022-08-25 22:02:15 -04:00
committed by Daniel Agar
parent b4a3597c7d
commit 6d1fb92eb7
70 changed files with 3738 additions and 2423 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
+5 -8
View File
@@ -39,10 +39,8 @@ constexpr bool
ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
arm_disarm_reason_t calling_reason)
const arming_state_t new_arming_state, actuator_armed_s &armed, HealthAndArmingChecks &checks,
const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason)
{
// Double check that our static arrays are still valid
static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
@@ -67,10 +65,9 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
&& !(status.hil_state == vehicle_status_s::HIL_STATE_ON)
&& (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) {
if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
true, // report_failures
safety_button_available, safety_off,
true)) { // is_arm_attempt
checks.update();
if (!checks.canArm(status.nav_state)) {
feedback_provided = true; // Preflight checks report error messages
valid_transition = false;
}
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
+4 -6
View File
@@ -33,7 +33,7 @@
#pragma once
#include "../PreFlightCheck/PreFlightCheck.hpp"
#include "../../HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include <drivers/drv_hrt.h>
#include <px4_platform_common/events.h>
#include <uORB/topics/actuator_armed.h>
@@ -54,11 +54,9 @@ public:
void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }
transition_result_t
arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode,
const bool safety_button_available, const bool safety_off, const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags,
arm_disarm_reason_t calling_reason);
arming_state_transition(vehicle_status_s &status, const arming_state_t new_arming_state,
actuator_armed_s &armed, HealthAndArmingChecks &checks, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason);
// Getters
uint8_t getArmState() const { return _arm_state; }
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
+9 -14
View File
@@ -32,7 +32,7 @@
****************************************************************************/
#include <gtest/gtest.h>
#include <ArmStateMachine.hpp>
#include "ArmStateMachine.hpp"
TEST(ArmStateMachineTest, ArmingStateTransitionTest)
{
@@ -61,12 +61,12 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
#define ATT_SAFETY_AVAILABLE true
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
static constexpr bool ATT_ARMED = true;
static constexpr bool ATT_DISARMED = false;
static constexpr bool ATT_SAFETY_AVAILABLE = true;
static constexpr bool ATT_SAFETY_NOT_AVAILABLE = true;
static constexpr bool ATT_SAFETY_OFF = true;
static constexpr bool ATT_SAFETY_ON = false;
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
@@ -253,22 +253,17 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest)
// Setup initial machine state
arm_state_machine.forceArmState(test->current_state.arming_state);
status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true;
vehicle_control_mode_s control_mode{};
HealthAndArmingChecks health_and_arming_checks(nullptr, status_flags, status);
// Attempt transition
transition_result_t result = arm_state_machine.arming_state_transition(
status,
control_mode,
test->safety_button_available,
test->safety_off,
test->requested_state,
armed,
health_and_arming_checks,
true /* enable pre-arm checks */,
nullptr /* no mavlink_log_pub */,
status_flags,
arm_disarm_reason_t::unit_test);
// Validate result of transition
src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt
+5 -2
View File
@@ -36,6 +36,9 @@ px4_add_library(ArmStateMachine
ArmStateMachine.cpp
)
target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(ArmStateMachine PUBLIC PreFlightCheck)
target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks)
px4_add_functional_gtest(SRC ArmStateMachineTest.cpp
LINKLIBS ArmStateMachine health_and_arming_checks sensor_calibration ArmAuthorization mode_util
)
px4_add_functional_gtest(SRC ArmStateMachineTest.cpp LINKLIBS ArmStateMachine)
src/modules/commander/Arming/CMakeLists.txt
-1
View File
@@ -34,4 +34,3 @@
add_subdirectory(ArmAuthorization)
add_subdirectory(ArmStateMachine)
add_subdirectory(HealthFlags)
add_subdirectory(PreFlightCheck)
src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt
-57
View File
@@ -1,57 +0,0 @@
############################################################################
#
# 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/distanceSensorChecks.cpp
checks/rcCalibrationCheck.cpp
checks/powerCheck.cpp
checks/airframeCheck.cpp
checks/ekf2Check.cpp
checks/failureDetectorCheck.cpp
checks/manualControlCheck.cpp
checks/modeCheck.cpp
checks/cpuResourceCheck.cpp
checks/sdcardCheck.cpp
checks/parachuteCheck.cpp
)
target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
-242
View File
@@ -1,242 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 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 <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
using namespace time_literals;
static constexpr unsigned max_mandatory_mag_count = 1;
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_mandatory_baro_count = 1;
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
bool report_failures,
const bool safety_button_available, const bool safety_off,
const bool is_arm_attempt)
{
report_failures = (report_failures && !status_flags.calibration_enabled);
bool failed = false;
failed = failed || !airframeCheck(mavlink_log_pub, status);
failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures);
/* ---- MAG ---- */
{
int32_t sys_has_mag = 1;
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
if (sys_has_mag == 1) {
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count,
mavlink_log_pub, status, magnetometerCheck);
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
}
}
/* ---- ACCEL ---- */
{
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count,
mavlink_log_pub, status, accelerometerCheck);
// TODO: highest priority (from params)
}
/* ---- GYRO ---- */
{
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count,
mavlink_log_pub, status, gyroCheck);
// TODO: highest priority (from params)
}
/* ---- BARO ---- */
{
int32_t sys_has_baro = 1;
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
if (sys_has_baro == 1) {
static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count,
mavlink_log_pub, status, baroCheck));
}
}
/* ---- IMU CONSISTENCY ---- */
// To be performed after the individual sensor checks have completed
{
if (!imuConsistencyCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
}
/* ---- Distance Sensor ---- */
{
int32_t sys_has_num_dist_sens = 0;
param_get(param_find("SYS_HAS_NUM_DIST"), &sys_has_num_dist_sens);
if (sys_has_num_dist_sens > 0) {
static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens,
mavlink_log_pub, status, distSensCheck));
}
}
/* ---- AIRSPEED ---- */
/* 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)) {
int32_t airspeed_mode = 0;
param_get(param_find("FW_ARSP_MODE"), &airspeed_mode);
const bool optional = (airspeed_mode == 1);
int32_t max_airspeed_check_en = 0;
param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);
float airspeed_trim = 10.0f;
param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim);
const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, is_arm_attempt, (bool)max_airspeed_check_en,
arming_max_airspeed_allowed)
&& !(bool)optional) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
int32_t com_rc_in_mode{0};
param_get(param_find("COM_RC_IN_MODE"), &com_rc_in_mode);
if (com_rc_in_mode == 0) {
if (rcCalibrationCheck(mavlink_log_pub, report_failures) != OK) {
if (report_failures) {
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 (status_flags.power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
if (!powerCheck(mavlink_log_pub, status, report_failures)) {
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) {
const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) &&
ekf2CheckSensorBias(mavlink_log_pub, report_failures);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status);
if (control_mode.flag_control_attitude_enabled
|| control_mode.flag_control_velocity_enabled
|| control_mode.flag_control_position_enabled) {
// healthy estimator only required for dependent control modes
failed |= !ekf_healthy;
}
}
/* ---- Failure Detector ---- */
if (!failureDetectorCheck(mavlink_log_pub, status, report_failures)) {
failed = true;
}
failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags);
failed = failed || !preArmCheck(mavlink_log_pub, status_flags, control_mode,
safety_button_available, safety_off, status, report_failures, is_arm_attempt);
/* Report status */
return !failed;
}
bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure,
const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check)
{
bool pass_check = true;
bool report_fail = report_failure;
/* check all sensors, but fail only for mandatory ones */
for (uint8_t i = 0u; i < ORB_MULTI_MAX_INSTANCES; i++) {
const bool is_mandatory = i < nb_mandatory_instances;
if (!sens_check(mavlink_log_pub, status, i, is_mandatory, report_fail)) {
pass_check = false;
}
}
return pass_check;
}
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
-109
View File
@@ -1,109 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 - 2022 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 <uORB/uORB.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
#include "../../Safety.hpp"
typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail);
class PreFlightCheck
{
public:
PreFlightCheck() = default;
~PreFlightCheck() = default;
/**
* Runs a preflight check to determine if the system is ready to be armed
*
* @param mavlink_log_pub
* Mavlink output orb handle reference for feedback when a sensor fails
**/
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
bool reportFailures,
const bool safety_button_available, const bool safety_off,
const bool is_arm_attempt = false);
private:
static bool sensorAvailabilityCheck(const bool report_failure,
const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check);
static bool isMagRequired(uint8_t instance);
static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail);
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
static bool isAccelRequired(uint8_t instance);
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail);
static bool isGyroRequired(uint8_t instance);
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail);
static bool isBaroRequired(uint8_t instance);
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail);
static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, 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 is_arm_attempt, const bool max_airspeed_check_en,
const float arming_max_airspeed_allowed);
static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail);
static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail);
static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail);
static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status);
static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
const vehicle_status_flags_s &status_flags);
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
vehicle_status_s &status, const bool report_fail, const bool is_arm_attempt);
};
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
-121
View File
@@ -1,121 +0,0 @@
/****************************************************************************
*
* 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 <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <math.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_accel.h>
using namespace time_literals;
bool PreFlightCheck::isAccelRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
const uint32_t device_id = static_cast<uint32_t>(accel.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().accel_device_id == device_id) {
is_used_by_nav = true;
break;
}
}
return is_used_by_nav;
}
bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
const bool is_required = is_mandatory || isAccelRequired(instance);
bool is_valid = false;
bool is_calibration_valid = false;
bool is_value_valid = false;
if (exists) {
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0)
&& (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
is_calibration_valid = true;
} else {
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0);
}
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 */) {
is_value_valid = true;
}
}
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
report_fail = false;
} else if (!is_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
report_fail = false;
} else if (!is_calibration_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
report_fail = false;
} else if (!is_value_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
report_fail = false;
}
}
const bool is_sensor_ok = is_valid && is_calibration_valid && is_value_valid;
return is_sensor_ok || !is_required;
}
src/modules/commander/Arming/PreFlightCheck/checks/airframeCheck.cpp
-58
View File
@@ -1,58 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
using namespace time_literals;
bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status)
{
bool success = true;
#ifdef CONFIG_ARCH_BOARD_PX4_FMU_V2
// We no longer support VTOL on fmu-v2, so we need to warn existing users.
if (status.is_vtol) {
mavlink_log_critical(mavlink_log_pub,
"VTOL is not supported with fmu-v2, see docs.px4.io/main/en/config/firmware.html#bootloader");
success = false;
}
#endif
return success;
}
src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp
-102
View File
@@ -1,102 +0,0 @@
/****************************************************************************
*
* 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 <HealthFlags.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed_validated.h>
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 is_arm_attempt, const bool max_airspeed_check_en,
const float arming_max_airspeed_allowed)
{
bool present = true;
bool success = true;
uORB::SubscriptionData<airspeed_validated_s> airspeed_validated_sub{ORB_ID(airspeed_validated)};
airspeed_validated_sub.update();
const airspeed_validated_s &airspeed_validated = airspeed_validated_sub.get();
/*
* Check if Airspeed Selector is up and running.
*/
if (hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
if (report_fail && !optional) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Selector module down.");
}
present = false;
success = false;
goto out;
}
/*
* Check if airspeed is declared valid or not by airspeed selector.
*/
if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed invalid.");
}
present = true;
success = false;
goto out;
}
/*
* Check if airspeed is higher than maximally accepted 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 (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed
&& !is_arm_attempt) {
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;
}
src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp
-95
View File
@@ -1,95 +0,0 @@
/****************************************************************************
*
* 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 <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_baro.h>
using namespace time_literals;
bool PreFlightCheck::isBaroRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
const uint32_t device_id = static_cast<uint32_t>(baro.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().baro_device_id == device_id) {
is_used_by_nav = true;
break;
}
}
return is_used_by_nav;
}
bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
const bool is_required = is_mandatory || isBaroRequired(instance);
bool valid = false;
if (exists) {
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0) && (hrt_elapsed_time(&baro.get().timestamp) < 1_s);
}
if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, is_required, valid, status);
}
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance);
report_fail = false;
} else if (!valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance);
report_fail = false;
}
}
return valid || !is_required;
}
src/modules/commander/Arming/PreFlightCheck/checks/cpuResourceCheck.cpp
-76
View File
@@ -1,76 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <lib/parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/cpuload.h>
using namespace time_literals;
bool PreFlightCheck::cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
bool success = true;
uORB::SubscriptionData<cpuload_s> cpuload_sub{ORB_ID(cpuload)};
cpuload_sub.update();
float cpuload_percent_max;
param_get(param_find("COM_CPU_MAX"), &cpuload_percent_max);
if (cpuload_percent_max > 0.f) {
if (hrt_elapsed_time(&cpuload_sub.get().timestamp) > 2_s) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Fail: No CPU load information");
}
}
const float cpuload_percent = cpuload_sub.get().load * 100.f;
if (cpuload_percent > cpuload_percent_max) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
}
}
}
return success;
}
src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp
-71
View File
@@ -1,71 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2022 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 <drivers/drv_hrt.h>
#include <px4_defines.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/distance_sensor.h>
using namespace time_literals;
bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK);
bool valid = false;
if (exists) {
uORB::SubscriptionData<distance_sensor_s> dist_sens_sub{ORB_ID(distance_sensor), instance};
dist_sens_sub.update();
const distance_sensor_s &dist_sens_data = dist_sens_sub.get();
valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
}
if (report_fail && is_mandatory) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: distance sensor %u missing", instance);
report_fail = false;
} else if (!valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
report_fail = false;
}
}
return valid || !is_mandatory;
}
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
-297
View File
@@ -1,297 +0,0 @@
/****************************************************************************
*
* 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 <HealthFlags.h>
#include <math.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_selector_status.h>
#include <uORB/topics/estimator_sensor_bias.h>
#include <uORB/topics/estimator_status.h>
using namespace time_literals;
bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail)
{
bool success = true; // start with a pass and change to a fail if any test fails
int32_t mag_strength_check = 1;
param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);
float hgt_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
float vel_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit);
float pos_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit);
float mag_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
int32_t arm_without_gps = 0;
param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
int32_t sys_has_gps = 1;
param_get(param_find("SYS_HAS_GPS"), &sys_has_gps);
bool gps_success = false;
bool gps_present = false;
// Get estimator status data if available and exit with a fail recorded if not
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
const estimator_status_s &status = status_sub.get();
if (status.timestamp == 0) {
success = 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_vert) {
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;
}
if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
const char *message = "Preflight%s: Strong magnetic interference detected";
if (mag_strength_check == 1) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, message, " Fail");
}
success = false;
goto out;
} else if (report_fail) {
mavlink_log_warning(mavlink_log_pub, message, "");
}
}
// check vertical position innovation test ratio
if (status.hgt_test_ratio > hgt_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
}
success = false;
goto out;
}
// check velocity innovation test ratio
if (status.vel_test_ratio > vel_test_ratio_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
if (status.pos_test_ratio > pos_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
}
success = false;
goto out;
}
// check magnetometer innovation test ratio
if (status.mag_test_ratio > mag_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
}
success = false;
goto out;
}
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
if (sys_has_gps == 1) {
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_present = true;
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_MAX_PDOP)) {
message = "Preflight%s: GPS PDOP too high";
} 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 (!arm_without_gps) {
mavlink_log_critical(mavlink_log_pub, message, " Fail");
} else {
mavlink_log_warning(mavlink_log_pub, message, "");
}
}
}
gps_success = false;
if (!arm_without_gps) {
success = false;
goto out;
}
}
}
out:
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status);
return success;
}
bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
// Get estimator states data if available and exit with a fail recorded if not
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::SubscriptionData<estimator_sensor_bias_s> estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance};
const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get();
if (hrt_elapsed_time(&bias.timestamp) < 30_s) {
// check accelerometer bias estimates
if (bias.accel_bias_valid) {
const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
// adjust test threshold by 3-sigma
const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f));
if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
if (report_fail) {
PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
(double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
}
return false;
}
}
}
// check gyro bias estimates
if (bias.gyro_bias_valid) {
const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
// adjust test threshold by 3-sigma
const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f));
if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
if (report_fail) {
PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
(double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
}
return false;
}
}
}
}
return true;
}
src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp
-68
View File
@@ -1,68 +0,0 @@
/****************************************************************************
*
* 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 <systemlib/mavlink_log.h>
bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const bool report_fail)
{
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");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected");
}
}
return false;
}
return true;
}
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
-107
View File
@@ -1,107 +0,0 @@
/****************************************************************************
*
* 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 <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_gyro.h>
using namespace time_literals;
bool PreFlightCheck::isGyroRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
const uint32_t device_id = static_cast<uint32_t>(gyro.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().gyro_device_id == device_id) {
is_used_by_nav = true;
break;
}
}
return is_used_by_nav;
}
bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
const bool is_required = is_mandatory || isGyroRequired(instance);
bool is_valid = false;
bool is_calibration_valid = false;
if (exists) {
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0)
&& (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
is_calibration_valid = true;
} else {
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0);
}
}
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance);
report_fail = false;
} else if (!is_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
report_fail = false;
} else if (!is_calibration_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance);
report_fail = false;
}
}
const bool is_sensor_ok = is_calibration_valid && is_valid;
return is_sensor_ok || !is_required;
}
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
-126
View File
@@ -1,126 +0,0 @@
/****************************************************************************
*
* 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 <HealthFlags.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensors_status_imu.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool report_status)
{
float accel_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
float gyro_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
// Get sensor_preflight data if available and exit with a fail recorded if not
uORB::SubscriptionData<sensors_status_imu_s> sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
const sensors_status_imu_s &imu = sensors_status_imu_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.
for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
if (imu.accel_device_ids[i] != 0) {
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
}
const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
if (accel_inconsistency_m_s_s > accel_test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i);
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
}
}
return false;
} else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i);
}
}
}
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
if (imu.gyro_device_ids[i] != 0) {
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
}
const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
if (gyro_inconsistency_rad_s > gyro_test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i);
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
}
}
return false;
} else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i);
}
}
}
}
return true;
}
src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp
-77
View File
@@ -1,77 +0,0 @@
/****************************************************************************
*
* 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 <HealthFlags.h>
#include <lib/parameters/param.h>
#include <mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_preflight_mag.h>
// 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<sensor_preflight_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
sensors_sub.update();
const sensor_preflight_mag_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 = 90;
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<float>(angle_difference_limit_deg);
if (!pass && report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compasses %d° inconsistent",
static_cast<int>(math::degrees<float>(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;
}
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
-130
View File
@@ -1,130 +0,0 @@
/****************************************************************************
*
* 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 <drivers/drv_hrt.h>
#include <HealthFlags.h>
#include <px4_defines.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_mag.h>
using namespace time_literals;
bool PreFlightCheck::isMagRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
const uint32_t device_id = static_cast<uint32_t>(magnetometer.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().mag_device_id == device_id) {
is_used_by_nav = true;;
break;
}
}
return is_used_by_nav;
}
bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
const bool is_required = is_mandatory || isMagRequired(instance);
bool is_valid = false;
bool is_calibration_valid = false;
bool is_mag_fault = false;
if (exists) {
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
const sensor_mag_s &mag_data = magnetometer.get();
is_valid = (mag_data.device_id != 0) && (mag_data.timestamp != 0) && (hrt_elapsed_time(&mag_data.timestamp) < 1_s);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
is_calibration_valid = true;
} else {
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", mag_data.device_id) >= 0);
}
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (estimator_status_sub.get().mag_device_id == static_cast<uint32_t>(mag_data.device_id)) {
if (estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)) {
is_mag_fault = true;
break;
}
}
}
}
const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault;
if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, is_required, is_sensor_ok, status);
} else if (instance == 1) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, is_required, is_sensor_ok, status);
}
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
report_fail = false;
} else if (!is_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance);
report_fail = false;
} else if (!is_calibration_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
report_fail = false;
} else if (is_mag_fault) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
report_fail = false;
}
}
return is_sensor_ok || !is_required;
}
src/modules/commander/Arming/PreFlightCheck/checks/manualControlCheck.cpp
-79
View File
@@ -1,79 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 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 <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_switches.h>
using namespace time_literals;
bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
bool success = true;
uORB::SubscriptionData<manual_control_switches_s> manual_control_switches_sub{ORB_ID(manual_control_switches)};
const manual_control_switches_s &manual_control_switches = manual_control_switches_sub.get();
if (manual_control_switches.timestamp != 0) {
// check action switches
if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Failure: RTL switch engaged");
}
}
if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Failure: Kill switch engaged");
}
}
if (manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Failure: Landing gear switch set in UP position");
}
}
}
return success;
}
src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp
-69
View File
@@ -1,69 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2022 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 <systemlib/mavlink_log.h>
using namespace time_literals;
bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
const vehicle_status_s &vehicle_status)
{
bool success = true;
switch (vehicle_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
case vehicle_status_s::NAVIGATION_STATE_STAB:
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
break;
default:
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Mode not suitable for takeoff");
}
break;
}
return success;
}
src/modules/commander/Arming/PreFlightCheck/checks/parachuteCheck.cpp
-68
View File
@@ -1,68 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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 <lib/parameters/param.h>
#include <lib/systemlib/mavlink_log.h>
using namespace time_literals;
bool PreFlightCheck::parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
const vehicle_status_flags_s &status_flags)
{
bool success = true;
int32_t param_com_parachute = false;
param_get(param_find("COM_PARACHUTE"), &param_com_parachute);
const bool parachute_required = param_com_parachute != 0;
if (parachute_required) {
if (!status_flags.parachute_system_present) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system missing");
}
} else if (!status_flags.parachute_system_healthy) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system not ready");
}
}
}
return success;
}
src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp
-107
View File
@@ -1,107 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 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 <drivers/drv_hrt.h>
#include <systemlib/mavlink_log.h>
#include <lib/parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/system_power.h>
#include <math.h>
using namespace time_literals;
bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail)
{
bool success = true;
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
// Ignore power check in HITL.
return true;
}
uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
system_power_sub.update();
const system_power_s &system_power = system_power_sub.get();
if (system_power.timestamp != 0) {
int32_t required_power_module_count = 0;
param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
// Check avionics rail voltages (if USB isn't connected)
if (!system_power.usb_connected) {
float avionics_power_rail_voltage = system_power.voltage5v_v;
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.8f) {
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);
}
}
const int power_module_count = math::countSetBits(system_power.brick_valid);
if (power_module_count < required_power_module_count) {
success = false;
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Power redundancy not met: %d instead of %" PRId32,
power_module_count, required_power_module_count);
}
}
}
} else {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "system power unavailable");
}
success = false;
}
return success;
}
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
-250
View File
@@ -1,250 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 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 <ArmAuthorization.h>
#include <HealthFlags.h>
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_command_ack.h>
bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off,
vehicle_status_s &status, const bool report_fail, const bool is_arm_attempt)
{
bool prearm_ok = true;
// rate control mode require valid angular velocity
if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Angular velocity invalid"); }
prearm_ok = false;
}
// attitude control mode require valid attitude
if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Attitude invalid"); }
prearm_ok = false;
}
// velocity control mode require valid velocity
if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Velocity invalid"); }
prearm_ok = false;
}
// position control mode require valid position
if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Position invalid"); }
prearm_ok = false;
}
// manual control mode require valid manual control (rc)
if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Manual control unavailable"); }
prearm_ok = false;
}
if (status_flags.flight_terminated) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Flight termination active"); }
prearm_ok = false;
}
// USB not connected
if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "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.power_input_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Connect power module"); }
prearm_ok = false;
}
// main battery level
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
status_flags.battery_healthy, status);
// Only arm if healthy
if (!status_flags.battery_healthy) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Check battery"); }
}
prearm_ok = false;
}
}
// Arm Requirements: mission
int32_t _param_com_arm_mis_req = 0;
param_get(param_find("COM_ARM_MIS_REQ"), &_param_com_arm_mis_req);
const bool mission_required = (_param_com_arm_mis_req == 1);
if (mission_required) {
if (!status_flags.auto_mission_available) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "No valid mission"); }
}
prearm_ok = false;
}
if (!status_flags.global_position_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Missions require a global position"); }
}
prearm_ok = false;
}
}
int32_t _param_com_arm_wo_gps = 1;
param_get(param_find("COM_ARM_WO_GPS"), &_param_com_arm_wo_gps);
const bool global_position_required = (_param_com_arm_wo_gps == 0);
if (global_position_required) {
if (!status_flags.global_position_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Global position required"); }
}
prearm_ok = false;
}
if (!status_flags.home_position_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Home position invalid"); }
}
prearm_ok = false;
}
}
// safety button
if (safety_button_available && !safety_off) {
// Fail transition if we need safety button press
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Press safety button first"); }
}
prearm_ok = false;
}
if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Avoidance system not ready"); }
}
prearm_ok = false;
}
int32_t _param_com_arm_chk_escs = 1;
param_get(param_find("COM_ARM_CHK_ESCS"), &_param_com_arm_chk_escs);
const bool esc_checks_required = (_param_com_arm_chk_escs == 0);
if (esc_checks_required && status_flags.escs_error) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "One or more ESCs are offline"); }
prearm_ok = false;
}
}
if (esc_checks_required && status_flags.escs_failure) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "One or more ESCs have a failure"); }
prearm_ok = false;
}
}
if (status.is_vtol) {
if (status.in_transition_mode) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Vehicle is in transition state"); }
prearm_ok = false;
}
}
if (!status_flags.circuit_breaker_vtol_fw_arming_check
&& status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Vehicle is not in multicopter mode"); }
prearm_ok = false;
}
}
}
int32_t _param_gf_action = 0;
param_get(param_find("GF_ACTION"), &_param_gf_action);
const bool gefence_action_configured = (_param_gf_action != 0);
if (gefence_action_configured && status.geofence_violated) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Vehicle outside geofence");
}
prearm_ok = false;
}
int32_t _param_com_arm_auth_req = 0;
param_get(param_find("COM_ARM_AUTH_REQ"), &_param_com_arm_auth_req);
const bool arm_authorization_configured = (_param_com_arm_auth_req != 0);
// Arm Requirements: authorization
// check last, and only if everything else has passed
// skip arm authorization check until actual arming attempt
if (arm_authorization_configured && prearm_ok && is_arm_attempt) {
if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
// feedback provided in arm_auth_check
prearm_ok = false;
}
}
return prearm_ok;
}
src/modules/commander/Arming/PreFlightCheck/checks/rcCalibrationCheck.cpp
-212
View File
@@ -1,212 +0,0 @@
/****************************************************************************
*
* 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 <parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/input_rc.h>
/**
* Maximum deadzone value
*/
#define RC_INPUT_MAX_DEADZONE_US 500
/**
* Minimum value
*/
#define RC_INPUT_LOWEST_MIN_US 0
/**
* Maximum value
*/
#define RC_INPUT_HIGHEST_MAX_US 2500
int PreFlightCheck::rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
{
char nbuf[20];
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
unsigned map_fail_count = 0;
const char *rc_map_mandatory[] = { /*"RC_MAP_MODE_SW",*/
/* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/
nullptr /* end marker */
};
unsigned j = 0;
/* first check channel mappings */
while (rc_map_mandatory[j] != nullptr) {
param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: PARAM %s MISSING.", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
j++;
continue;
}
int32_t mapping;
param_get(map_parm, &mapping);
if (mapping > input_rc_s::RC_INPUT_MAX_CHANNELS) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: %s >= NUMBER OF CHANNELS.", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
}
if (mapping == 0) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: mandatory %s is unmapped.", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
map_fail_count++;
}
j++;
}
unsigned total_fail_count = 0;
unsigned channels_failed = 0;
for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
/* should the channel be enabled? */
uint8_t count = 0;
/* initialize values to values failing the check */
float param_min = 0.0f;
float param_max = 0.0f;
float param_trim = 0.0f;
float param_rev = 0.0f;
float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f;
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles_min = param_find(nbuf);
param_get(_parameter_handles_min, &param_min);
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles_trim = param_find(nbuf);
param_get(_parameter_handles_trim, &param_trim);
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles_max = param_find(nbuf);
param_get(_parameter_handles_max, &param_max);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles_rev = param_find(nbuf);
param_get(_parameter_handles_rev, &param_rev);
/* channel deadzone */
sprintf(nbuf, "RC%d_DZ", i + 1);
_parameter_handles_dz = param_find(nbuf);
param_get(_parameter_handles_dz, &param_dz);
/* assert min..center..max ordering */
if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MIN < %u.", i + 1, RC_INPUT_LOWEST_MIN_US); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_MAX > %u.", i + 1, RC_INPUT_HIGHEST_MAX_US); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
if (param_trim < param_min) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM < MIN (%d/%d).", i + 1, (int)param_trim, (int)param_min); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
if (param_trim > param_max) {
count++;
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_TRIM > MAX (%d/%d).", i + 1, (int)param_trim, (int)param_max); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "RC ERROR: RC%d_DZ > %u.", i + 1, RC_INPUT_MAX_DEADZONE_US); }
/* give system time to flush error message in case there are more */
px4_usleep(100000);
count++;
}
total_fail_count += count;
if (count) {
channels_failed++;
}
}
if (channels_failed) {
px4_sleep(2);
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.",
total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
}
px4_usleep(100000);
}
return total_fail_count + map_fail_count;
}
src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp
-74
View File
@@ -1,74 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 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 <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#ifdef __PX4_DARWIN
#include <sys/param.h>
#include <sys/mount.h>
#else
#include <sys/statfs.h>
#endif
bool PreFlightCheck::sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once,
const bool report_fail)
{
bool success = true;
int32_t param_com_arm_sdcard{0};
param_get(param_find("COM_ARM_SDCARD"), &param_com_arm_sdcard);
if (param_com_arm_sdcard > 0) {
struct statfs statfs_buf;
if (!sd_card_detected_once && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
sd_card_detected_once = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
}
if (!sd_card_detected_once) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Warning! Missing FMU SD Card.");
}
if (param_com_arm_sdcard == 2) {
// disallow arming without sd card
success = false;
}
}
}
return success;
}