mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 15:40:31 +08:00
commander: rework arming checks to use the events interface
This commit is contained in:
Beat Küng
Daniel Agar
+1
-1
@@ -7,4 +7,4 @@ uint8[25] arguments # (optional) arguments, depend on event id
|
||||
|
||||
uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 8
|
||||
uint8 ORB_QUEUE_LENGTH = 16
|
||||
|
||||
@@ -113,3 +113,22 @@ uint64 onboard_control_sensors_health
|
||||
|
||||
bool safety_button_available # Set to true if a safety button is connected
|
||||
bool safety_off # Set to true if safety is off
|
||||
bool auto_mission_available
|
||||
|
||||
bool power_input_valid # set if input power is valid
|
||||
bool usb_connected # set to true (never cleared) once telemetry received from usb link
|
||||
|
||||
bool parachute_system_present
|
||||
bool parachute_system_healthy
|
||||
|
||||
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||
bool avoidance_system_valid # Status of the obstacle avoidance system
|
||||
|
||||
bool battery_healthy # set if battery is available and not low
|
||||
|
||||
bool circuit_breaker_engaged_power_check
|
||||
bool circuit_breaker_engaged_airspd_check
|
||||
bool circuit_breaker_flight_termination_disabled
|
||||
bool circuit_breaker_engaged_usb_check
|
||||
bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in fixed-wing mode should be allowed
|
||||
|
||||
|
||||
@@ -1,7 +1,21 @@
|
||||
# This is a struct used by the commander internally.
|
||||
# TODO: rename to failsafe_flags (will be input to failsafe state machine)
|
||||
#
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
# Per-mode requirements
|
||||
uint32 mode_req_angular_velocity
|
||||
uint32 mode_req_attitude
|
||||
uint32 mode_req_local_position
|
||||
uint32 mode_req_global_position
|
||||
uint32 mode_req_local_alt
|
||||
uint32 mode_req_mission
|
||||
uint32 mode_req_offboard_signal
|
||||
uint32 mode_req_home_position
|
||||
uint32 mode_req_prevent_arming # if set, cannot arm while in this mode
|
||||
uint32 mode_req_other # other requirements, not covered above (for external modes)
|
||||
|
||||
|
||||
bool calibration_enabled
|
||||
bool pre_flight_checks_pass # true if all checks necessary to arm pass
|
||||
bool auto_mission_available
|
||||
@@ -13,8 +27,6 @@ bool local_velocity_valid # set to true by the commander app if the quality of
|
||||
bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
|
||||
bool gps_position_valid
|
||||
bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
|
||||
bool power_input_valid # set if input power is valid
|
||||
bool battery_healthy # set if battery is available and not low
|
||||
bool escs_error # set to true if one or more ESCs reporting esc_status are offline
|
||||
bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure
|
||||
|
||||
@@ -23,25 +35,10 @@ bool position_reliant_on_optical_flow
|
||||
bool position_reliant_on_vision_position
|
||||
|
||||
bool dead_reckoning
|
||||
bool flight_terminated
|
||||
|
||||
bool circuit_breaker_engaged_power_check
|
||||
bool circuit_breaker_engaged_airspd_check
|
||||
bool circuit_breaker_flight_termination_disabled
|
||||
bool circuit_breaker_engaged_usb_check
|
||||
bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in fixed-wing mode should be allowed
|
||||
|
||||
bool offboard_control_signal_lost
|
||||
|
||||
bool rc_signal_found_once
|
||||
bool rc_calibration_in_progress
|
||||
bool rc_calibration_valid # set if RC calibration is valid
|
||||
bool vtol_transition_failure # Set to true if vtol transition failed
|
||||
bool usb_connected # status of the USB power supply
|
||||
bool sd_card_detected_once # set to true if the SD card was detected
|
||||
|
||||
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||
bool avoidance_system_valid # Status of the obstacle avoidance system
|
||||
|
||||
bool parachute_system_present
|
||||
bool parachute_system_healthy
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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; }
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -34,4 +34,3 @@
|
||||
add_subdirectory(ArmAuthorization)
|
||||
add_subdirectory(ArmStateMachine)
|
||||
add_subdirectory(HealthFlags)
|
||||
add_subdirectory(PreFlightCheck)
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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);
|
||||
};
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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, ¶m_min);
|
||||
|
||||
/* trim values */
|
||||
sprintf(nbuf, "RC%d_TRIM", i + 1);
|
||||
_parameter_handles_trim = param_find(nbuf);
|
||||
param_get(_parameter_handles_trim, ¶m_trim);
|
||||
|
||||
/* max values */
|
||||
sprintf(nbuf, "RC%d_MAX", i + 1);
|
||||
_parameter_handles_max = param_find(nbuf);
|
||||
param_get(_parameter_handles_max, ¶m_max);
|
||||
|
||||
/* channel reverse */
|
||||
sprintf(nbuf, "RC%d_REV", i + 1);
|
||||
_parameter_handles_rev = param_find(nbuf);
|
||||
param_get(_parameter_handles_rev, ¶m_rev);
|
||||
|
||||
/* channel deadzone */
|
||||
sprintf(nbuf, "RC%d_DZ", i + 1);
|
||||
_parameter_handles_dz = param_find(nbuf);
|
||||
param_get(_parameter_handles_dz, ¶m_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;
|
||||
}
|
||||
@@ -34,6 +34,7 @@
|
||||
add_subdirectory(failure_detector)
|
||||
add_subdirectory(HealthAndArmingChecks)
|
||||
add_subdirectory(Arming)
|
||||
add_subdirectory(ModeUtil)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__commander
|
||||
@@ -62,12 +63,12 @@ px4_add_module(
|
||||
geo
|
||||
health_and_arming_checks
|
||||
hysteresis
|
||||
PreFlightCheck
|
||||
ArmAuthorization
|
||||
ArmStateMachine
|
||||
HealthFlags
|
||||
sensor_calibration
|
||||
world_magnetic_model
|
||||
mode_util
|
||||
)
|
||||
|
||||
if(PX4_TESTING)
|
||||
|
||||
@@ -45,7 +45,6 @@
|
||||
#include "Commander.hpp"
|
||||
|
||||
/* commander module headers */
|
||||
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
|
||||
#include "Arming/ArmAuthorization/ArmAuthorization.h"
|
||||
#include "Arming/HealthFlags/HealthFlags.h"
|
||||
#include "commander_helper.h"
|
||||
@@ -300,26 +299,13 @@ int Commander::custom_command(int argc, char *argv[])
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "check")) {
|
||||
uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
vehicle_status_s vehicle_status{};
|
||||
vehicle_status_sub.copy(&vehicle_status);
|
||||
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS);
|
||||
|
||||
uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
vehicle_status_flags_s vehicle_status_flags{};
|
||||
vehicle_status_flags_sub.copy(&vehicle_status_flags);
|
||||
uORB::SubscriptionData<vehicle_status_flags_s> vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED");
|
||||
|
||||
uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
vehicle_control_mode_s vehicle_control_mode{};
|
||||
vehicle_control_mode_sub.copy(&vehicle_control_mode);
|
||||
|
||||
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags,
|
||||
vehicle_control_mode,
|
||||
true, // report_failures
|
||||
false, // safety_buttton_available not known
|
||||
false); // safety_off not known
|
||||
PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED");
|
||||
|
||||
print_health_flags(vehicle_status);
|
||||
print_health_flags(vehicle_status_sub.get());
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -511,11 +497,9 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
|
||||
|
||||
bool Commander::shutdown_if_allowed()
|
||||
{
|
||||
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
_actuator_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
|
||||
arm_disarm_reason_t::shutdown);
|
||||
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
|
||||
false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
|
||||
}
|
||||
|
||||
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
|
||||
@@ -764,11 +748,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, run_preflight_checks,
|
||||
&_mavlink_log_pub, _vehicle_status_flags,
|
||||
calling_reason);
|
||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks,
|
||||
&_mavlink_log_pub, calling_reason);
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
@@ -808,11 +790,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, false,
|
||||
&_mavlink_log_pub, _vehicle_status_flags,
|
||||
calling_reason);
|
||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false,
|
||||
&_mavlink_log_pub, calling_reason);
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
|
||||
@@ -834,7 +814,8 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
||||
|
||||
Commander::Commander() :
|
||||
ModuleParams(nullptr),
|
||||
_failure_detector(this)
|
||||
_failure_detector(this),
|
||||
_health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status)
|
||||
{
|
||||
_vehicle_land_detected.landed = true;
|
||||
|
||||
@@ -854,7 +835,7 @@ Commander::Commander() :
|
||||
|
||||
_vehicle_status_flags.offboard_control_signal_lost = true;
|
||||
|
||||
_vehicle_status_flags.power_input_valid = true;
|
||||
_vehicle_status.power_input_valid = true;
|
||||
|
||||
// default for vtol is rotary wing
|
||||
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
||||
@@ -868,12 +849,6 @@ Commander::Commander() :
|
||||
_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
|
||||
|
||||
updateParameters();
|
||||
|
||||
// run preflight immediately to find all relevant parameters, but don't report
|
||||
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
|
||||
false, // report_failures
|
||||
false, // safety_buttton_available not known
|
||||
false); // safety_off not known
|
||||
}
|
||||
|
||||
Commander::~Commander()
|
||||
@@ -1288,7 +1263,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
|
||||
// check if current mission and first item are valid
|
||||
if (_vehicle_status_flags.auto_mission_available) {
|
||||
if (_vehicle_status.auto_mission_available) {
|
||||
|
||||
// requested first mission item valid
|
||||
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
|
||||
@@ -1422,10 +1397,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
} else {
|
||||
|
||||
/* try to go to INIT/PREFLIGHT arming state */
|
||||
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_INIT, _actuator_armed,
|
||||
false /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
|
||||
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks,
|
||||
false /* fRunPreArmChecks */, &_mavlink_log_pub,
|
||||
(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
|
||||
) {
|
||||
|
||||
@@ -2124,7 +2098,7 @@ void Commander::updateParameters()
|
||||
}
|
||||
|
||||
|
||||
_vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
|
||||
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
|
||||
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
|
||||
@@ -2149,19 +2123,6 @@ void Commander::updateParameters()
|
||||
_vehicle_status.is_vtol = is_vtol(_vehicle_status);
|
||||
_vehicle_status.is_vtol_tailsitter = is_vtol_tailsitter(_vehicle_status);
|
||||
|
||||
// CP_DIST: collision preventation enabled if CP_DIST > 0
|
||||
if (is_rotary_wing(_vehicle_status) || is_vtol(_vehicle_status)) {
|
||||
if (_param_cp_dist == PARAM_INVALID) {
|
||||
_param_cp_dist = param_find("CP_DIST");
|
||||
}
|
||||
|
||||
float cp_dist = 0;
|
||||
|
||||
if (_param_cp_dist != PARAM_INVALID && (param_get(_param_cp_dist, &cp_dist) == PX4_OK)) {
|
||||
_collision_prevention_enabled = cp_dist > 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
// _mode_switch_mapped = (RC_MAP_FLTMODE > 0)
|
||||
if (_param_rc_map_fltmode != PARAM_INVALID && (param_get(_param_rc_map_fltmode, &value_int32) == PX4_OK)) {
|
||||
_mode_switch_mapped = (value_int32 > 0);
|
||||
@@ -2230,7 +2191,7 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* Update OA parameter */
|
||||
_vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
|
||||
@@ -2260,10 +2221,10 @@ Commander::run()
|
||||
!system_power.brick_valid &&
|
||||
!system_power.usb_connected) {
|
||||
/* flying only on servo rail, this is unsafe */
|
||||
_vehicle_status_flags.power_input_valid = false;
|
||||
_vehicle_status.power_input_valid = false;
|
||||
|
||||
} else {
|
||||
_vehicle_status_flags.power_input_valid = true;
|
||||
_vehicle_status.power_input_valid = true;
|
||||
}
|
||||
|
||||
_system_power_usb_connected = system_power.usb_connected;
|
||||
@@ -2476,11 +2437,9 @@ Commander::run()
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
|
||||
|
||||
_arm_state_machine.arming_state_transition(_vehicle_status, _vehicle_control_mode,
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff(),
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed,
|
||||
true /* fRunPreArmChecks */, &_mavlink_log_pub, _vehicle_status_flags,
|
||||
arm_disarm_reason_t::transition_to_standby);
|
||||
_arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks,
|
||||
true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby);
|
||||
}
|
||||
|
||||
/* start mission result check */
|
||||
@@ -2494,7 +2453,7 @@ Commander::run()
|
||||
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
|
||||
&& (mission_result.instance_count > 0);
|
||||
|
||||
_vehicle_status_flags.auto_mission_available = mission_result_ok && mission_result.valid;
|
||||
_vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid;
|
||||
|
||||
if (mission_result_ok) {
|
||||
if (_vehicle_status.mission_failure != mission_result.failure) {
|
||||
@@ -2513,7 +2472,7 @@ Commander::run()
|
||||
if (_vehicle_status_flags.home_position_valid &&
|
||||
(prev_mission_instance_count != mission_result.instance_count)) {
|
||||
|
||||
if (!_vehicle_status_flags.auto_mission_available) {
|
||||
if (!_vehicle_status.auto_mission_available) {
|
||||
/* the mission is invalid */
|
||||
tune_mission_fail(true);
|
||||
|
||||
@@ -2535,7 +2494,7 @@ Commander::run()
|
||||
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
|
||||
&& mission_result.finished) {
|
||||
|
||||
if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status_flags.auto_mission_available) {
|
||||
if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) {
|
||||
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags,
|
||||
_commander_state);
|
||||
|
||||
@@ -2658,7 +2617,7 @@ Commander::run()
|
||||
|
||||
/* Check for mission flight termination */
|
||||
if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination &&
|
||||
!_vehicle_status_flags.circuit_breaker_flight_termination_disabled) {
|
||||
!_vehicle_status.circuit_breaker_flight_termination_disabled) {
|
||||
|
||||
|
||||
if (!_flight_termination_triggered && !_lockdown_triggered) {
|
||||
@@ -2770,7 +2729,7 @@ Commander::run()
|
||||
events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning},
|
||||
"Critical failure detected: lockdown");
|
||||
|
||||
} else if (!_vehicle_status_flags.circuit_breaker_flight_termination_disabled &&
|
||||
} else if (!_vehicle_status.circuit_breaker_flight_termination_disabled &&
|
||||
!_flight_termination_triggered && !_lockdown_triggered) {
|
||||
|
||||
_actuator_armed.force_failsafe = true;
|
||||
@@ -2868,8 +2827,6 @@ Commander::run()
|
||||
checkWindSpeedThresholds();
|
||||
}
|
||||
|
||||
_vehicle_status_flags.flight_terminated = _actuator_armed.force_failsafe || _actuator_armed.manual_lockdown;
|
||||
|
||||
/* Get current timestamp */
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
@@ -3013,14 +2970,12 @@ Commander::run()
|
||||
if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_changed
|
||||
|| !(_actuator_armed == actuator_armed_prev)) {
|
||||
|
||||
// Evaluate current prearm status (skip during arm -> disarm transition)
|
||||
if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
|
||||
// Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already)
|
||||
if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status_flags.calibration_enabled) {
|
||||
perf_begin(_preflight_check_perf);
|
||||
_vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status,
|
||||
_vehicle_status_flags,
|
||||
_vehicle_control_mode,
|
||||
false, // report_failures
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff());
|
||||
_health_and_arming_checks.update();
|
||||
_vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm(
|
||||
_vehicle_status.nav_state);
|
||||
perf_end(_preflight_check_perf);
|
||||
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true,
|
||||
@@ -3072,7 +3027,7 @@ Commander::run()
|
||||
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
|
||||
_arm_tune_played = true;
|
||||
|
||||
} else if (!_vehicle_status_flags.usb_connected &&
|
||||
} else if (!_vehicle_status.usb_connected &&
|
||||
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
||||
/* play tune on battery critical */
|
||||
@@ -3151,17 +3106,17 @@ Commander::run()
|
||||
void
|
||||
Commander::get_circuit_breaker_params()
|
||||
{
|
||||
_vehicle_status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(),
|
||||
_vehicle_status.circuit_breaker_engaged_power_check = circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(),
|
||||
CBRK_SUPPLY_CHK_KEY);
|
||||
_vehicle_status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(),
|
||||
_vehicle_status.circuit_breaker_engaged_usb_check = circuit_breaker_enabled_by_val(_param_cbrk_usb_chk.get(),
|
||||
CBRK_USB_CHK_KEY);
|
||||
_vehicle_status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(
|
||||
_vehicle_status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled_by_val(
|
||||
_param_cbrk_airspd_chk.get(),
|
||||
CBRK_AIRSPD_CHK_KEY);
|
||||
_vehicle_status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(
|
||||
_vehicle_status.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(
|
||||
_param_cbrk_flightterm.get(),
|
||||
CBRK_FLIGHTTERM_KEY);
|
||||
_vehicle_status_flags.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val(
|
||||
_vehicle_status.circuit_breaker_vtol_fw_arming_check = circuit_breaker_enabled_by_val(
|
||||
_param_cbrk_vtolarming.get(),
|
||||
CBRK_VTOLARMING_KEY);
|
||||
}
|
||||
@@ -3604,7 +3559,7 @@ void Commander::data_link_check()
|
||||
switch (telemetry.type) {
|
||||
case telemetry_status_s::LINK_TYPE_USB:
|
||||
// set (but don't unset) telemetry via USB as active once a MAVLink connection is up
|
||||
_vehicle_status_flags.usb_connected = true;
|
||||
_vehicle_status.usb_connected = true;
|
||||
break;
|
||||
|
||||
case telemetry_status_s::LINK_TYPE_IRIDIUM: {
|
||||
@@ -3641,13 +3596,6 @@ void Commander::data_link_check()
|
||||
mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
|
||||
events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
|
||||
}
|
||||
|
||||
if (!_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) {
|
||||
// make sure to report preflight check failures to a connecting GCS
|
||||
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode,
|
||||
true, // report_failures
|
||||
_safety.isButtonAvailable(), _safety.isSafetyOff());
|
||||
}
|
||||
}
|
||||
|
||||
_datalink_last_heartbeat_gcs = telemetry.timestamp;
|
||||
@@ -3680,8 +3628,8 @@ void Commander::data_link_check()
|
||||
bool healthy = telemetry.parachute_system_healthy;
|
||||
|
||||
_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
|
||||
_vehicle_status_flags.parachute_system_present = true;
|
||||
_vehicle_status_flags.parachute_system_healthy = healthy;
|
||||
_vehicle_status.parachute_system_present = true;
|
||||
_vehicle_status.parachute_system_healthy = healthy;
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _vehicle_status);
|
||||
}
|
||||
|
||||
@@ -3692,7 +3640,7 @@ void Commander::data_link_check()
|
||||
}
|
||||
|
||||
_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
|
||||
_vehicle_status_flags.avoidance_system_valid = telemetry.avoidance_system_healthy;
|
||||
_vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3729,21 +3677,21 @@ void Commander::data_link_check()
|
||||
if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
|
||||
&& !_parachute_system_lost) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost");
|
||||
_vehicle_status_flags.parachute_system_present = false;
|
||||
_vehicle_status_flags.parachute_system_healthy = false;
|
||||
_vehicle_status.parachute_system_present = false;
|
||||
_vehicle_status.parachute_system_healthy = false;
|
||||
_parachute_system_lost = true;
|
||||
_status_changed = true;
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _vehicle_status);
|
||||
}
|
||||
|
||||
// AVOIDANCE SYSTEM state check (only if it is enabled)
|
||||
if (_vehicle_status_flags.avoidance_system_required && !_onboard_controller_lost) {
|
||||
if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) {
|
||||
// if heartbeats stop
|
||||
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
|
||||
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
|
||||
|
||||
_avoidance_system_lost = true;
|
||||
_vehicle_status_flags.avoidance_system_valid = false;
|
||||
_vehicle_status.avoidance_system_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3776,18 +3724,18 @@ void Commander::avoidance_check()
|
||||
}
|
||||
|
||||
const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
|
||||
const bool cp_healthy = _vehicle_status_flags.avoidance_system_valid || distance_sensor_valid;
|
||||
const bool cp_healthy = _vehicle_status.avoidance_system_valid || distance_sensor_valid;
|
||||
|
||||
const bool sensor_oa_present = cp_healthy || _vehicle_status_flags.avoidance_system_required
|
||||
const bool sensor_oa_present = cp_healthy || _vehicle_status.avoidance_system_required
|
||||
|| _collision_prevention_enabled;
|
||||
|
||||
const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
|
||||
const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
|
||||
&& _vehicle_control_mode.flag_control_position_enabled);
|
||||
|
||||
const bool sensor_oa_enabled = ((auto_mode && _vehicle_status_flags.avoidance_system_required) || (pos_ctl_mode
|
||||
const bool sensor_oa_enabled = ((auto_mode && _vehicle_status.avoidance_system_required) || (pos_ctl_mode
|
||||
&& _collision_prevention_enabled));
|
||||
const bool sensor_oa_healthy = ((auto_mode && _vehicle_status_flags.avoidance_system_valid) || (pos_ctl_mode
|
||||
const bool sensor_oa_healthy = ((auto_mode && _vehicle_status.avoidance_system_valid) || (pos_ctl_mode
|
||||
&& cp_healthy));
|
||||
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
|
||||
@@ -3939,7 +3887,7 @@ void Commander::battery_status_check()
|
||||
_battery_warning = worst_warning;
|
||||
}
|
||||
|
||||
_vehicle_status_flags.battery_healthy =
|
||||
_vehicle_status.battery_healthy =
|
||||
// All connected batteries are regularly being published
|
||||
(hrt_elapsed_time(&oldest_update) < 5_s)
|
||||
// There is at least one connected battery (in any slot)
|
||||
@@ -4323,7 +4271,7 @@ void Commander::manual_control_check()
|
||||
|
||||
} else {
|
||||
// if not mavlink also report valid RC calibration for health
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _vehicle_status_flags.rc_calibration_valid,
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true,
|
||||
_vehicle_status);
|
||||
}
|
||||
|
||||
|
||||
@@ -35,11 +35,11 @@
|
||||
|
||||
/* Helper classes */
|
||||
#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
|
||||
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
|
||||
#include "failure_detector/FailureDetector.hpp"
|
||||
#include "Safety.hpp"
|
||||
#include "state_machine_helper.h"
|
||||
#include "worker_thread.hpp"
|
||||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||
|
||||
#include <containers/Bitset.hpp>
|
||||
#include <lib/controllib/blocks.hpp>
|
||||
@@ -264,7 +264,6 @@ private:
|
||||
)
|
||||
|
||||
// optional parameters
|
||||
param_t _param_cp_dist{PARAM_INVALID};
|
||||
param_t _param_mav_comp_id{PARAM_INVALID};
|
||||
param_t _param_mav_sys_id{PARAM_INVALID};
|
||||
param_t _param_mav_type{PARAM_INVALID};
|
||||
@@ -317,7 +316,6 @@ private:
|
||||
bool _geofence_warning_action_on{false};
|
||||
bool _geofence_violated_prev{false};
|
||||
|
||||
bool _collision_prevention_enabled{false};
|
||||
|
||||
bool _rtl_time_actions_done{false};
|
||||
|
||||
@@ -425,7 +423,6 @@ private:
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
|
||||
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
|
||||
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
@@ -456,4 +453,5 @@ private:
|
||||
|
||||
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
|
||||
HealthAndArmingChecks _health_and_arming_checks;
|
||||
};
|
||||
|
||||
@@ -34,7 +34,26 @@
|
||||
px4_add_library(health_and_arming_checks
|
||||
Common.cpp
|
||||
HealthAndArmingChecks.cpp
|
||||
checks/systemCheck.cpp
|
||||
checks/magnetometerCheck.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/estimatorCheck.cpp
|
||||
checks/failureDetectorCheck.cpp
|
||||
checks/manualControlCheck.cpp
|
||||
checks/modeCheck.cpp
|
||||
checks/cpuResourceCheck.cpp
|
||||
checks/sdcardCheck.cpp
|
||||
checks/parachuteCheck.cpp
|
||||
checks/batteryCheck.cpp
|
||||
)
|
||||
add_dependencies(health_and_arming_checks mode_util)
|
||||
|
||||
px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
|
||||
LINKLIBS health_and_arming_checks mode_util
|
||||
|
||||
@@ -32,6 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include "Common.hpp"
|
||||
#include "../ModeUtil/mode_requirements.hpp"
|
||||
|
||||
void Report::getHealthReport(health_report_s &report) const
|
||||
{
|
||||
@@ -157,7 +158,12 @@ void Report::reset()
|
||||
_results[_current_result].reset();
|
||||
_next_buffer_idx = 0;
|
||||
_buffer_overflowed = false;
|
||||
_failing_checks_prevent_mode_switch = 0;
|
||||
}
|
||||
|
||||
void Report::prepare(uint8_t vehicle_type)
|
||||
{
|
||||
// Get mode requirements before running any checks (in particular the mode checks require them)
|
||||
getModeRequirements(vehicle_type, _status_flags);
|
||||
}
|
||||
|
||||
NavModes Report::getModeGroup(uint8_t nav_state) const
|
||||
|
||||
@@ -316,6 +316,7 @@ private:
|
||||
* - report() (which can be called independently as well)
|
||||
*/
|
||||
void reset();
|
||||
void prepare(uint8_t vehicle_type);
|
||||
void finalize();
|
||||
|
||||
bool report(bool is_armed, bool force);
|
||||
|
||||
@@ -45,6 +45,8 @@ bool HealthAndArmingChecks::update(bool force_reporting)
|
||||
{
|
||||
_reporter.reset();
|
||||
|
||||
_reporter.prepare(_context.status().vehicle_type);
|
||||
|
||||
for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
|
||||
if (!_checks[i]) {
|
||||
break;
|
||||
@@ -64,6 +66,8 @@ bool HealthAndArmingChecks::update(bool force_reporting)
|
||||
_reporter._mavlink_log_pub = &_mavlink_log_pub;
|
||||
_reporter.reset();
|
||||
|
||||
_reporter.prepare(_context.status().vehicle_type);
|
||||
|
||||
for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
|
||||
if (!_checks[i]) {
|
||||
break;
|
||||
|
||||
@@ -39,6 +39,26 @@
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/health_report.h>
|
||||
|
||||
#include "checks/accelerometerCheck.hpp"
|
||||
#include "checks/airspeedCheck.hpp"
|
||||
#include "checks/baroCheck.hpp"
|
||||
#include "checks/cpuResourceCheck.hpp"
|
||||
#include "checks/distanceSensorChecks.hpp"
|
||||
#include "checks/estimatorCheck.hpp"
|
||||
#include "checks/failureDetectorCheck.hpp"
|
||||
#include "checks/gyroCheck.hpp"
|
||||
#include "checks/imuConsistencyCheck.hpp"
|
||||
#include "checks/magnetometerCheck.hpp"
|
||||
#include "checks/manualControlCheck.hpp"
|
||||
#include "checks/modeCheck.hpp"
|
||||
#include "checks/parachuteCheck.hpp"
|
||||
#include "checks/powerCheck.hpp"
|
||||
#include "checks/rcCalibrationCheck.hpp"
|
||||
#include "checks/sdcardCheck.hpp"
|
||||
#include "checks/systemCheck.hpp"
|
||||
#include "checks/batteryCheck.hpp"
|
||||
|
||||
|
||||
class HealthAndArmingChecks : public ModuleParams
|
||||
{
|
||||
public:
|
||||
@@ -58,6 +78,11 @@ public:
|
||||
*/
|
||||
bool canArm(uint8_t nav_state) const { return _reporter.canArm(nav_state); }
|
||||
|
||||
/**
|
||||
* Whether switching into a given navigation mode is possible
|
||||
*/
|
||||
bool canRun(uint8_t nav_state) const { return _reporter.canRun(nav_state); }
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
private:
|
||||
@@ -68,7 +93,44 @@ private:
|
||||
uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
|
||||
|
||||
// all checks
|
||||
HealthAndArmingCheckBase *_checks[10] = {
|
||||
AccelerometerChecks _accelerometer_checks;
|
||||
AirspeedChecks _airspeed_checks;
|
||||
BaroChecks _baro_checks;
|
||||
CpuResourceChecks _cpu_resource_checks;
|
||||
DistanceSensorChecks _distance_sensor_checks;
|
||||
EstimatorChecks _estimator_checks;
|
||||
FailureDetectorChecks _failure_detector_checks;
|
||||
GyroChecks _gyro_checks;
|
||||
ImuConsistencyChecks _imu_consistency_checks;
|
||||
MagnetometerChecks _magnetometer_checks;
|
||||
ManualControlChecks _manual_control_checks;
|
||||
ModeChecks _mode_checks;
|
||||
ParachuteChecks _parachute_checks;
|
||||
PowerChecks _power_checks;
|
||||
RcCalibrationChecks _rc_calibration_checks;
|
||||
SdCardChecks _sd_card_checks;
|
||||
SystemChecks _system_checks;
|
||||
BatteryChecks _battery_checks;
|
||||
|
||||
HealthAndArmingCheckBase *_checks[30] = {
|
||||
&_accelerometer_checks,
|
||||
&_airspeed_checks,
|
||||
&_baro_checks,
|
||||
&_cpu_resource_checks,
|
||||
&_distance_sensor_checks,
|
||||
&_estimator_checks,
|
||||
&_failure_detector_checks,
|
||||
&_gyro_checks,
|
||||
&_imu_consistency_checks,
|
||||
&_magnetometer_checks,
|
||||
&_manual_control_checks,
|
||||
&_mode_checks,
|
||||
&_parachute_checks,
|
||||
&_power_checks,
|
||||
&_rc_calibration_checks,
|
||||
&_sd_card_checks,
|
||||
&_system_checks,
|
||||
&_battery_checks,
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "accelerometerCheck.hpp"
|
||||
|
||||
#include <lib/sensor_calibration/Utilities.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void AccelerometerChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
for (int instance = 0; instance < _sensor_accel_sub.size(); instance++) {
|
||||
const bool is_required = instance == 0 || isAccelRequired(instance);
|
||||
|
||||
if (!is_required) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const bool exists = _sensor_accel_sub[instance].advertised();
|
||||
bool is_valid = false;
|
||||
bool is_calibration_valid = false;
|
||||
|
||||
if (exists) {
|
||||
sensor_accel_s accel_data;
|
||||
is_valid = _sensor_accel_sub[instance].copy(&accel_data) && (accel_data.device_id != 0) && (accel_data.timestamp != 0)
|
||||
&& (hrt_elapsed_time(&accel_data.timestamp) < 1_s);
|
||||
|
||||
if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
is_calibration_valid = true;
|
||||
|
||||
} else {
|
||||
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel_data.device_id) >= 0);
|
||||
}
|
||||
|
||||
reporter.setIsPresent(health_component_t::gyro);
|
||||
}
|
||||
|
||||
const bool is_sensor_ok = is_valid && is_calibration_valid;
|
||||
|
||||
if (!is_sensor_ok) {
|
||||
if (!exists) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::accel, events::ID("check_accel_missing"),
|
||||
events::Log::Error, "Accelerometer sensor {1} missing", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel Sensor %u missing", instance);
|
||||
}
|
||||
|
||||
} else if (!is_valid) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::accel, events::ID("check_accel_no_data"),
|
||||
events::Log::Error, "No valid data from Accelerometer {1}", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Accel %u", instance);
|
||||
}
|
||||
|
||||
} else if (!is_calibration_valid) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::accel, events::ID("check_accel_not_calibrated"),
|
||||
events::Log::Error, "Accelerometer {1} uncalibrated", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel %u uncalibrated", instance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool AccelerometerChecks::isAccelRequired(int instance)
|
||||
{
|
||||
sensor_accel_s sensor_accel;
|
||||
|
||||
if (!_sensor_accel_sub[instance].copy(&sensor_accel)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t device_id = static_cast<uint32_t>(sensor_accel.device_id);
|
||||
|
||||
if (device_id == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool is_used_by_nav = false;
|
||||
|
||||
for (int i = 0; i < _estimator_status_sub.size(); i++) {
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.accel_device_id == device_id) {
|
||||
is_used_by_nav = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return is_used_by_nav;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <lib/sensor_calibration/Accelerometer.hpp>
|
||||
|
||||
class AccelerometerChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
AccelerometerChecks() = default;
|
||||
~AccelerometerChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
bool isAccelRequired(int instance);
|
||||
|
||||
uORB::SubscriptionMultiArray<sensor_accel_s, calibration::Accelerometer::MAX_SENSOR_COUNT> _sensor_accel_sub{ORB_ID::sensor_accel};
|
||||
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
|
||||
};
|
||||
@@ -0,0 +1,125 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "airspeedCheck.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
AirspeedChecks::AirspeedChecks()
|
||||
: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_trim_handle(param_find("FW_AIRSPD_TRIM"))
|
||||
{
|
||||
}
|
||||
|
||||
void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (context.status().circuit_breaker_engaged_airspd_check ||
|
||||
(context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !context.status().is_vtol)) {
|
||||
return;
|
||||
}
|
||||
|
||||
int32_t airspeed_mode = 0;
|
||||
param_get(_param_fw_arsp_mode_handle, &airspeed_mode);
|
||||
const bool optional = (airspeed_mode == 1);
|
||||
|
||||
airspeed_validated_s airspeed_validated;
|
||||
|
||||
if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s) {
|
||||
|
||||
reporter.setIsPresent(health_component_t::differential_pressure);
|
||||
|
||||
float airspeed_trim = 10.0f;
|
||||
param_get(_param_fw_airspd_trim_handle, &airspeed_trim);
|
||||
|
||||
const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed
|
||||
|
||||
/*
|
||||
* Check if airspeed is declared valid or not by airspeed selector.
|
||||
*/
|
||||
if (!PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)) {
|
||||
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,
|
||||
events::ID("check_airspeed_invalid"),
|
||||
events::Log::Error, "Airspeed invalid");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed invalid");
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Check if airspeed is higher than maximally accepted while the vehicle is landed / not flying
|
||||
* Negative and positive offsets are considered. This check is optional, because the pitot cover
|
||||
* might have been removed before arming.
|
||||
*/
|
||||
if (!context.isArmed() && _param_com_arm_arsp_en.get()
|
||||
&& fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Check the airspeed calibration and the pitot.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Measured: {1:.1m/s}, limit: {2:.1m/s}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_ARSP_EN</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(NavModes::None, health_component_t::differential_pressure,
|
||||
events::ID("check_airspeed_too_high"),
|
||||
events::Log::Error, "Airspeed too high", airspeed_validated.calibrated_airspeed_m_s, arming_max_airspeed_allowed);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed too high - check cal or pitot");
|
||||
}
|
||||
}
|
||||
|
||||
} else if (!optional) {
|
||||
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Most likely the airspeed selector module is not running.
|
||||
* This check can be configured via <param>CBRK_AIRSPD_CHK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,
|
||||
events::ID("check_airspeed_no_data"),
|
||||
events::Log::Error, "No airspeed data");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Airspeed selector module down");
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
+18
-28
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -31,38 +31,28 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "../PreFlightCheck.hpp"
|
||||
#pragma once
|
||||
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
#include "../Common.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
|
||||
bool PreFlightCheck::parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
|
||||
const vehicle_status_flags_s &status_flags)
|
||||
class AirspeedChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
bool success = true;
|
||||
public:
|
||||
AirspeedChecks();
|
||||
~AirspeedChecks() = default;
|
||||
|
||||
int32_t param_com_parachute = false;
|
||||
param_get(param_find("COM_PARACHUTE"), ¶m_com_parachute);
|
||||
const bool parachute_required = param_com_parachute != 0;
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
if (parachute_required) {
|
||||
if (!status_flags.parachute_system_present) {
|
||||
success = false;
|
||||
private:
|
||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Fail: Parachute system missing");
|
||||
}
|
||||
const param_t _param_fw_arsp_mode_handle;
|
||||
const param_t _param_fw_airspd_trim_handle;
|
||||
|
||||
} 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;
|
||||
}
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamBool<px4::params::COM_ARM_ARSP_EN>) _param_com_arm_arsp_en
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,116 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "baroCheck.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void BaroChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (!_param_sys_has_baro.get()) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int instance = 0; instance < _sensor_baro_sub.size(); instance++) {
|
||||
const bool is_required = instance == 0 || isBaroRequired(instance);
|
||||
|
||||
if (!is_required) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const bool exists = _sensor_baro_sub[instance].advertised();
|
||||
bool is_valid = false;
|
||||
|
||||
if (exists) {
|
||||
sensor_baro_s baro_data;
|
||||
is_valid = _sensor_baro_sub[instance].copy(&baro_data) && (baro_data.device_id != 0) && (baro_data.timestamp != 0)
|
||||
&& (hrt_elapsed_time(&baro_data.timestamp) < 1_s);
|
||||
reporter.setIsPresent(health_component_t::absolute_pressure);
|
||||
}
|
||||
|
||||
const bool is_sensor_ok = is_valid;
|
||||
|
||||
if (!is_sensor_ok) {
|
||||
if (!exists) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::absolute_pressure,
|
||||
events::ID("check_baro_missing"),
|
||||
events::Log::Error, "Barometer {1} missing", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: barometer %u missing", instance);
|
||||
}
|
||||
|
||||
} else if (!is_valid) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::absolute_pressure,
|
||||
events::ID("check_baro_no_data"),
|
||||
events::Log::Error, "No valid data from barometer {1}", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Baro %u", instance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool BaroChecks::isBaroRequired(int instance)
|
||||
{
|
||||
sensor_baro_s sensor_baro;
|
||||
|
||||
if (!_sensor_baro_sub[instance].copy(&sensor_baro)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t device_id = static_cast<uint32_t>(sensor_baro.device_id);
|
||||
|
||||
if (device_id == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool is_used_by_nav = false;
|
||||
|
||||
for (int i = 0; i < _estimator_status_sub.size(); i++) {
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.baro_device_id == device_id) {
|
||||
is_used_by_nav = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return is_used_by_nav;
|
||||
}
|
||||
+20
-28
@@ -31,39 +31,31 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "../PreFlightCheck.hpp"
|
||||
#pragma once
|
||||
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include "../Common.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <lib/sensor_calibration/Barometer.hpp>
|
||||
|
||||
bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
|
||||
const vehicle_status_s &vehicle_status)
|
||||
class BaroChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
bool success = true;
|
||||
public:
|
||||
BaroChecks() = default;
|
||||
~BaroChecks() = default;
|
||||
|
||||
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;
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
default:
|
||||
success = false;
|
||||
private:
|
||||
bool isBaroRequired(int instance);
|
||||
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Mode not suitable for takeoff");
|
||||
}
|
||||
uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
|
||||
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro
|
||||
)
|
||||
};
|
||||
+12
-16
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -31,28 +31,24 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "../PreFlightCheck.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include "batteryCheck.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
bool PreFlightCheck::airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status)
|
||||
void BatteryChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
bool success = true;
|
||||
if (!context.status().battery_healthy) {
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4_FMU_V2
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"),
|
||||
events::Log::Error, "Battery unhealthy");
|
||||
|
||||
// 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;
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Battery unhealthy");
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
reporter.setIsPresent(health_component_t::battery); // assume it's present
|
||||
|
||||
return success;
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
class BatteryChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
BatteryChecks() = default;
|
||||
~BatteryChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
};
|
||||
@@ -0,0 +1,84 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "cpuResourceCheck.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (_param_com_cpu_max.get() < FLT_EPSILON) {
|
||||
return;
|
||||
}
|
||||
|
||||
cpuload_s cpuload;
|
||||
|
||||
if (!_cpuload_sub.copy(&cpuload) || hrt_elapsed_time(&cpuload.timestamp) > 2_s) {
|
||||
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* If the system does not provide any CPU load information, use the parameter <param>COM_CPU_MAX</param>
|
||||
* to disable the check.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"),
|
||||
events::Log::Error, "No CPU load information");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information");
|
||||
}
|
||||
|
||||
} else {
|
||||
const float cpuload_percent = cpuload.load * 100.f;
|
||||
|
||||
if (cpuload_percent > _param_com_cpu_max.get()) {
|
||||
|
||||
/* EVENT
|
||||
* @description
|
||||
* The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
|
||||
* rate via <param>IMU_GYRO_RATEMAX</param>.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* The threshold can be adjusted via <param>COM_CPU_MAX</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure<float>(NavModes::All, health_component_t::system, events::ID("check_cpuload_too_high"),
|
||||
events::Log::Error, "CPU load too high: {1:.1}%", cpuload_percent);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
+15
-36
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* 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
|
||||
@@ -31,46 +31,25 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "../PreFlightCheck.hpp"
|
||||
#pragma once
|
||||
|
||||
#include "../Common.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)
|
||||
class CpuResourceChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
bool success = true;
|
||||
public:
|
||||
CpuResourceChecks() = default;
|
||||
~CpuResourceChecks() = default;
|
||||
|
||||
uORB::SubscriptionData<cpuload_s> cpuload_sub{ORB_ID(cpuload)};
|
||||
cpuload_sub.update();
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
float cpuload_percent_max;
|
||||
param_get(param_find("COM_CPU_MAX"), &cpuload_percent_max);
|
||||
private:
|
||||
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
|
||||
|
||||
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;
|
||||
}
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,84 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "distanceSensorChecks.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void DistanceSensorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (_param_sys_has_num_dist.get() <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int instance = 0; instance < _distance_sensor_sub.size(); instance++) {
|
||||
const bool exists = _distance_sensor_sub[instance].advertised();
|
||||
const bool is_mandatory = instance < _param_sys_has_num_dist.get();
|
||||
bool valid = false;
|
||||
|
||||
if (exists) {
|
||||
distance_sensor_s dist_sens;
|
||||
valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s;
|
||||
reporter.setIsPresent(health_component_t::distance_sensor);
|
||||
}
|
||||
|
||||
if (is_mandatory) {
|
||||
if (!exists) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>SYS_HAS_NUM_DIST</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::distance_sensor,
|
||||
events::ID("check_distance_sensor_missing"),
|
||||
events::Log::Error, "Distance sensor {1} missing", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Distance Sensor %u missing", instance);
|
||||
}
|
||||
|
||||
} else if (!valid) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::distance_sensor,
|
||||
events::ID("check_distance_sensor_invalid"),
|
||||
events::Log::Error, "No valid data from distance sensor {1}", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: no valid data from distance sensor %u", instance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
+15
-30
@@ -31,41 +31,26 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "../PreFlightCheck.hpp"
|
||||
#pragma once
|
||||
|
||||
#include "../Common.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/SubscriptionMultiArray.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)
|
||||
class DistanceSensorChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK);
|
||||
bool valid = false;
|
||||
public:
|
||||
DistanceSensorChecks() = default;
|
||||
~DistanceSensorChecks() = default;
|
||||
|
||||
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();
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
|
||||
}
|
||||
private:
|
||||
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_sub{ORB_ID::distance_sensor};
|
||||
|
||||
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;
|
||||
}
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::SYS_HAS_NUM_DIST>) _param_sys_has_num_dist
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,473 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "estimatorCheck.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (_param_sys_mc_est_group.get() != 2) {
|
||||
return;
|
||||
}
|
||||
|
||||
const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
|
||||
bool missing_data = false;
|
||||
|
||||
if (_param_sens_imu_mode.get() == 0) { // multi-ekf
|
||||
estimator_selector_status_s estimator_selector_status;
|
||||
|
||||
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
|
||||
bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance)
|
||||
&& _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
|
||||
|
||||
if (!instance_changed) {
|
||||
missing_data = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
missing_data = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!missing_data) {
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
if (_estimator_status_sub.copy(&estimator_status)) {
|
||||
|
||||
checkEstimatorStatus(context, reporter, estimator_status, required_groups);
|
||||
|
||||
} else {
|
||||
missing_data = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (missing_data) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_missing_data"),
|
||||
events::Log::Info, "Waiting for estimator to initialize");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ekf2 missing data");
|
||||
}
|
||||
|
||||
} else {
|
||||
reporter.setIsPresent(health_component_t::local_position_estimate);
|
||||
checkSensorBias(context, reporter, required_groups);
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
|
||||
const estimator_status_s &estimator_status, NavModes required_groups)
|
||||
{
|
||||
if (estimator_status.pre_flt_fail_innov_heading) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_heading_not_stable"),
|
||||
events::Log::Error, "Heading estimate not stable");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: heading estimate not stable");
|
||||
}
|
||||
|
||||
} else if (estimator_status.pre_flt_fail_innov_vel_horiz) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_hor_vel_not_stable"),
|
||||
events::Log::Error, "Horizontal velocity estimate not stable");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal velocity estimate not stable");
|
||||
}
|
||||
|
||||
} else if (estimator_status.pre_flt_fail_innov_vel_vert) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_vert_vel_not_stable"),
|
||||
events::Log::Error, "Vertical velocity estimate not stable");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity estimate not stable");
|
||||
}
|
||||
|
||||
} else if (estimator_status.pre_flt_fail_innov_height) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_hgt_not_stable"),
|
||||
events::Log::Error, "Height estimate not stable");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate not stable");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if ((_param_com_arm_mag_str.get() >= 1) && estimator_status.pre_flt_fail_mag_field_disturbed) {
|
||||
NavModes required_groups_mag = required_groups;
|
||||
|
||||
if (_param_com_arm_mag_str.get() != 1) {
|
||||
required_groups_mag = NavModes::None; // optional
|
||||
}
|
||||
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>COM_ARM_MAG_STR</param> and <param>EKF2_MAG_CHECK</param> parameters.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_mag_interference"),
|
||||
events::Log::Warning, "Strong magnetic interference detected");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference detected");
|
||||
}
|
||||
}
|
||||
|
||||
// check vertical position innovation test ratio
|
||||
if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) {
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_hgt_est_err"),
|
||||
events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// check velocity innovation test ratio
|
||||
if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) {
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_vel_est_err"),
|
||||
events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// check horizontal position innovation test ratio
|
||||
if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) {
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_pos_est_err"),
|
||||
events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// check magnetometer innovation test ratio
|
||||
if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) {
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
* This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_yaw_est_err"),
|
||||
events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
|
||||
}
|
||||
}
|
||||
|
||||
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
|
||||
if (_param_sys_has_gps.get()) {
|
||||
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
|
||||
const bool ekf_gps_check_fail = estimator_status.gps_check_fail_flags > 0;
|
||||
|
||||
if (ekf_gps_fusion) {
|
||||
reporter.setIsPresent(health_component_t::gps); // should be based on the sensor data directly
|
||||
}
|
||||
|
||||
if (ekf_gps_check_fail) {
|
||||
NavModes required_groups_gps = required_groups;
|
||||
events::Log log_level = events::Log::Error;
|
||||
|
||||
if (_param_com_arm_wo_gps.get()) {
|
||||
required_groups_gps = NavModes::None; // optional
|
||||
log_level = events::Log::Warning;
|
||||
}
|
||||
|
||||
// Only report the first failure to avoid spamming
|
||||
const char *message = nullptr;
|
||||
|
||||
if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
|
||||
message = "Preflight%s: GPS fix too low";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_fix_too_low"),
|
||||
log_level, "GPS fix too low");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
|
||||
message = "Preflight%s: not enough GPS Satellites";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_num_sats_too_low"),
|
||||
log_level, "Not enough GPS Satellites");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
|
||||
message = "Preflight%s: GPS PDOP too high";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_pdop_too_high"),
|
||||
log_level, "GPS PDOP too high");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
|
||||
message = "Preflight%s: GPS Horizontal Pos Error too high";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_hor_pos_err_too_high"),
|
||||
log_level, "GPS Horizontal Position Error too high");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
|
||||
message = "Preflight%s: GPS Vertical Pos Error too high";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_vert_pos_err_too_high"),
|
||||
log_level, "GPS Vertical Position Error too high");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
|
||||
message = "Preflight%s: GPS Speed Accuracy too low";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_speed_acc_too_low"),
|
||||
log_level, "GPS Speed Accuracy too low");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
|
||||
message = "Preflight%s: GPS Horizontal Pos Drift too high";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_hor_pos_drift_too_high"),
|
||||
log_level, "GPS Horizontal Position Drift too high");
|
||||
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
|
||||
message = "Preflight%s: GPS Vertical Pos Drift too high";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_vert_pos_drift_too_high"),
|
||||
log_level, "GPS Vertical Position Drift too high");
|
||||
|
||||
} else if (estimator_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";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_hor_speed_drift_too_high"),
|
||||
log_level, "GPS Horizontal Speed Drift too high");
|
||||
|
||||
} else if (estimator_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";
|
||||
/* EVENT
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_vert_speed_drift_too_high"),
|
||||
log_level, "GPS Vertical Speed Drift too high");
|
||||
|
||||
} else {
|
||||
if (!ekf_gps_fusion) {
|
||||
// Likely cause unknown
|
||||
message = "Preflight%s: Estimator not using GPS";
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_not_fusing"),
|
||||
log_level, "Estimator not using GPS");
|
||||
|
||||
} 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";
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_generic"),
|
||||
log_level, "Poor GPS Quality");
|
||||
}
|
||||
}
|
||||
|
||||
if (message && reporter.mavlink_log_pub()) {
|
||||
if (!_param_com_arm_wo_gps.get()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter, NavModes required_groups)
|
||||
{
|
||||
// _estimator_sensor_bias_sub instance got changed above already
|
||||
estimator_sensor_bias_s bias;
|
||||
|
||||
if (_estimator_sensor_bias_sub.copy(&bias) && 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 (int 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) {
|
||||
/* EVENT
|
||||
* An accelerometer recalibration might help.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Axis {1}: |{2:.8}| > {3:.8} + {4:.8}
|
||||
*
|
||||
* This check can be configured via <param>EKF2_ABL_LIM</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<uint8_t, float, float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_high_accel_bias"),
|
||||
events::Log::Error, "High Accelerometer Bias", axis_index,
|
||||
bias.accel_bias[axis_index], ekf_ab_test_limit, test_uncertainty);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Accelerometer Bias");
|
||||
}
|
||||
|
||||
return; // avoid showing more than one error
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check gyro bias estimates
|
||||
if (bias.gyro_bias_valid) {
|
||||
const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit;
|
||||
|
||||
for (int 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) {
|
||||
/* EVENT
|
||||
* A Gyro recalibration might help.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Axis {1}: |{2:.8}| > {3:.8} + {4:.8}
|
||||
*
|
||||
* This check can be configured via <param>EKF2_ABL_GYRLIM</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<uint8_t, float, float, float>(required_groups, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_high_gyro_bias"),
|
||||
events::Log::Error, "High Gyro Bias", axis_index,
|
||||
bias.gyro_bias[axis_index], ekf_gb_test_limit, test_uncertainty);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: High Gyro Bias");
|
||||
}
|
||||
|
||||
return; // avoid showing more than one error
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
|
||||
|
||||
class EstimatorChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
EstimatorChecks() = default;
|
||||
~EstimatorChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
|
||||
NavModes required_groups);
|
||||
void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
|
||||
|
||||
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
|
||||
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
|
||||
(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_VEL>) _param_com_arm_ekf_vel,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_POS>) _param_com_arm_ekf_pos,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_YAW>) _param_com_arm_ekf_yaw,
|
||||
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
|
||||
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps
|
||||
)
|
||||
};
|
||||
@@ -0,0 +1,115 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "failureDetectorCheck.hpp"
|
||||
|
||||
void FailureDetectorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (context.status().failure_detector_status != vehicle_status_s::FAILURE_NONE) {
|
||||
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* The vehicle exceeded the maximum configured roll angle.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>FD_FAIL_R</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_roll"),
|
||||
events::Log::Critical, "Attitude failure detected");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (roll)");
|
||||
}
|
||||
|
||||
} else if (context.status().failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* The vehicle exceeded the maximum configured pitch angle.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>FD_FAIL_P</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_pitch"),
|
||||
events::Log::Critical, "Attitude failure detected");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Attitude failure (pitch)");
|
||||
}
|
||||
}
|
||||
|
||||
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ALT) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_alt"),
|
||||
events::Log::Critical, "Altitude failure detected");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Altitude failure");
|
||||
}
|
||||
}
|
||||
|
||||
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_EXT) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>FD_EXT_ATS_EN</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_failure_detector_ext"),
|
||||
events::Log::Critical, "Failure triggered by external system");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Failure triggered by external system");
|
||||
}
|
||||
}
|
||||
|
||||
if (context.status().failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* One or more ESCs failed to arm.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>FD_ESCS_EN</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_failure_detector_arm_esc"),
|
||||
events::Log::Critical, "ESC failure");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC failure detected");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
|
||||
class FailureDetectorChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
FailureDetectorChecks() = default;
|
||||
~FailureDetectorChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
};
|
||||
@@ -0,0 +1,131 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "gyroCheck.hpp"
|
||||
|
||||
#include <lib/sensor_calibration/Utilities.hpp>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
void GyroChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
for (int instance = 0; instance < _sensor_gyro_sub.size(); instance++) {
|
||||
const bool is_required = instance == 0 || isGyroRequired(instance);
|
||||
|
||||
if (!is_required) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const bool exists = _sensor_gyro_sub[instance].advertised();
|
||||
bool is_valid = false;
|
||||
bool is_calibration_valid = false;
|
||||
|
||||
if (exists) {
|
||||
sensor_gyro_s gyro_data;
|
||||
is_valid = _sensor_gyro_sub[instance].copy(&gyro_data) && (gyro_data.device_id != 0) && (gyro_data.timestamp != 0)
|
||||
&& (hrt_elapsed_time(&gyro_data.timestamp) < 1_s);
|
||||
|
||||
if (context.status().hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
is_calibration_valid = true;
|
||||
|
||||
} else {
|
||||
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro_data.device_id) >= 0);
|
||||
}
|
||||
|
||||
reporter.setIsPresent(health_component_t::gyro);
|
||||
}
|
||||
|
||||
const bool is_sensor_ok = is_valid && is_calibration_valid;
|
||||
|
||||
if (!is_sensor_ok) {
|
||||
if (!exists) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::gyro, events::ID("check_gyro_missing"),
|
||||
events::Log::Error, "Gyro sensor {1} missing", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro Sensor %u missing", instance);
|
||||
}
|
||||
|
||||
} else if (!is_valid) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::gyro, events::ID("check_gyro_no_data"),
|
||||
events::Log::Error, "No valid data from Gyro {1}", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid data from Gyro %u", instance);
|
||||
}
|
||||
|
||||
} else if (!is_calibration_valid) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure<uint8_t>(NavModes::All, health_component_t::gyro, events::ID("check_gyro_not_calibrated"),
|
||||
events::Log::Error, "Gyro {1} uncalibrated", instance);
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro %u uncalibrated", instance);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool GyroChecks::isGyroRequired(int instance)
|
||||
{
|
||||
sensor_gyro_s sensor_gyro;
|
||||
|
||||
if (!_sensor_gyro_sub[instance].copy(&sensor_gyro)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t device_id = static_cast<uint32_t>(sensor_gyro.device_id);
|
||||
|
||||
if (device_id == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool is_used_by_nav = false;
|
||||
|
||||
for (int i = 0; i < _estimator_status_sub.size(); i++) {
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
if (_estimator_status_sub[i].copy(&estimator_status) && estimator_status.gyro_device_id == device_id) {
|
||||
is_used_by_nav = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return is_used_by_nav;
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <lib/sensor_calibration/Gyroscope.hpp>
|
||||
|
||||
class GyroChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
GyroChecks() = default;
|
||||
~GyroChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
bool isGyroRequired(int instance);
|
||||
|
||||
uORB::SubscriptionMultiArray<sensor_gyro_s, calibration::Gyroscope::MAX_SENSOR_COUNT> _sensor_gyro_sub{ORB_ID::sensor_gyro};
|
||||
uORB::SubscriptionMultiArray<estimator_status_s> _estimator_status_sub{ORB_ID::estimator_status};
|
||||
};
|
||||
@@ -0,0 +1,118 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "imuConsistencyCheck.hpp"
|
||||
|
||||
void ImuConsistencyChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
sensors_status_imu_s imu;
|
||||
|
||||
if (!_sensors_status_imu_sub.copy(&imu) || context.isArmed()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
|
||||
const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
|
||||
|
||||
NavModes required_groups = NavModes::None;
|
||||
|
||||
if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get()) {
|
||||
required_groups = NavModes::All;
|
||||
}
|
||||
|
||||
if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get() * 0.8f) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Check the calibration.
|
||||
*
|
||||
* Inconsistency value: {2}.
|
||||
* Configured Threshold: {3}.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>COM_ARM_IMU_ACC</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<uint8_t, float, float>(required_groups, health_component_t::accel,
|
||||
events::ID("check_imu_accel_inconsistent"),
|
||||
events::Log::Warning, "Accel {1} inconsistent", i, accel_inconsistency_m_s_s, _param_com_arm_imu_acc.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Accel %u inconsistent - check cal", i);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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) {
|
||||
|
||||
const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
|
||||
|
||||
NavModes required_groups = NavModes::None;
|
||||
|
||||
if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get()) {
|
||||
required_groups = NavModes::All;
|
||||
}
|
||||
|
||||
if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get() * 0.5f) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Check the calibration.
|
||||
*
|
||||
* Inconsistency value: {2}.
|
||||
* Configured Threshold: {3}.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>COM_ARM_IMU_GYR</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure<uint8_t, float, float>(required_groups, health_component_t::gyro,
|
||||
events::ID("check_imu_gyro_inconsistent"),
|
||||
events::Log::Warning, "Gyro {1} inconsistent", i, gyro_inconsistency_rad_s, _param_com_arm_imu_gyr.get());
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Gyro %u inconsistent - check cal", i);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/sensors_status_imu.h>
|
||||
|
||||
class ImuConsistencyChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
ImuConsistencyChecks() = default;
|
||||
~ImuConsistencyChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
uORB::Subscription _sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamFloat<px4::params::COM_ARM_IMU_ACC>) _param_com_arm_imu_acc,
|
||||
(ParamFloat<px4::params::COM_ARM_IMU_GYR>) _param_com_arm_imu_gyr
|
||||
)
|
||||
};
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user