Fixed correct RC loss detection, AR.Drone is now shutting down motors after 1 s of RC loss. Added debug topic.

3 files changed, 25 insertions, 4 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 80d2c58f8..4135abf60 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -69,6 +69,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
 #include <mavlink/mavlink_log.h>
  
 #include <systemlib/param/param.h>
@@ -1174,6 +1175,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* End battery voltage check */
 
 		/* Start RC state check */
+		bool prev_lost = current_status.rc_signal_lost;
 
 		if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) {
 
@@ -1238,10 +1240,18 @@ int commander_thread_main(int argc, char *argv[])
 			/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
 			current_status.rc_signal_cutting_off = true;
 			current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
+
 			/* if the RC signal is gone for a full second, consider it lost */
 			if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true;
 		}
 
+		/* Check if this is the first loss or first gain*/
+		if ((!prev_lost && current_status.rc_signal_lost) ||
+			prev_lost && !current_status.rc_signal_lost) {
+			/* publish rc lost */
+			publish_armed_status(&current_status);
+		}
+
 		/* End mode switch */
 
 		/* END RC state check */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 4e2166a3a..51ed95114 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -199,10 +199,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 	if (invalid_state == false || old_state != new_state) {
 		current_status->state_machine = new_state;
 		state_machine_publish(status_pub, current_status, mavlink_fd);
-		struct actuator_armed_s armed;
-		armed.armed = current_status->flag_system_armed;
-		orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-		orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		publish_armed_status(current_status);
 		ret = OK;
 	}
 	if (invalid_state) {
@@ -220,6 +217,14 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
 
+void publish_armed_status(const struct vehicle_status_s *current_status) {
+	struct actuator_armed_s armed;
+	armed.armed = current_status->flag_system_armed;
+	armed.failsafe = current_status->rc_signal_lost;
+	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+}
+
 
 /*
  * Private functions, update the state machine
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 4908c799a..c4d1b78a5 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
  */
 void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
+/**
+ * Publish the armed state depending on the current system state
+ *
+ * @param current_status the current system status
+ */
+void publish_armed_status(const struct vehicle_status_s *current_status);