vehicle_status_flags: cleanup message, move non-failsafe flags to vehicle_status

This commit is contained in:
Beat Küng
2022-10-11 22:31:20 -04:00
committed by Daniel Agar
parent ae6377dfa0
commit e4bb219d10
30 changed files with 325 additions and 259 deletions
@@ -60,6 +60,7 @@ px4_add_library(health_and_arming_checks
checks/missionCheck.cpp
checks/rcAndDataLinkCheck.cpp
checks/vtolCheck.cpp
checks/offboardCheck.cpp
)
add_dependencies(health_and_arming_checks mode_util)
@@ -39,6 +39,18 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu
_context(status),
_reporter(status_flags)
{
// Initialize mode requirements to invalid
_failsafe_flags.angular_velocity_invalid = true;
_failsafe_flags.attitude_invalid = true;
_failsafe_flags.local_altitude_invalid = true;
_failsafe_flags.local_position_invalid = true;
_failsafe_flags.local_position_invalid_relaxed = true;
_failsafe_flags.local_velocity_invalid = true;
_failsafe_flags.global_position_invalid = true;
_failsafe_flags.gps_position_invalid = true;
_failsafe_flags.auto_mission_missing = true;
_failsafe_flags.offboard_control_signal_lost = true;
_failsafe_flags.home_position_invalid = true;
}
bool HealthAndArmingChecks::update(bool force_reporting)
@@ -65,6 +65,7 @@
#include "checks/missionCheck.hpp"
#include "checks/rcAndDataLinkCheck.hpp"
#include "checks/vtolCheck.hpp"
#include "checks/offboardCheck.hpp"
class HealthAndArmingChecks : public ModuleParams
@@ -132,6 +133,7 @@ private:
MissionChecks _mission_checks;
RcAndDataLinkChecks _rc_and_data_link_checks;
VtolChecks _vtol_checks;
OffboardChecks _offboard_checks;
HealthAndArmingCheckBase *_checks[30] = {
&_accelerometer_checks,
@@ -147,7 +149,9 @@ private:
&_magnetometer_checks,
&_manual_control_checks,
&_home_position_checks,
&_mode_checks, // must be after _estimator_checks & _home_position_checks
&_mission_checks,
&_offboard_checks, // must be after _estimator_checks
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
&_parachute_checks,
&_power_checks,
&_rc_calibration_checks,
@@ -157,7 +161,6 @@ private:
&_wind_checks,
&_geofence_checks, // must be after _home_position_checks
&_flight_time_checks,
&_mission_checks,
&_rc_and_data_link_checks,
&_vtol_checks,
};
@@ -119,11 +119,11 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
}
// set mode requirements
const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid;
const bool condition_gps_position_was_valid = !reporter.failsafeFlags().gps_position_invalid;
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
reporter.failsafeFlags());
if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) {
if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) {
gpsNoLongerValid(context, reporter);
}
}
@@ -658,7 +658,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
PX4_DEBUG("GPS no longer valid");
// report GPS failure if armed and the global position estimate is still valid
if (context.isArmed() && reporter.failsafeFlags().global_position_valid) {
if (context.isArmed() && !reporter.failsafeFlags().global_position_invalid) {
if (reporter.mavlink_log_pub()) {
mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t");
}
@@ -706,26 +706,26 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
}
}
failsafe_flags.global_position_valid =
checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
_last_gpos_fail_time_us, failsafe_flags.global_position_valid);
failsafe_flags.global_position_invalid =
!checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
failsafe_flags.local_position_valid =
checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
_last_lpos_fail_time_us, failsafe_flags.local_position_valid);
failsafe_flags.local_position_invalid =
!checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
_last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
failsafe_flags.local_position_valid_relaxed =
checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
_last_lpos_relaxed_fail_time_us, failsafe_flags.local_position_valid);
failsafe_flags.local_position_invalid_relaxed =
!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
_last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
failsafe_flags.local_velocity_valid =
checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
_last_lvel_fail_time_us, failsafe_flags.local_velocity_valid);
failsafe_flags.local_velocity_invalid =
!checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
_last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid);
// altitude
failsafe_flags.local_altitude_valid = lpos.z_valid
&& (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s));
failsafe_flags.local_altitude_invalid = !lpos.z_valid
|| (now - lpos.timestamp > (_param_com_pos_fs_delay.get() * 1_s));
// attitude
@@ -740,11 +740,11 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
&& (fabsf(q(3)) <= 1.f + eps);
const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps;
failsafe_flags.attitude_valid = now < attitude.timestamp + 1_s && norm_in_tolerance
&& no_element_larger_than_one;
failsafe_flags.attitude_invalid = now > attitude.timestamp + 1_s || !norm_in_tolerance
|| !no_element_larger_than_one;
} else {
failsafe_flags.attitude_valid = false;
failsafe_flags.attitude_invalid = true;
}
// angular velocity
@@ -754,13 +754,13 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
&& now < angular_velocity.timestamp + 1_s;
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
const bool angular_velocity_valid = condition_angular_velocity_time_valid
&& condition_angular_velocity_finite;
const bool angular_velocity_invalid = !condition_angular_velocity_time_valid
|| !condition_angular_velocity_finite;
if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) {
if (!failsafe_flags.angular_velocity_invalid && angular_velocity_invalid) {
const char err_str[] {"angular velocity no longer valid"};
if (!condition_angular_velocity_time_valid) {
if (!condition_angular_velocity_time_valid && angular_velocity.timestamp != 0) {
PX4_ERR("%s (timeout)", err_str);
} else if (!condition_angular_velocity_finite) {
@@ -768,7 +768,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
}
}
failsafe_flags.angular_velocity_valid = angular_velocity_valid;
failsafe_flags.angular_velocity_invalid = angular_velocity_invalid;
// gps
@@ -782,10 +782,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL);
_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now);
failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
failsafe_flags.gps_position_invalid = !_vehicle_gps_position_valid.get_state();
} else {
failsafe_flags.gps_position_valid = false;
failsafe_flags.gps_position_invalid = true;
}
}
@@ -59,7 +59,7 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
}
if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL
&& !reporter.failsafeFlags().home_position_valid) {
&& reporter.failsafeFlags().home_position_invalid) {
/* EVENT
* @description
* <profile name="dev">
@@ -38,10 +38,10 @@ void HomePositionChecks::checkAndReport(const Context &context, Report &reporter
home_position_s home_position;
if (_home_position_sub.copy(&home_position)) {
reporter.failsafeFlags().home_position_valid = home_position.valid_alt && home_position.valid_hpos;
reporter.failsafeFlags().home_position_invalid = !home_position.valid_alt || !home_position.valid_hpos;
} else {
reporter.failsafeFlags().home_position_valid = false;
reporter.failsafeFlags().home_position_invalid = true;
}
// No need to report, as it's a mode requirement
@@ -36,6 +36,7 @@
void MissionChecks::checkAndReport(const Context &context, Report &reporter)
{
reporter.failsafeFlags().mission_failure = false;
reporter.failsafeFlags().auto_mission_missing = true;
mission_result_s mission_result;
if (_mission_result_sub.copy(&mission_result) && mission_result.valid) {
@@ -53,5 +54,8 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter)
mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t");
}
}
// This is a mode requirement, no need to report
reporter.failsafeFlags().auto_mission_missing = mission_result.instance_count <= 0;
}
}
@@ -42,7 +42,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
// Failing mode requirements generally also clear the can_run bits which prevents mode switching and
// might trigger a failsafe if already in that mode.
if (!reporter.failsafeFlags().angular_velocity_valid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
if (reporter.failsafeFlags().angular_velocity_invalid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
/* EVENT
* @description
* Make sure the gyroscope is providing valid data.
@@ -53,7 +53,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_angular_velocity);
}
if (!reporter.failsafeFlags().attitude_valid && reporter.failsafeFlags().mode_req_attitude != 0) {
if (reporter.failsafeFlags().attitude_invalid && reporter.failsafeFlags().mode_req_attitude != 0) {
/* EVENT
* @description
* Wait until the estimator initialized
@@ -66,11 +66,11 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
NavModes local_position_modes = NavModes::None;
if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) {
if (reporter.failsafeFlags().local_position_invalid && reporter.failsafeFlags().mode_req_local_position != 0) {
local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position;
}
if (!reporter.failsafeFlags().local_position_valid_relaxed
if (reporter.failsafeFlags().local_position_invalid_relaxed
&& reporter.failsafeFlags().mode_req_local_position_relaxed != 0) {
local_position_modes = local_position_modes | (NavModes)reporter.failsafeFlags().mode_req_local_position_relaxed;
}
@@ -84,7 +84,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits(local_position_modes);
}
if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) {
if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
/* EVENT
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system,
@@ -93,7 +93,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
}
if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) {
if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
/* EVENT
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system,
@@ -102,12 +102,22 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt);
}
if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) {
NavModes mission_required_modes = (NavModes)reporter.failsafeFlags().mode_req_mission;
if (_param_com_arm_mis_req.get()) {
mission_required_modes = NavModes::All;
}
if (reporter.failsafeFlags().auto_mission_missing && mission_required_modes != NavModes::None) {
/* EVENT
* @description
* Upload a mission first.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
* </profile>
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system,
reporter.armingCheckFailure(mission_required_modes, health_component_t::system,
events::ID("check_modes_mission"),
events::Log::Info, "No valid mission available");
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission);
@@ -116,7 +126,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) {
/* EVENT
* @description
* The offboard component is not sending setpoints.
* The offboard component is not sending setpoints or the required estimate (e.g. position) is missing.
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system,
events::ID("check_modes_offboard_signal"),
@@ -124,7 +134,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal);
}
if (!reporter.failsafeFlags().home_position_valid && reporter.failsafeFlags().mode_req_home_position != 0) {
if (reporter.failsafeFlags().home_position_invalid && reporter.failsafeFlags().mode_req_home_position != 0) {
/* EVENT
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_home_position, health_component_t::system,
@@ -48,4 +48,7 @@ public:
private:
void checkArmingRequirement(const Context &context, Report &reporter);
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req
);
};
@@ -0,0 +1,70 @@
/****************************************************************************
*
* 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 "offboardCheck.hpp"
using namespace time_literals;
OffboardChecks::OffboardChecks()
{
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
}
void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
{
reporter.failsafeFlags().offboard_control_signal_lost = true;
offboard_control_mode_s offboard_control_mode;
if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
|| offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
|| offboard_control_mode.actuator;
if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
offboard_available = false;
} else if (offboard_control_mode.velocity && reporter.failsafeFlags().local_velocity_invalid) {
offboard_available = false;
} else if (offboard_control_mode.acceleration && reporter.failsafeFlags().local_velocity_invalid) {
// OFFBOARD acceleration handled by position controller
offboard_available = false;
}
_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
// This is a mode requirement, no need to report
reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
}
}
@@ -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/topics/offboard_control_mode.h>
#include <lib/hysteresis/hysteresis.h>
class OffboardChecks : public HealthAndArmingCheckBase
{
public:
OffboardChecks();
~OffboardChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
systemlib::Hysteresis _offboard_available{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t
);
};
@@ -72,29 +72,9 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
}
}
// Arm Requirements: mission
if (_param_com_arm_mis_req.get() && !context.isArmed()) {
if (!context.status().auto_mission_available) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
* </profile>
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_mission"),
events::Log::Error, "No valid mission");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid mission");
}
}
}
reporter.failsafeFlags().auto_mission_available = context.status().auto_mission_available;
// Global position required
if (!_param_com_arm_wo_gps.get() && !context.isArmed()) {
if (!reporter.failsafeFlags().global_position_valid) {
if (reporter.failsafeFlags().global_position_invalid) {
/* EVENT
* @description
* <profile name="dev">
@@ -108,7 +88,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required");
}
} else if (!reporter.failsafeFlags().home_position_valid) {
} else if (reporter.failsafeFlags().home_position_invalid) {
/* EVENT
* @description
* <profile name="dev">
@@ -52,7 +52,6 @@ private:
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action