mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-13 16:30:36 +08:00
commander: implement external modes and mode executors
This commit is contained in:
Beat Küng
@@ -0,0 +1,471 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef CONSTRAINED_FLASH
|
||||
|
||||
#include "ModeManagement.hpp"
|
||||
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
bool ModeExecutors::hasFreeExecutors() const
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM; ++i) {
|
||||
if (!_mode_executors[i].valid) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int ModeExecutors::addExecutor(const ModeExecutors::ModeExecutor &executor)
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM; ++i) {
|
||||
if (!_mode_executors[i].valid) {
|
||||
_mode_executors[i] = executor;
|
||||
_mode_executors[i].valid = true;
|
||||
return i + FIRST_EXECUTOR_ID;
|
||||
}
|
||||
}
|
||||
|
||||
PX4_ERR("logic error");
|
||||
return -1;
|
||||
}
|
||||
|
||||
void ModeExecutors::removeExecutor(int id)
|
||||
{
|
||||
if (valid(id)) {
|
||||
_mode_executors[id - FIRST_EXECUTOR_ID].valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
void ModeExecutors::printStatus(int executor_in_charge) const
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM; ++i) {
|
||||
if (_mode_executors[i].valid) {
|
||||
int executor_id = i + FIRST_EXECUTOR_ID;
|
||||
PX4_INFO("Mode Executor %i: owned nav_state: %i, in charge: %s", executor_id, _mode_executors[i].owned_nav_state,
|
||||
executor_id == executor_in_charge ? "yes" : "no");
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Modes::hasFreeExternalModes() const
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM; ++i) {
|
||||
if (!_modes[i].valid) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t Modes::addExternalMode(const Modes::Mode &mode)
|
||||
{
|
||||
for (int i = 0; i < MAX_NUM; ++i) {
|
||||
if (!_modes[i].valid) {
|
||||
_modes[i] = mode;
|
||||
_modes[i].valid = true;
|
||||
return i + FIRST_EXTERNAL_NAV_STATE;
|
||||
}
|
||||
}
|
||||
|
||||
PX4_ERR("logic error");
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool Modes::removeExternalMode(uint8_t nav_state, const char *name)
|
||||
{
|
||||
if (valid(nav_state) && strncmp(name, _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].name, sizeof(Mode::name)) == 0) {
|
||||
_modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
PX4_ERR("trying to remove invalid mode %s", name);
|
||||
return false;
|
||||
}
|
||||
|
||||
void Modes::printStatus() const
|
||||
{
|
||||
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||
if (valid(i)) {
|
||||
const Modes::Mode &cur_mode = mode(i);
|
||||
PX4_INFO("External Mode %i: nav_state: %i, name: %s", i - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1, i,
|
||||
cur_mode.name);
|
||||
|
||||
if (cur_mode.replaces_nav_state != Mode::REPLACES_NAV_STATE_NONE
|
||||
&& cur_mode.replaces_nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
|
||||
PX4_INFO(" Replaces mode: %s", mode_util::nav_state_names[cur_mode.replaces_nav_state]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ModeManagement::ModeManagement(ExternalChecks &external_checks)
|
||||
: _external_checks(external_checks)
|
||||
{
|
||||
_external_checks.setExternalNavStates(Modes::FIRST_EXTERNAL_NAV_STATE, Modes::LAST_EXTERNAL_NAV_STATE);
|
||||
}
|
||||
|
||||
void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
|
||||
{
|
||||
register_ext_component_request_s request;
|
||||
int max_updates = 5;
|
||||
|
||||
while (!update_request.change_user_intended_nav_state && _register_ext_component_request_sub.update(&request)
|
||||
&& --max_updates >= 0) {
|
||||
request.name[sizeof(request.name) - 1] = '\0';
|
||||
PX4_DEBUG("got registration request: %s %llu, arming: %i mode: %i executor: %i", request.name, request.request_id,
|
||||
request.register_arming_check, request.register_mode, request.register_mode_executor);
|
||||
register_ext_component_reply_s reply{};
|
||||
reply.mode_executor_id = -1;
|
||||
reply.mode_id = -1;
|
||||
reply.arming_check_id = -1;
|
||||
static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
|
||||
memcpy(reply.name, request.name, sizeof(request.name));
|
||||
reply.request_id = request.request_id;
|
||||
reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
|
||||
|
||||
// validate
|
||||
bool request_valid = true;
|
||||
|
||||
if (request.register_mode_executor && !request.register_mode) {
|
||||
request_valid = false;
|
||||
}
|
||||
|
||||
if (request.register_mode && !request.register_arming_check) {
|
||||
request_valid = false;
|
||||
}
|
||||
|
||||
reply.success = false;
|
||||
|
||||
if (request_valid) {
|
||||
// check free space
|
||||
reply.success = true;
|
||||
|
||||
if (request.register_arming_check && !_external_checks.hasFreeRegistrations()) {
|
||||
PX4_WARN("No free slots for arming checks");
|
||||
reply.success = false;
|
||||
}
|
||||
|
||||
if (request.register_mode) {
|
||||
if (!_modes.hasFreeExternalModes()) {
|
||||
PX4_WARN("No free slots for modes");
|
||||
reply.success = false;
|
||||
|
||||
} else if (request.enable_replace_internal_mode) {
|
||||
// Check if another one already replaces the same mode
|
||||
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||
if (_modes.valid(i)) {
|
||||
const Modes::Mode &cur_mode = _modes.mode(i);
|
||||
|
||||
if (cur_mode.replaces_nav_state == request.replace_internal_mode) {
|
||||
// TODO: we could add priorities and allow the highest priority to do the replacement
|
||||
PX4_ERR("Trying to replace an already replaced mode (%i)", request.replace_internal_mode);
|
||||
reply.success = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (request.register_mode_executor && !_mode_executors.hasFreeExecutors()) {
|
||||
PX4_WARN("No free slots for executors");
|
||||
reply.success = false;
|
||||
}
|
||||
|
||||
// register component(s)
|
||||
if (reply.success) {
|
||||
int nav_mode_id = -1;
|
||||
|
||||
if (request.register_mode) {
|
||||
Modes::Mode mode{};
|
||||
strncpy(mode.name, request.name, sizeof(mode.name));
|
||||
|
||||
if (request.enable_replace_internal_mode) {
|
||||
mode.replaces_nav_state = request.replace_internal_mode;
|
||||
}
|
||||
|
||||
nav_mode_id = _modes.addExternalMode(mode);
|
||||
reply.mode_id = nav_mode_id;
|
||||
}
|
||||
|
||||
if (request.register_mode_executor) {
|
||||
ModeExecutors::ModeExecutor executor{};
|
||||
executor.owned_nav_state = nav_mode_id;
|
||||
int registration_id = _mode_executors.addExecutor(executor);
|
||||
|
||||
if (nav_mode_id != -1) {
|
||||
_modes.mode(nav_mode_id).mode_executor_registration_id = registration_id;
|
||||
}
|
||||
|
||||
reply.mode_executor_id = registration_id;
|
||||
}
|
||||
|
||||
if (request.register_arming_check) {
|
||||
int8_t replace_nav_state = request.enable_replace_internal_mode ? request.replace_internal_mode : -1;
|
||||
int registration_id = _external_checks.addRegistration(nav_mode_id, replace_nav_state);
|
||||
|
||||
if (nav_mode_id != -1) {
|
||||
_modes.mode(nav_mode_id).arming_check_registration_id = registration_id;
|
||||
}
|
||||
|
||||
reply.arming_check_id = registration_id;
|
||||
}
|
||||
|
||||
// Activate the mode?
|
||||
if (request.register_mode_executor && request.activate_mode_immediately && nav_mode_id != -1) {
|
||||
update_request.change_user_intended_nav_state = true;
|
||||
update_request.user_intended_nav_state = nav_mode_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
reply.timestamp = hrt_absolute_time();
|
||||
_register_ext_component_reply_pub.publish(reply);
|
||||
}
|
||||
}
|
||||
|
||||
void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request)
|
||||
{
|
||||
unregister_ext_component_s request;
|
||||
int max_updates = 5;
|
||||
|
||||
while (!update_request.change_user_intended_nav_state && _unregister_ext_component_sub.update(&request)
|
||||
&& --max_updates >= 0) {
|
||||
request.name[sizeof(request.name) - 1] = '\0';
|
||||
PX4_DEBUG("got unregistration request: %s arming: %i mode: %i executor: %i", request.name,
|
||||
(int)request.arming_check_id, (int)request.mode_id, (int)request.mode_executor_id);
|
||||
|
||||
if (request.arming_check_id != -1) {
|
||||
_external_checks.removeRegistration(request.arming_check_id, request.mode_id);
|
||||
}
|
||||
|
||||
if (request.mode_id != -1) {
|
||||
if (_modes.removeExternalMode(request.mode_id, request.name)) {
|
||||
removeModeExecutor(request.mode_executor_id);
|
||||
// else: if the mode was already removed (due to a timeout), the executor was also removed already
|
||||
}
|
||||
|
||||
// If the removed mode is currently active, switch to Hold
|
||||
if (user_intended_nav_state == request.mode_id) {
|
||||
update_request.change_user_intended_nav_state = true;
|
||||
update_request.user_intended_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active,
|
||||
UpdateRequest &update_request)
|
||||
{
|
||||
_failsafe_action_active = failsafe_action_active;
|
||||
_external_checks.update();
|
||||
|
||||
bool allow_update_while_armed = false;
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
|
||||
// For simulation, allow registering modes while armed for developer convenience
|
||||
allow_update_while_armed = true;
|
||||
#endif
|
||||
|
||||
if (armed && !allow_update_while_armed) {
|
||||
// Reject registration requests
|
||||
register_ext_component_request_s request;
|
||||
|
||||
if (_register_ext_component_request_sub.update(&request)) {
|
||||
PX4_ERR("Not accepting registration requests while armed");
|
||||
register_ext_component_reply_s reply{};
|
||||
reply.success = false;
|
||||
static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
|
||||
memcpy(reply.name, request.name, sizeof(request.name));
|
||||
reply.request_id = request.request_id;
|
||||
reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
|
||||
reply.timestamp = hrt_absolute_time();
|
||||
_register_ext_component_reply_pub.publish(reply);
|
||||
}
|
||||
|
||||
} else {
|
||||
// Check for unresponsive modes
|
||||
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||
if (_modes.valid(i)) {
|
||||
const Modes::Mode &mode = _modes.mode(i);
|
||||
|
||||
// Remove only if not currently selected
|
||||
if (user_intended_nav_state != i && _external_checks.isUnresponsive(mode.arming_check_registration_id)) {
|
||||
PX4_DEBUG("Removing unresponsive mode %i", i);
|
||||
_external_checks.removeRegistration(mode.arming_check_registration_id, i);
|
||||
removeModeExecutor(mode.mode_executor_registration_id);
|
||||
_modes.removeExternalMode(i, mode.name);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// As we're disarmed we can use the user intended mode, as no failsafe will be active
|
||||
checkNewRegistrations(update_request);
|
||||
checkUnregistrations(user_intended_nav_state, update_request);
|
||||
}
|
||||
|
||||
update_request.control_setpoint_update = checkConfigControlSetpointUpdates();
|
||||
}
|
||||
|
||||
void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state)
|
||||
{
|
||||
// Update mode executor in charge
|
||||
int mode_executor_for_intended_nav_state = -1;
|
||||
|
||||
if (_modes.valid(user_intended_nav_state)) {
|
||||
mode_executor_for_intended_nav_state = _modes.mode(user_intended_nav_state).mode_executor_registration_id;
|
||||
}
|
||||
|
||||
if (mode_executor_for_intended_nav_state == -1) {
|
||||
// Not an owned mode: check source
|
||||
if (source == ModeChangeSource::User) {
|
||||
// Give control to the pilot
|
||||
_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
|
||||
}
|
||||
|
||||
} else {
|
||||
// Switched into an owned mode: put executor in charge
|
||||
_mode_executor_in_charge = mode_executor_for_intended_nav_state;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t ModeManagement::getNavStateReplacementIfValid(uint8_t nav_state, bool report_error)
|
||||
{
|
||||
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||
if (_modes.valid(i)) {
|
||||
Modes::Mode &mode = _modes.mode(i);
|
||||
|
||||
if (mode.replaces_nav_state == nav_state) {
|
||||
if (_external_checks.isUnresponsive(mode.arming_check_registration_id)) {
|
||||
if (!mode.unresponsive_reported && report_error) {
|
||||
mode.unresponsive_reported = true;
|
||||
events::send(events::ID("commander_mode_fallback_internal"), events::Log::Critical,
|
||||
"External mode is unresponsive, falling back to internal");
|
||||
}
|
||||
|
||||
return nav_state;
|
||||
|
||||
} else {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return nav_state;
|
||||
}
|
||||
|
||||
uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state)
|
||||
{
|
||||
if (_modes.valid(nav_state)) {
|
||||
const Modes::Mode &mode = _modes.mode(nav_state);
|
||||
|
||||
if (mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
|
||||
return mode.replaces_nav_state;
|
||||
}
|
||||
}
|
||||
|
||||
return nav_state;
|
||||
}
|
||||
|
||||
void ModeManagement::removeModeExecutor(int mode_executor_id)
|
||||
{
|
||||
if (mode_executor_id == -1) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_mode_executor_in_charge == mode_executor_id) {
|
||||
_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
|
||||
}
|
||||
|
||||
_mode_executors.removeExecutor(mode_executor_id);
|
||||
}
|
||||
|
||||
int ModeManagement::modeExecutorInCharge() const
|
||||
{
|
||||
if (_failsafe_action_active) {
|
||||
return ModeExecutors::AUTOPILOT_EXECUTOR_ID;
|
||||
}
|
||||
|
||||
return _mode_executor_in_charge;
|
||||
}
|
||||
|
||||
bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
if (nav_state >= Modes::FIRST_EXTERNAL_NAV_STATE && nav_state <= Modes::LAST_EXTERNAL_NAV_STATE) {
|
||||
if (_modes.valid(nav_state)) {
|
||||
control_mode = _modes.mode(nav_state).config_control_setpoint;
|
||||
ret = true;
|
||||
|
||||
} else {
|
||||
Modes::Mode::setControlModeDefaults(control_mode);
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void ModeManagement::printStatus() const
|
||||
{
|
||||
_modes.printStatus();
|
||||
_mode_executors.printStatus(modeExecutorInCharge());
|
||||
}
|
||||
|
||||
bool ModeManagement::checkConfigControlSetpointUpdates()
|
||||
{
|
||||
bool had_update = false;
|
||||
vehicle_control_mode_s config_control_setpoint;
|
||||
int max_updates = 5;
|
||||
|
||||
while (_config_control_setpoints_sub.update(&config_control_setpoint) && --max_updates >= 0) {
|
||||
if (_modes.valid(config_control_setpoint.source_id)) {
|
||||
_modes.mode(config_control_setpoint.source_id).config_control_setpoint = config_control_setpoint;
|
||||
had_update = true;
|
||||
|
||||
} else {
|
||||
if (!_invalid_mode_printed) {
|
||||
PX4_ERR("Control sp config request for invalid mode: %i", config_control_setpoint.source_id);
|
||||
_invalid_mode_printed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return had_update;
|
||||
}
|
||||
|
||||
#endif /* CONSTRAINED_FLASH */
|
||||
Reference in New Issue
Block a user