fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander preflight checks: new structure using events interface

Browse Source 
- 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 2020-01-27 09:55:41 +01:00 committed by Daniel Agar
parent 04f7df3848
commit 07afcf4586
9 changed files with 1033 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/px4events/jsonout.py
View File

@ -25,6 +25,8 @@ class JsonOutput():
event_obj = {}
event_obj['name'] = e.name
event_obj['message'] = e.message
if e.type is not None:
event_obj['type'] = e.type
if e.description is not None:
event_obj['description'] = e.description
args = []

5
Tools/px4events/srcparser.py
View File

@ -23,6 +23,7 @@ class Event(object):
self.message = None
self.description = None
self.group = "default"
self.type = None
self._arguments = []
@staticmethod
@ -130,6 +131,8 @@ class SourceParser(object):
if not event.group in known_groups:
raise Exception("Unknown event group: '{}'\nKnown groups: {}\n" \
"If this is not a typo, add the new group to the script".format(event.group, known_groups))
elif tag == "type":
event.type = value.strip()
elif tag.startswith("arg"):
arg_index = int(tag[3:])-1
arg_name = value.strip()
@ -237,7 +240,7 @@ class SourceParser(object):
event.group = "health"
else:
event.group = "arming_check"
event.prepend_arguments([('common::navigation_mode_category_t', 'modes'),
event.prepend_arguments([('navigation_mode_group_t', 'modes'),
('uint8_t', 'health_component_index')])
else:
raise Exception("unknown event method call: {}, args: {}".format(call, args))

223
src/lib/events/enums.json
View File

@ -4,6 +4,207 @@
"1": {
"namespace": "px4",
"enums": {
"navigation_mode_group_t": {
"type": "uint32_t",
"description": "Navigation/flight mode group bits. All modes within a group share the same arming checks. For now we directly map group == mode.",
"is_bitfield": true,
"entries": {
"1": {
"name": "manual",
"description": "Fully manual mode (w/o any controller support)"
},
"2": {
"name": "altctl",
"description": "Altitude mode"
},
"4": {
"name": "posctl",
"description": "Position mode"
},
"8": {
"name": "mission",
"description": "Mission mode"
},
"16": {
"name": "loiter",
"description": "Loiter"
},
"32": {
"name": "rtl",
"description": "RTL"
},
"1024": {
"name": "acro",
"description": "Acro"
},
"16384": {
"name": "offboard",
"description": "Offboard"
},
"32768": {
"name": "stab",
"description": "Stabilized"
},
"131072": {
"name": "takeoff",
"description": "Takeoff"
},
"262144": {
"name": "land",
"description": "Land"
},
"524288": {
"name": "follow_target",
"description": "Follow Target"
},
"1048576": {
"name": "precland",
"description": "Precision Land"
},
"2097152": {
"name": "orbit",
"description": "Orbit"
},
"4194304": {
"name": "vtol_takeoff",
"description": "VTOL Takeoff"
}
}
},
"health_component_t": {
"type": "uint32_t",
"description": "Bitfield for subsystems & components",
"is_bitfield": true,
"entries": {
"1": {
"name": "none",
"description": "None",
"comment": "Not assigned to any component"
},
"2": {
"name": "absolute_pressure",
"description": "Absolute pressure"
},
"4": {
"name": "differential_pressure",
"description": "Differential pressure"
},
"8": {
"name": "gps",
"description": "GPS"
},
"16": {
"name": "optical_flow",
"description": "Optical flow"
},
"32": {
"name": "vision_position",
"description": "Vision position estimate"
},
"64": {
"name": "distance_sensor",
"description": "Distance sensor"
},
"128": {
"name": "manual_control_input",
"description": "RC or virtual joystick input"
},
"256": {
"name": "motors_escs",
"description": "Motors/ESCs"
},
"512": {
"name": "utm",
"description": "UTM"
},
"1024": {
"name": "logging",
"description": "Logging"
},
"2048": {
"name": "battery",
"description": "Battery"
},
"4096": {
"name": "communication_links",
"description": "Communication links"
},
"8192": {
"name": "rate_controller",
"description": "Rate controller"
},
"16384": {
"name": "attitude_controller",
"description": "Attitude controller"
},
"32768": {
"name": "position_controller",
"description": "Position controller"
},
"65536": {
"name": "attitude_estimate",
"description": "Attitude estimate"
},
"131072": {
"name": "local_position_estimate",
"description": "Local position estimate"
},
"262144": {
"name": "mission",
"description": "Mission"
},
"524288": {
"name": "avoidance",
"description": "Avoidance"
},
"1048576": {
"name": "system",
"description": "System",
"comment": "e.g. CPU or RAM"
},
"2097152": {
"name": "camera",
"description": "Camera"
},
"4194304": {
"name": "gimbal",
"description": "Gimbal"
},
"8388608": {
"name": "payload",
"description": "Payload"
},
"16777216": {
"name": "global_position_estimate",
"description": "Global position estimate"
},
"33554432": {
"name": "storage",
"description": "Storage",
"comment": "e.g. SD card or FRAM"
},
"67108864": {
"name": "parachute",
"description": "Parachute"
},
"134217728": {
"name": "magnetometer",
"description": "Magnetometer"
},
"268435456": {
"name": "accel",
"description": "Accelerometer"
},
"536870912": {
"name": "gyro",
"description": "Gyroscope"
},
"1073741824": {
"name": "remote_control",
"description": "Remote Control"
}
}
},
"sensor_type_t": {
"type": "uint8_t",
"description": "Sensor type for failover reporting",
@ -381,7 +582,27 @@
}
}
}
}
},
"navigation_mode_groups": {
"groups": {
"0": [65536],
"1": [131072],
"2": [196608],
"3": [67371008],
"4": [50593792],
"5": [84148224],
"10": [327680],
"14": [393216],
"15": [458752],
"17": [33816576],
"18": [100925440],
"19": [134479872],
"20": [151257088],
"21": [16973824],
"22": [168034304]
}
},
"supported_protocols": [ "health_and_arming_check" ]
}
}
}

2
src/modules/commander/CMakeLists.txt
View File

@ -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

38
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt Normal file
View File

@ -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
)

243
src/modules/commander/HealthAndArmingChecks/Common.cpp Normal file
View File

@ -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 &current_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;
}

378
src/modules/commander/HealthAndArmingChecks/Common.hpp Normal file
View File

@ -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(); }
};

75
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp Normal file
View File

@ -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();
}
}

69
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp Normal file
View File

@ -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] = {
};
};