commander: send parachute command on termination (#17564)

* commander: send parachute command on termination

This sends the DO_PARACHUTE command to parachute component.

* commander: fix lying comments and printf

* commander: use one flag for termination triggered

This merges the duplicate flags _flight_termination_triggered and
_flight_flight_termination_printed.

* commander: correct variable name

* commander: always send tune with parachute

* commander: fix target_component for parachute cmd

The previous changes were wrong in that all commands were now sent to
the parachute component which doesn't make any sense. Of course only the
parachute command should be sent there.

msg/vehicle_command.msg

@@ -120,6 +120,10 @@ uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1      # Continuous zoom up/down unt
 uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2           # Zoom value as proportion of full camera range
 uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3    # Zoom to a focal length
 
+uint8 PARACHUTE_ACTION_DISABLE = 0
+uint8 PARACHUTE_ACTION_ENABLE = 1
+uint8 PARACHUTE_ACTION_RELEASE = 2
+
 uint8 FAILURE_UNIT_SENSOR_GYRO = 0
 uint8 FAILURE_UNIT_SENSOR_ACCEL = 1
 uint8 FAILURE_UNIT_SENSOR_MAG = 2

src/modules/commander/Commander.cpp

@@ -915,6 +915,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
 			if (cmd.param1 > 1.5f) {
+				// Test termination command triggers lockdown but not actual termination.
 				if (!_lockdown_triggered) {
 					_armed.lockdown = true;
 					_lockdown_triggered = true;
@@ -922,16 +923,12 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				}
 
 			} else if (cmd.param1 > 0.5f) {
-				//XXX update state machine?
+				// Trigger real termination.
 				if (!_flight_termination_triggered) {
 					_armed.force_failsafe = true;
 					_flight_termination_triggered = true;
 					PX4_WARN("forcing failsafe (termination)");
-				}
-
-				if ((int)cmd.param2 <= 0) {
-					/* reset all commanded failure modes */
-					PX4_WARN("reset all non-flighttermination failsafe commands");
+					send_parachute_command();
 				}
 
 			} else {
@@ -2170,9 +2167,15 @@ Commander::run()
 
 				case (geofence_result_s::GF_ACTION_TERMINATE) : {
 						PX4_WARN("Flight termination because of geofence");
-						mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
-						_armed.force_failsafe = true;
-						_status_changed = true;
+
+						if (!_flight_termination_triggered && !_lockdown_triggered) {
+							_flight_termination_triggered = true;
+							mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
+							_armed.force_failsafe = true;
+							_status_changed = true;
+							send_parachute_command();
+						}
+
 						break;
 					}
 				}
@@ -2220,12 +2223,13 @@ Commander::run()
 		if (_armed.armed && _mission_result_sub.get().flight_termination &&
 		    !_status_flags.circuit_breaker_flight_termination_disabled) {
 
-			_armed.force_failsafe = true;
-			_status_changed = true;
 
-			if (!_flight_termination_printed) {
+			if (!_flight_termination_triggered && !_lockdown_triggered) {
 				mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated");
-				_flight_termination_printed = true;
+				_flight_termination_triggered = true;
+				_armed.force_failsafe = true;
+				_status_changed = true;
+				send_parachute_command();
 			}
 
 			if (_counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
@@ -2474,9 +2478,9 @@ Commander::run()
 
 				if (_status.failure_detector_status & (vehicle_status_s::FAILURE_ROLL | vehicle_status_s::FAILURE_PITCH |
 								       vehicle_status_s::FAILURE_ALT | vehicle_status_s::FAILURE_EXT)) {
-					const bool is_second_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
+					const bool is_right_after_takeoff = hrt_elapsed_time(&_status.takeoff_time) < (1_s * _param_com_lkdown_tko.get());
 
-					if (is_second_after_takeoff && !_lockdown_triggered) {
+					if (is_right_after_takeoff && !_lockdown_triggered) {
 						// This handles the case where something fails during the early takeoff phase
 						_armed.lockdown = true;
 						_lockdown_triggered = true;
@@ -2488,7 +2492,7 @@ Commander::run()
 						_armed.force_failsafe = true;
 						_flight_termination_triggered = true;
 						mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight");
-						set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
+						send_parachute_command();
 					}
 				}
 			}
@@ -4056,6 +4060,25 @@ void Commander::esc_status_check()
 	_last_esc_status_updated = esc_status.timestamp;
 }
 
+void Commander::send_parachute_command()
+{
+	vehicle_command_s vcmd{};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
+	vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
+
+	uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
+	vcmd.source_system = vehicle_status_sub.get().system_id;
+	vcmd.target_system = vehicle_status_sub.get().system_id;
+	vcmd.source_component = vehicle_status_sub.get().component_id;
+	vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
+
+	uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
+	vcmd.timestamp = hrt_absolute_time();
+	vcmd_pub.publish(vcmd);
+
+	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {

src/modules/commander/Commander.hpp

@@ -187,6 +187,8 @@ private:
 
 	bool stabilization_required();
 
+	void send_parachute_command();
+
 	DEFINE_PARAMETERS(
 
 		(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
@@ -373,7 +375,6 @@ private:
 	bool		_failsafe_old{false};	///< check which state machines for changes, clear "changed" flag
 	bool		_have_taken_off_since_arming{false};
 	bool		_should_set_home_on_takeoff{true};
-	bool		_flight_termination_printed{false};
 	bool		_system_power_usb_connected{false};
 
 	cpuload_s		_cpuload{};