mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
336 lines
12 KiB
C++
336 lines
12 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "externalChecks.hpp"
|
|
|
|
static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint32_t &bits)
|
|
{
|
|
if (requirement_set) {
|
|
bits |= 1u << nav_state;
|
|
}
|
|
|
|
if (replaces_nav_state != -1) {
|
|
if (requirement_set) {
|
|
bits |= 1u << replaces_nav_state;
|
|
|
|
} else {
|
|
bits &= ~(1u << replaces_nav_state);
|
|
}
|
|
}
|
|
}
|
|
|
|
int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state)
|
|
{
|
|
int free_registration_index = -1;
|
|
|
|
for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
|
|
if (!registrationValid(i)) {
|
|
free_registration_index = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (free_registration_index != -1) {
|
|
_active_registrations_mask |= 1 << free_registration_index;
|
|
_registrations[free_registration_index].nav_mode_id = nav_mode_id;
|
|
_registrations[free_registration_index].replaces_nav_state = replaces_nav_state;
|
|
_registrations[free_registration_index].num_no_response = 0;
|
|
_registrations[free_registration_index].unresponsive = false;
|
|
_registrations[free_registration_index].total_num_unresponsive = 0;
|
|
|
|
if (!_registrations[free_registration_index].reply) {
|
|
_registrations[free_registration_index].reply = new arming_check_reply_s();
|
|
}
|
|
}
|
|
|
|
return free_registration_index;
|
|
}
|
|
|
|
bool ExternalChecks::removeRegistration(int registration_id, int8_t nav_mode_id)
|
|
{
|
|
if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
|
|
return false;
|
|
}
|
|
|
|
if (registrationValid(registration_id)) {
|
|
if (_registrations[registration_id].nav_mode_id == nav_mode_id) {
|
|
_active_registrations_mask &= ~(1u << registration_id);
|
|
return true;
|
|
}
|
|
}
|
|
|
|
PX4_ERR("trying to remove inactive external check");
|
|
return false;
|
|
}
|
|
|
|
bool ExternalChecks::isUnresponsive(int registration_id)
|
|
{
|
|
if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
|
|
return false;
|
|
}
|
|
|
|
if (registrationValid(registration_id)) {
|
|
return _registrations[registration_id].unresponsive;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
|
|
{
|
|
checkNonRegisteredModes(context, reporter);
|
|
|
|
if (_active_registrations_mask == 0) {
|
|
return;
|
|
}
|
|
|
|
NavModes unresponsive_modes{NavModes::None};
|
|
|
|
for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
|
|
if (!registrationValid(reg_idx) || !_registrations[reg_idx].reply) {
|
|
continue;
|
|
}
|
|
|
|
arming_check_reply_s &reply = *_registrations[reg_idx].reply;
|
|
|
|
int8_t nav_mode_id = _registrations[reply.registration_id].nav_mode_id;
|
|
|
|
if (_registrations[reply.registration_id].unresponsive) {
|
|
|
|
if (nav_mode_id != -1) {
|
|
unresponsive_modes = unresponsive_modes | reporter.getModeGroup(nav_mode_id);
|
|
setOrClearRequirementBits(true, nav_mode_id, -1, reporter.failsafeFlags().mode_req_other);
|
|
}
|
|
|
|
} else {
|
|
NavModes modes;
|
|
|
|
// We distinguish between two cases:
|
|
// - external navigation mode: in that case we set the single arming can_run bit for the mode
|
|
// - generic external arming check: set all arming bits
|
|
if (nav_mode_id == -1) {
|
|
modes = NavModes::All;
|
|
|
|
} else {
|
|
modes = reporter.getModeGroup(nav_mode_id);
|
|
|
|
int8_t replaces_nav_state = _registrations[reply.registration_id].replaces_nav_state;
|
|
|
|
if (replaces_nav_state != -1) {
|
|
modes = modes | reporter.getModeGroup(replaces_nav_state);
|
|
// Also clear the arming bits for the replaced mode, as the user intention is always set to the
|
|
// replaced mode.
|
|
// We only have to clear the bits, as for the internal/replaced mode, the bits are not cleared yet.
|
|
}
|
|
|
|
if (!reply.can_arm_and_run) {
|
|
setOrClearRequirementBits(true, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_other);
|
|
}
|
|
|
|
// Mode requirements
|
|
// A replacement mode will also replace the mode requirements of the internal/replaced mode
|
|
setOrClearRequirementBits(reply.mode_req_angular_velocity, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_angular_velocity);
|
|
setOrClearRequirementBits(reply.mode_req_attitude, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_attitude);
|
|
setOrClearRequirementBits(reply.mode_req_local_alt, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_local_alt);
|
|
setOrClearRequirementBits(reply.mode_req_local_position, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_local_position);
|
|
setOrClearRequirementBits(reply.mode_req_local_position_relaxed, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_local_position_relaxed);
|
|
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_global_position);
|
|
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_mission);
|
|
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_home_position);
|
|
setOrClearRequirementBits(reply.mode_req_prevent_arming, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_prevent_arming);
|
|
setOrClearRequirementBits(reply.mode_req_manual_control, nav_mode_id, replaces_nav_state,
|
|
reporter.failsafeFlags().mode_req_manual_control);
|
|
}
|
|
|
|
if (!reply.can_arm_and_run) {
|
|
reporter.clearArmingBits(modes);
|
|
}
|
|
|
|
if (reply.health_component_index > 0) {
|
|
reporter.setHealth((health_component_t)(1ull << reply.health_component_index),
|
|
reply.health_component_is_present, reply.health_component_warning,
|
|
reply.health_component_error);
|
|
}
|
|
|
|
for (int i = 0; i < reply.num_events; ++i) {
|
|
// set the modes, which is the first argument
|
|
memcpy(reply.events[i].arguments, &modes, sizeof(modes));
|
|
|
|
reporter.addExternalEvent(reply.events[i], modes);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (unresponsive_modes != NavModes::None) {
|
|
/* EVENT
|
|
* @description
|
|
* The application running the mode might have crashed or the CPU load is too high.
|
|
*/
|
|
reporter.armingCheckFailure(unresponsive_modes, health_component_t::system,
|
|
events::ID("check_external_modes_unresponsive"),
|
|
events::Log::Critical, "Mode is unresponsive");
|
|
}
|
|
|
|
}
|
|
|
|
void ExternalChecks::update()
|
|
{
|
|
if (_active_registrations_mask == 0) {
|
|
return;
|
|
}
|
|
|
|
const hrt_abstime now = hrt_absolute_time();
|
|
|
|
// Check for incoming replies
|
|
arming_check_reply_s reply;
|
|
int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH;
|
|
|
|
while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) {
|
|
if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id)
|
|
&& _current_request_id == reply.request_id) {
|
|
_reply_received_mask |= 1u << reply.registration_id;
|
|
_registrations[reply.registration_id].num_no_response = 0;
|
|
|
|
// Prevent toggling between unresponsive & responsive state
|
|
if (_registrations[reply.registration_id].total_num_unresponsive <= 3) {
|
|
_registrations[reply.registration_id].unresponsive = false;
|
|
}
|
|
|
|
if (_registrations[reply.registration_id].reply) {
|
|
*_registrations[reply.registration_id].reply = reply;
|
|
}
|
|
|
|
// PX4_DEBUG("Registration id=%i: %i events", reply.registration_id, reply.num_events);
|
|
}
|
|
}
|
|
|
|
if (_last_update > 0) {
|
|
if (_reply_received_mask == _active_registrations_mask) { // Got all responses
|
|
// Nothing to do
|
|
} else if (now > _last_update + REQUEST_TIMEOUT && !_had_timeout) { // Timeout
|
|
_had_timeout = true;
|
|
unsigned no_reply = _active_registrations_mask & ~_reply_received_mask;
|
|
|
|
for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
|
|
if ((1u << i) & no_reply) {
|
|
if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) {
|
|
// Clear immediately if not a mode
|
|
if (_registrations[i].nav_mode_id == -1) {
|
|
removeRegistration(i, -1);
|
|
PX4_WARN("No response from %i, removing", i);
|
|
|
|
} else {
|
|
_registrations[i].unresponsive = true;
|
|
|
|
if (_registrations[i].total_num_unresponsive < 100) {
|
|
++_registrations[i].total_num_unresponsive;
|
|
}
|
|
|
|
PX4_WARN("No response from %i, flagging unresponsive", i);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Start a new request?
|
|
if (now > _last_update + UPDATE_INTERVAL) {
|
|
_reply_received_mask = 0;
|
|
_last_update = now;
|
|
_had_timeout = false;
|
|
|
|
// Request the state from all registered components
|
|
arming_check_request_s request{};
|
|
request.request_id = ++_current_request_id;
|
|
request.timestamp = hrt_absolute_time();
|
|
_arming_check_request_pub.publish(request);
|
|
}
|
|
}
|
|
|
|
void ExternalChecks::setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state)
|
|
{
|
|
_first_external_nav_state = first_external_nav_state;
|
|
_last_external_nav_state = last_external_nav_state;
|
|
}
|
|
|
|
void ExternalChecks::checkNonRegisteredModes(const Context &context, Report &reporter) const
|
|
{
|
|
// Clear the arming bits for all non-registered external modes.
|
|
// But only report if one of them is selected, so we don't need to generate the extra event in most cases.
|
|
bool report_mode_not_available = false;
|
|
|
|
for (uint8_t external_nav_state = _first_external_nav_state; external_nav_state <= _last_external_nav_state;
|
|
++external_nav_state) {
|
|
bool found = false;
|
|
|
|
for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
|
|
if (registrationValid(reg_idx) && _registrations[reg_idx].nav_mode_id == external_nav_state) {
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!found) {
|
|
if (external_nav_state == context.status().nav_state_user_intention) {
|
|
report_mode_not_available = true;
|
|
}
|
|
|
|
reporter.clearArmingBits(reporter.getModeGroup(external_nav_state));
|
|
setOrClearRequirementBits(true, external_nav_state, -1, reporter.failsafeFlags().mode_req_other);
|
|
}
|
|
}
|
|
|
|
if (report_mode_not_available) {
|
|
/* EVENT
|
|
* @description
|
|
* The application running the mode is not started.
|
|
*/
|
|
reporter.armingCheckFailure(reporter.getModeGroup(context.status().nav_state_user_intention),
|
|
health_component_t::system,
|
|
events::ID("check_external_modes_unavailable"),
|
|
events::Log::Error, "Mode is not registered");
|
|
}
|
|
}
|
|
|