feat(control_allocator): cancel check when nav_state changes

style: reordering & comment for readability

src/modules/control_allocator/ActuatorGroupPreflightCheck.cpp

@@ -134,6 +134,9 @@ void ActuatorGroupPreflightCheck::handleCommand(hrt_abstime now, bool is_tiltrot
 		sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
 	}
 
+	vehicle_status_s vehicle_status{};
+	_vehicle_status_sub.copy(&vehicle_status);
+
 	// Torque and thrust have inputs in [-1, 1]. Tilt expects [0, 1] (0=vertical, 1=horizontal);
 	const float min_input = group == vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT ? 0.f : -1.f;
 
@@ -141,9 +144,11 @@ void ActuatorGroupPreflightCheck::handleCommand(hrt_abstime now, bool is_tiltrot
 	_requires_armed = isThrust(group);
 	_input = math::constrain(vehicle_command.param2, min_input, 1.f);
 	_started = now;
+	_started_nav_state = vehicle_status.nav_state;
+	_last_command = vehicle_command;
+
 	_running = true;
 
-	_last_command = vehicle_command;
 	sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS, now);
 }
 
@@ -153,15 +158,20 @@ void ActuatorGroupPreflightCheck::updateState(hrt_abstime now)
 
 	actuator_armed_s actuator_armed{};
 	vehicle_land_detected_s vehicle_land_detected{};
+	vehicle_status_s vehicle_status{};
 
-	if (_armed_sub.copy(&actuator_armed) && _vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+	if (_armed_sub.copy(&actuator_armed)
+	    && _vehicle_land_detected_sub.copy(&vehicle_land_detected)
+	    && _vehicle_status_sub.copy(&vehicle_status)) {
 
 		// Cancel if any of the conditions we depended on at start are lost
-		// (thrust -> armed, torque/tilt -> pre-armed, always !landed).
+		// (thrust -> armed, torque/tilt -> pre-armed, always !landed),
+		// or when the nav_state changes (safety measure).
 		const bool armed_lost = actuator_armed.armed != _requires_armed;
 		const bool prearmed_lost = !_requires_armed && !actuator_armed.prearmed;
+		const bool nav_state_changed = vehicle_status.nav_state != _started_nav_state;
 
-		if (armed_lost || prearmed_lost || !vehicle_land_detected.landed) {
+		if (armed_lost || prearmed_lost || !vehicle_land_detected.landed || nav_state_changed) {
 			_running = false;
 			sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
 			return;

src/modules/control_allocator/ActuatorGroupPreflightCheck.hpp

@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_status.h>
 
 /**
  * Drives the VEHICLE_CMD_ACTUATOR_GROUP_TEST state machine: holds a fixed
@@ -100,12 +101,17 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	bool _running{false};
-	bool _requires_armed{false};
+
+	// These describe the currently running check, only set when started
 	uint8_t _group{0};
+	bool _requires_armed{false};
 	float _input{0.0f};
-	vehicle_command_s _last_command{};
 	hrt_abstime _started{0};
+	uint8_t _started_nav_state{};
+	vehicle_command_s _last_command{};
 };