mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-13 00:57:34 +08:00
commander preflight checks: new structure using events interface
- Reporter class that keeps track of results (list of events + whether arming is possible + health). And only reports when one or more checks change.
This commit is contained in:
Beat Küng
@@ -32,6 +32,7 @@
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(failure_detector)
|
||||
add_subdirectory(HealthAndArmingChecks)
|
||||
add_subdirectory(Arming)
|
||||
|
||||
px4_add_module(
|
||||
@@ -59,6 +60,7 @@ px4_add_module(
|
||||
circuit_breaker
|
||||
failure_detector
|
||||
geo
|
||||
health_and_arming_checks
|
||||
hysteresis
|
||||
PreFlightCheck
|
||||
ArmAuthorization
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(health_and_arming_checks
|
||||
Common.cpp
|
||||
HealthAndArmingChecks.cpp
|
||||
)
|
||||
|
||||
@@ -0,0 +1,243 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "Common.hpp"
|
||||
|
||||
void Report::healthFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message)
|
||||
{
|
||||
healthFailure(required_modes, component, log_levels.external);
|
||||
addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index);
|
||||
}
|
||||
|
||||
void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message)
|
||||
{
|
||||
armingCheckFailure(required_modes, component, log_levels.external);
|
||||
addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index);
|
||||
}
|
||||
|
||||
NavModes Report::reportedModes(NavModes required_modes)
|
||||
{
|
||||
// Make sure optional checks are still shown in the UI
|
||||
if (required_modes == NavModes::None) {
|
||||
return NavModes::All;
|
||||
}
|
||||
|
||||
return required_modes;
|
||||
}
|
||||
|
||||
void Report::setIsPresent(health_component_t component)
|
||||
{
|
||||
HealthResults &health = _results[_current_result].health;
|
||||
health.is_present = health.is_present | component;
|
||||
}
|
||||
|
||||
void Report::setHealth(health_component_t component, bool is_present, bool warning, bool error)
|
||||
{
|
||||
HealthResults &health = _results[_current_result].health;
|
||||
|
||||
if (is_present) {
|
||||
health.is_present = health.is_present | component;
|
||||
}
|
||||
|
||||
if (warning) {
|
||||
health.warning = health.warning | component;
|
||||
}
|
||||
|
||||
if (error) {
|
||||
health.error = health.error | component;
|
||||
}
|
||||
}
|
||||
|
||||
void Report::healthFailure(NavModes required_modes, HealthComponentIndex component, events::Log log_level)
|
||||
{
|
||||
// update current health results
|
||||
HealthResults &health = _results[_current_result].health;
|
||||
|
||||
if (log_level <= events::Log::Error) {
|
||||
health.error = health.error | (events::px4::enums::health_component_t)(1 << component.index);
|
||||
|
||||
} else if (log_level <= events::Log::Warning) {
|
||||
health.warning = health.warning | (events::px4::enums::health_component_t)(1 << component.index);
|
||||
}
|
||||
|
||||
// clear relevant arming bits
|
||||
clearArmingBits(required_modes);
|
||||
}
|
||||
|
||||
void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex component, events::Log log_level)
|
||||
{
|
||||
// update current arming check results
|
||||
ArmingCheckResults &arming_checks = _results[_current_result].arming_checks;
|
||||
|
||||
if (log_level <= events::Log::Error) {
|
||||
arming_checks.error = arming_checks.error | (events::px4::enums::health_component_t)(1 << component.index);
|
||||
|
||||
} else if (log_level <= events::Log::Warning) {
|
||||
arming_checks.warning = arming_checks.warning | (events::px4::enums::health_component_t)(1 << component.index);
|
||||
}
|
||||
|
||||
// clear relevant arming bits
|
||||
clearArmingBits(required_modes);
|
||||
}
|
||||
|
||||
void Report::clearArmingBits(NavModes modes)
|
||||
{
|
||||
ArmingCheckResults &arming_checks = _results[_current_result].arming_checks;
|
||||
NavModes &can_arm = arming_checks.can_arm;
|
||||
can_arm = can_arm & ~modes;
|
||||
}
|
||||
|
||||
void Report::clearCanRunBits(NavModes modes)
|
||||
{
|
||||
ArmingCheckResults &arming_checks = _results[_current_result].arming_checks;
|
||||
NavModes &can_run = arming_checks.can_run;
|
||||
can_run = can_run & ~modes;
|
||||
}
|
||||
|
||||
void Report::reset()
|
||||
{
|
||||
_current_result = (_current_result + 1) % 2;
|
||||
_results[_current_result].reset();
|
||||
_next_buffer_idx = 0;
|
||||
_buffer_overflowed = false;
|
||||
_failing_checks_prevent_mode_switch = 0;
|
||||
}
|
||||
|
||||
NavModes Report::getModeGroup(uint8_t nav_state) const
|
||||
{
|
||||
// Note: this needs to match with the json metadata definition "navigation_mode_groups"
|
||||
return (NavModes)(1u << nav_state);
|
||||
}
|
||||
|
||||
void Report::finalize()
|
||||
{
|
||||
_results[_current_result].arming_checks.valid = true;
|
||||
_already_reported = false;
|
||||
}
|
||||
|
||||
bool Report::report(bool is_armed, bool force)
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
const bool has_difference = _had_unreported_difference || _results[0] != _results[1];
|
||||
|
||||
if (now - _last_report < min_reporting_interval && !force) {
|
||||
if (has_difference) {
|
||||
_had_unreported_difference = true;
|
||||
}
|
||||
|
||||
PX4_DEBUG("not reporting yet (%i)", has_difference);
|
||||
return false;
|
||||
}
|
||||
|
||||
const bool need_to_report = (has_difference && !_already_reported) || force;
|
||||
|
||||
if (!need_to_report) {
|
||||
PX4_DEBUG("not reporting, no update (%i)", has_difference);
|
||||
return false;
|
||||
}
|
||||
|
||||
const Results ¤t_results = _results[_current_result];
|
||||
|
||||
// If we have too many events, the result is still correct, we just don't report everything
|
||||
const int max_num_events = event_s::ORB_QUEUE_LENGTH - 2;
|
||||
|
||||
if (_buffer_overflowed || current_results.num_events > max_num_events) {
|
||||
PX4_WARN("Too many arming check events (%i, %i > %i). Not reporting all", _buffer_overflowed,
|
||||
current_results.num_events, max_num_events);
|
||||
}
|
||||
|
||||
if (!current_results.arming_checks.valid) {
|
||||
PX4_ERR("BUG: arming checks not valid");
|
||||
}
|
||||
|
||||
_last_report = now;
|
||||
_already_reported = true;
|
||||
_had_unreported_difference = false;
|
||||
PX4_DEBUG("Sending Arming/Health report (num events: %i, can_arm: 0x%x)", _results[_current_result].num_events + 2,
|
||||
(int)current_results.arming_checks.can_arm);
|
||||
|
||||
// send arming summary
|
||||
navigation_mode_group_t can_arm_and_run;
|
||||
|
||||
if (is_armed) {
|
||||
can_arm_and_run = (navigation_mode_group_t)current_results.arming_checks.can_run;
|
||||
|
||||
} else {
|
||||
can_arm_and_run = (navigation_mode_group_t)current_results.arming_checks.can_arm;
|
||||
}
|
||||
|
||||
/* EVENT
|
||||
* @arg1 chunk_idx
|
||||
* @arg2 error
|
||||
* @arg3 warning
|
||||
* @arg4 can_arm_and_run
|
||||
* @type summary
|
||||
* @group arming_check
|
||||
*/
|
||||
events::send<uint8_t, events::px4::enums::health_component_t, events::px4::enums::health_component_t,
|
||||
events::px4::enums::navigation_mode_group_t>(events::ID("commander_arming_check_summary"), events::Log::Protocol,
|
||||
"Arming check summary event", 0, current_results.arming_checks.error, current_results.arming_checks.warning,
|
||||
can_arm_and_run);
|
||||
|
||||
// send all events
|
||||
int offset = 0;
|
||||
events::EventType event;
|
||||
|
||||
for (int i = 0; i < max_num_events && offset < _next_buffer_idx; ++i) {
|
||||
EventBufferHeader *header = (EventBufferHeader *)(_event_buffer + offset);
|
||||
memcpy(&event.id, &header->id, sizeof(event.id));
|
||||
event.log_levels = header->log_levels;
|
||||
memcpy(event.arguments, _event_buffer + offset + sizeof(EventBufferHeader), header->size);
|
||||
memset(event.arguments + header->size, 0, sizeof(event.arguments) - header->size);
|
||||
events::send(event);
|
||||
offset += sizeof(EventBufferHeader) + header->size;
|
||||
}
|
||||
|
||||
// send health summary
|
||||
/* EVENT
|
||||
* @arg1 chunk_idx
|
||||
* @arg2 is_present
|
||||
* @arg3 error
|
||||
* @arg4 warning
|
||||
* @type summary
|
||||
* @group health
|
||||
*/
|
||||
events::send<uint8_t, events::px4::enums::health_component_t, events::px4::enums::health_component_t,
|
||||
events::px4::enums::health_component_t>(events::ID("commander_health_summary"),
|
||||
events::Log::Protocol,
|
||||
"Health report summary event", 0, current_results.health.is_present,
|
||||
current_results.health.error, current_results.health.warning);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,378 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/events.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <limits.h>
|
||||
|
||||
#if 0 // for debugging, prints event messages to console each time arming & health checks run (not necessarily change)
|
||||
#include <px4_platform_common/log.h>
|
||||
#define CONSOLE_PRINT_ARMING_CHECK_EVENT(log_level, id, str) PX4_INFO_RAW("Health/Arming Event 0x%08x: %s\n", id, str)
|
||||
#else
|
||||
#define CONSOLE_PRINT_ARMING_CHECK_EVENT(log_level, id, str)
|
||||
#endif
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class HealthAndArmingChecks;
|
||||
|
||||
using navigation_mode_group_t = events::px4::enums::navigation_mode_group_t;
|
||||
using health_component_t = events::px4::enums::health_component_t;
|
||||
|
||||
enum class NavModes : uint32_t {
|
||||
None = 0, ///< Using NavModes = None means arming is still possible (optional check)
|
||||
|
||||
// Add the modes here as needed, but generally rather use mode requirements instead of checks for individual modes.
|
||||
Manual = (uint32_t)navigation_mode_group_t::manual,
|
||||
Stabilized = (uint32_t)navigation_mode_group_t::stab,
|
||||
PositionControl = (uint32_t)navigation_mode_group_t::posctl,
|
||||
Mission = (uint32_t)navigation_mode_group_t::mission,
|
||||
Takeoff = (uint32_t)navigation_mode_group_t::takeoff,
|
||||
|
||||
All = 0xffffffff
|
||||
};
|
||||
static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatch");
|
||||
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t),
|
||||
"type too small, use next larger type");
|
||||
|
||||
static inline NavModes operator|(NavModes a, NavModes b)
|
||||
{
|
||||
return static_cast<NavModes>(static_cast<uint32_t>(a) | static_cast<uint32_t>(b));
|
||||
}
|
||||
|
||||
static inline NavModes operator&(NavModes a, NavModes b)
|
||||
{
|
||||
return static_cast<NavModes>(static_cast<uint32_t>(a) & static_cast<uint32_t>(b));
|
||||
}
|
||||
|
||||
static inline NavModes operator~(NavModes a)
|
||||
{
|
||||
return static_cast<NavModes>(~static_cast<uint32_t>(a));
|
||||
}
|
||||
|
||||
class HealthComponentIndex
|
||||
{
|
||||
public:
|
||||
constexpr uint8_t log2(uint64_t x)
|
||||
{
|
||||
uint8_t i = 0;
|
||||
|
||||
while (x > 1) {
|
||||
x >>= 1;
|
||||
++i;
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
constexpr HealthComponentIndex(health_component_t component)
|
||||
: index(log2((uint64_t)component))
|
||||
{
|
||||
}
|
||||
const uint8_t index;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class Context
|
||||
* Provides commonly used information for health and arming checks
|
||||
*/
|
||||
class Context
|
||||
{
|
||||
public:
|
||||
Context(const vehicle_status_s &status)
|
||||
: _status(status)
|
||||
{}
|
||||
|
||||
~Context() = default;
|
||||
|
||||
bool isArmed() const { return _status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; }
|
||||
|
||||
const vehicle_status_s &status() const { return _status; }
|
||||
|
||||
private:
|
||||
const vehicle_status_s &_status;
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @class Report
|
||||
* Keeps track of health and arming report and reports the results whenever something changes.
|
||||
*/
|
||||
class Report
|
||||
{
|
||||
public:
|
||||
|
||||
struct HealthResults {
|
||||
HealthResults() { reset(); }
|
||||
|
||||
health_component_t is_present;
|
||||
health_component_t error;
|
||||
health_component_t warning;
|
||||
|
||||
void reset()
|
||||
{
|
||||
is_present = {};
|
||||
error = {};
|
||||
warning = {};
|
||||
}
|
||||
bool operator!=(const HealthResults &other)
|
||||
{
|
||||
return is_present != other.is_present || error != other.error ||
|
||||
warning != other.warning;
|
||||
}
|
||||
};
|
||||
|
||||
struct ArmingCheckResults {
|
||||
ArmingCheckResults() { reset(); }
|
||||
|
||||
health_component_t error;
|
||||
health_component_t warning;
|
||||
|
||||
NavModes can_arm; ///< whether arming is possible for each mode group (bitset)
|
||||
NavModes can_run; ///< whether switching into a certain mode is possible (while armed)
|
||||
|
||||
bool valid; ///< whether can_arm is valid, i.e. can be used
|
||||
|
||||
void reset()
|
||||
{
|
||||
error = {};
|
||||
warning = {};
|
||||
can_arm = (NavModes) - 1; // bits are cleared for failed checks
|
||||
can_run = (NavModes) - 1;
|
||||
valid = false;
|
||||
}
|
||||
bool operator!=(const ArmingCheckResults &other)
|
||||
{
|
||||
return error != other.error || warning != other.warning ||
|
||||
can_arm != other.can_arm || can_run != other.can_run || valid != other.valid;
|
||||
}
|
||||
};
|
||||
|
||||
Report(vehicle_status_flags_s &status_flags) : _status_flags(status_flags) { }
|
||||
~Report() = default;
|
||||
|
||||
vehicle_status_flags_s &failsafeFlags() { return _status_flags; }
|
||||
|
||||
/**
|
||||
* Whether arming is possible for a given navigation mode
|
||||
*/
|
||||
bool canArm(uint8_t nav_state) const
|
||||
{
|
||||
return _results[_current_result].arming_checks.valid &&
|
||||
(uint32_t)(_results[_current_result].arming_checks.can_arm & getModeGroup(nav_state)) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Whether a navigation mode can be run (while already armed)
|
||||
*/
|
||||
bool canRun(uint8_t nav_state) const
|
||||
{
|
||||
return _results[_current_result].arming_checks.valid &&
|
||||
(uint32_t)(_results[_current_result].arming_checks.can_run & getModeGroup(nav_state)) != 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Report a health failure. A health issue generally refers to a hardware issue, independent from environment
|
||||
* or e.g. calibration.
|
||||
*/
|
||||
template<typename... Args>
|
||||
void healthFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message, Args... args);
|
||||
|
||||
void healthFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message);
|
||||
|
||||
void setIsPresent(health_component_t component);
|
||||
|
||||
/**
|
||||
* Directly set the health of a component. Using healthFailure() and setIsPresent() is preferred
|
||||
*/
|
||||
void setHealth(health_component_t component, bool is_present, bool warning, bool error);
|
||||
|
||||
|
||||
/**
|
||||
* Report an arming check failure. If required_modes is None, arming is still possible.
|
||||
*/
|
||||
template<typename... Args>
|
||||
void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message, Args... args);
|
||||
|
||||
void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message);
|
||||
|
||||
/**
|
||||
* Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
|
||||
* mode is being run.
|
||||
* @param modes affected modes
|
||||
*/
|
||||
void clearCanRunBits(NavModes modes);
|
||||
|
||||
const HealthResults &healthResults() const { return _results[_current_result].health; }
|
||||
const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
|
||||
private:
|
||||
|
||||
/**
|
||||
* Stores all results, and used to compare against results from the previous run,
|
||||
* to know if we need to report. Note that only changed event arguments will not
|
||||
* trigger reporting, which should generally be desirable
|
||||
* (e.g. to avoid frequent updates due to changing floats).
|
||||
*/
|
||||
struct Results {
|
||||
Results() { reset(); }
|
||||
|
||||
HealthResults health;
|
||||
ArmingCheckResults arming_checks;
|
||||
int num_events;
|
||||
uint32_t event_id_hash; ///< Simple hash over all current event ID's
|
||||
|
||||
void reset() { health.reset(); arming_checks.reset(); num_events = 0; event_id_hash = 0; }
|
||||
|
||||
bool operator!=(const Results &other)
|
||||
{
|
||||
return health != other.health || arming_checks != other.arming_checks ||
|
||||
num_events != other.num_events || event_id_hash != other.event_id_hash;
|
||||
}
|
||||
};
|
||||
|
||||
struct __attribute__((__packed__)) EventBufferHeader {
|
||||
uint8_t size; ///< arguments size
|
||||
uint32_t id;
|
||||
uint8_t log_levels;
|
||||
};
|
||||
|
||||
void healthFailure(NavModes required_modes, HealthComponentIndex component, events::Log log_level);
|
||||
void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, events::Log log_level);
|
||||
|
||||
template<typename... Args>
|
||||
bool addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, Args... args);
|
||||
|
||||
NavModes reportedModes(NavModes required_modes);
|
||||
|
||||
void clearArmingBits(NavModes modes);
|
||||
|
||||
NavModes getModeGroup(uint8_t nav_state) const;
|
||||
|
||||
friend class HealthAndArmingChecks;
|
||||
/**
|
||||
* Reset current results.
|
||||
* The calling order needs to be:
|
||||
* - reset()
|
||||
* - run all checks
|
||||
* - finalize()
|
||||
* - report() (which can be called independently as well)
|
||||
*/
|
||||
void reset();
|
||||
void finalize();
|
||||
|
||||
bool report(bool is_armed, bool force);
|
||||
|
||||
static constexpr hrt_abstime min_reporting_interval{2_s};
|
||||
|
||||
/// event buffer: stores current events + arguments.
|
||||
/// Since the amount of extra arguments varies, 4 bytes is used here as estimate
|
||||
uint8_t _event_buffer[(event_s::ORB_QUEUE_LENGTH - 2) * (sizeof(EventBufferHeader) + 1 + 1 + 4)];
|
||||
int _next_buffer_idx{0};
|
||||
bool _buffer_overflowed{false};
|
||||
|
||||
bool _already_reported{false};
|
||||
bool _had_unreported_difference{false}; ///< true if there was a difference not reported yet (due to rate limitation)
|
||||
hrt_abstime _last_report{0};
|
||||
|
||||
Results _results[2]; ///< Previous and current results to check for changes
|
||||
int _current_result{0};
|
||||
|
||||
vehicle_status_flags_s &_status_flags;
|
||||
};
|
||||
|
||||
template<typename... Args>
|
||||
void Report::healthFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message, Args... args)
|
||||
{
|
||||
healthFailure(required_modes, component, log_levels.external);
|
||||
addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), (uint8_t)component.index, args...);
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
|
||||
const events::LogLevels &log_levels, const char *message, Args... args)
|
||||
{
|
||||
armingCheckFailure(required_modes, component, log_levels.external);
|
||||
addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), (uint8_t)component.index, args...);
|
||||
}
|
||||
|
||||
template<typename... Args>
|
||||
bool Report::addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, Args... args)
|
||||
{
|
||||
CONSOLE_PRINT_ARMING_CHECK_EVENT(log_level, event_id, message);
|
||||
|
||||
constexpr unsigned args_size = events::util::sizeofArguments(args...);
|
||||
static_assert(args_size <= sizeof(events::EventType::arguments), "Too many arguments");
|
||||
unsigned total_size = sizeof(EventBufferHeader) + args_size;
|
||||
|
||||
if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
|
||||
_buffer_overflowed = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
EventBufferHeader *header = (EventBufferHeader *)(_event_buffer + _next_buffer_idx);
|
||||
memcpy(&header->id, &event_id, sizeof(event_id)); // header might be unaligned
|
||||
header->log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
|
||||
header->size = args_size;
|
||||
events::util::fillEventArguments(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), args...);
|
||||
_next_buffer_idx += total_size;
|
||||
++_results[_current_result].num_events;
|
||||
_results[_current_result].event_id_hash ^= event_id; // very simple hash
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @class HealthAndArmingCheckBase
|
||||
* Base class for all checks
|
||||
*/
|
||||
class HealthAndArmingCheckBase : public ModuleParams
|
||||
{
|
||||
public:
|
||||
HealthAndArmingCheckBase() : ModuleParams(nullptr) {};
|
||||
~HealthAndArmingCheckBase() = default;
|
||||
|
||||
virtual void checkAndReport(const Context &context, Report &reporter) = 0;
|
||||
|
||||
void updateParams() override { ModuleParams::updateParams(); }
|
||||
};
|
||||
@@ -0,0 +1,75 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "HealthAndArmingChecks.hpp"
|
||||
|
||||
HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags,
|
||||
vehicle_status_s &status)
|
||||
: ModuleParams(parent),
|
||||
_context(status),
|
||||
_reporter(status_flags)
|
||||
{
|
||||
}
|
||||
|
||||
bool HealthAndArmingChecks::update(bool force_reporting)
|
||||
{
|
||||
_reporter.reset();
|
||||
|
||||
for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
|
||||
if (!_checks[i]) {
|
||||
break;
|
||||
}
|
||||
|
||||
_checks[i]->checkAndReport(_context, _reporter);
|
||||
}
|
||||
|
||||
_reporter.finalize();
|
||||
|
||||
if (_reporter.report(_context.isArmed(), force_reporting)) {
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void HealthAndArmingChecks::updateParams()
|
||||
{
|
||||
for (unsigned i = 0; i < sizeof(_checks) / sizeof(_checks[0]); ++i) {
|
||||
if (!_checks[i]) {
|
||||
break;
|
||||
}
|
||||
|
||||
_checks[i]->updateParams();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Common.hpp"
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
class HealthAndArmingChecks : public ModuleParams
|
||||
{
|
||||
public:
|
||||
HealthAndArmingChecks(ModuleParams *parent, vehicle_status_flags_s &status_flags, vehicle_status_s &status);
|
||||
~HealthAndArmingChecks() = default;
|
||||
|
||||
/**
|
||||
* Run arming checks and report if necessary.
|
||||
* This should be called regularly (e.g. 1Hz).
|
||||
* @param force_reporting if true, force reporting even if nothing changed
|
||||
* @return true if there was a report (also when force_reporting=true)
|
||||
*/
|
||||
bool update(bool force_reporting = false);
|
||||
|
||||
/**
|
||||
* Whether arming is possible for a given navigation mode
|
||||
*/
|
||||
bool canArm(uint8_t nav_state) const { return _reporter.canArm(nav_state); }
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
private:
|
||||
Context _context;
|
||||
Report _reporter;
|
||||
|
||||
// all checks
|
||||
HealthAndArmingCheckBase *_checks[10] = {
|
||||
};
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user