From 07afcf4586c71b41d4d87add01df66619c3f993f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 27 Jan 2020 09:55:41 +0100 Subject: [PATCH] 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. --- Tools/px4events/jsonout.py | 2 + Tools/px4events/srcparser.py | 5 +- src/lib/events/enums.json | 223 ++++++++++- src/modules/commander/CMakeLists.txt | 2 + .../HealthAndArmingChecks/CMakeLists.txt | 38 ++ .../HealthAndArmingChecks/Common.cpp | 243 +++++++++++ .../HealthAndArmingChecks/Common.hpp | 378 ++++++++++++++++++ .../HealthAndArmingChecks.cpp | 75 ++++ .../HealthAndArmingChecks.hpp | 69 ++++ 9 files changed, 1033 insertions(+), 2 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/CMakeLists.txt create mode 100644 src/modules/commander/HealthAndArmingChecks/Common.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/Common.hpp create mode 100644 src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp diff --git a/Tools/px4events/jsonout.py b/Tools/px4events/jsonout.py index 7e5affb08a..f0644b0b86 100644 --- a/Tools/px4events/jsonout.py +++ b/Tools/px4events/jsonout.py @@ -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 = [] diff --git a/Tools/px4events/srcparser.py b/Tools/px4events/srcparser.py index 7a6c643cca..db8eda7b70 100644 --- a/Tools/px4events/srcparser.py +++ b/Tools/px4events/srcparser.py @@ -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)) diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 95efd34331..99ccb90bbc 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -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" ] } } } diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 538a79f93f..bdc6943852 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -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 diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt new file mode 100644 index 0000000000..98bdfe7dd4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -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 +) + diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp new file mode 100644 index 0000000000..ed14d6925b --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/Common.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(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(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; +} diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp new file mode 100644 index 0000000000..290898619b --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -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 +#include +#include +#include +#include + +#include +#include + +#if 0 // for debugging, prints event messages to console each time arming & health checks run (not necessarily change) +#include +#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(static_cast(a) | static_cast(b)); +} + +static inline NavModes operator&(NavModes a, NavModes b) +{ + return static_cast(static_cast(a) & static_cast(b)); +} + +static inline NavModes operator~(NavModes a) +{ + return static_cast(~static_cast(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 + 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 + 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 + 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 +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 +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 +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(); } +}; diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp new file mode 100644 index 0000000000..a4a8e25806 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp @@ -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(); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp new file mode 100644 index 0000000000..871822fadd --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -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 + +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] = { + }; +}; +