commander: implement external modes and mode executors

2026-11-10 05:21:35 +08:00 · 2023-11-08 10:21:27 +01:00
parent 1ad5a9de08
commit 9cb8245851
32 changed files with 1629 additions and 51 deletions
@@ -0,0 +1,335 @@
+/****************************************************************************
+ *
+ *   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");
+	}
+}
+