From 57c2085ce436028c933fb3c219953bcba3414401 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 26 Apr 2019 18:05:02 +0200
Subject: [PATCH] IO driver - Recover flight termination state from IO after
 FMU reboot in air

---
 src/drivers/px4io/px4io.cpp | 47 +++++++++++++++++++++++++++++++++----
 1 file changed, 43 insertions(+), 4 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 1cb209f2ab..2bca254803 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -726,17 +726,56 @@ PX4IO::init()
 			errx(1, "PRM CMPID");
 		}
 
-		/* send command to arm system via command API */
+		/* prepare vehicle command */
 		vehicle_command_s vcmd = {};
-		vcmd.timestamp = hrt_absolute_time();
-		vcmd.param1 = 1.0f; /* request arming */
-		vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
 		vcmd.target_system = (uint8_t)sys_id;
 		vcmd.target_component = (uint8_t)comp_id;
 		vcmd.source_system = (uint8_t)sys_id;
 		vcmd.source_component = (uint8_t)comp_id;
 		vcmd.confirmation = true; /* ask to confirm command */
 
+		if (reg & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+			mavlink_log_emergency(&_mavlink_log_pub, "IO is in failsafe, force failsafe");
+			/* send command to terminate flight via command API */
+			vcmd.timestamp = hrt_absolute_time();
+			vcmd.param1 = 1.0f; /* request flight termination */
+			vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
+
+			/* send command once */
+			orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+
+			/* spin here until IO's state has propagated into the system */
+			do {
+				orb_check(safety_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
+				}
+
+				/* wait 50 ms */
+				px4_usleep(50000);
+
+				/* abort after 5s */
+				if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) {
+					mavlink_log_emergency(&_mavlink_log_pub, "Failed to recover from in-air restart (3), abort");
+					return 1;
+				}
+
+				/* re-send if necessary */
+				if (!safety.force_failsafe) {
+					orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
+					PX4_WARN("re-sending flight termination cmd");
+				}
+
+				/* keep waiting for state change for 2 s */
+			} while (!safety.force_failsafe);
+		}
+
+		/* send command to arm system via command API */
+		vcmd.timestamp = hrt_absolute_time();
+		vcmd.param1 = 1.0f; /* request arming */
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
+
 		/* send command once */
 		orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);