Rework the PX4IO software architecture:

 - Use a separate thread for handing R/C inputs and outputs.
 - Remove all PX4IO R/C receiver configuration; it's all automatic now.
 - Rework the main loop, dedicate it to PX4FMU communications after startup.
 - Fix several issues in the px4io driver that would cause a crash if PX4IO was not responding.

1 files changed, 36 insertions, 0 deletions

diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 24fc9951a..d5bd103c1 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -51,6 +51,8 @@
 #include "px4io.h"
 
 static struct hrt_call arming_call;
+static struct hrt_call heartbeat_call;
+static struct hrt_call failsafe_call;
 
 /*
  * Count the number of times in a row that we see the arming button
@@ -63,13 +65,22 @@ static unsigned counter;
 
 static bool safety_led_state;
 static bool safety_button_pressed;
+
 static void safety_check_button(void *arg);
+static void heartbeat_blink(void *arg);
+static void failsafe_blink(void *arg);
 
 void
 safety_init(void)
 {
 	/* arrange for the button handler to be called at 10Hz */
 	hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
+
+	/* arrange for the heartbeat handler to be called at 4Hz */
+	hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
+
+	/* arrange for the failsafe blinker to be called at 8Hz */
+	hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
 }
 
 static void
@@ -117,3 +128,28 @@ safety_check_button(void *arg)
 	}
 	LED_SAFETY(safety_led_state);
 }
+
+
+static void
+heartbeat_blink(void *arg)
+{
+	static bool heartbeat = false;
+
+	/* XXX add flags here that need to be frobbed by various loops */
+
+	LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+failsafe_blink(void *arg)
+{
+	static bool failsafe = false;
+
+	/* blink the failsafe LED if we don't have FMU input */
+	if (!system_state.mixer_use_fmu) {
+		failsafe = !failsafe;
+	} else {
+		failsafe = false;
+	}
+	LED_AMBER(failsafe);
+}
\ No newline at end of file