mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 03:23:35 +08:00
vehicle_status_flags: cleanup message, move non-failsafe flags to vehicle_status
This commit is contained in:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user