From 487597b3859ee6bee9b0efda1069d15afb7bb29d Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 3 Nov 2012 01:13:17 -0700
Subject: Checkpoint; messages from FMU now make it to IO intact; fix HRT init
 timing, process more bytes from the serial port, add some simple packet
 counting.

---
 apps/drivers/boards/px4io/px4io_init.c |  5 -----
 apps/px4io/comms.c                     | 29 +++++++++++++++++------------
 apps/px4io/mixer.c                     |  2 +-
 apps/px4io/px4io.c                     |  8 ++++++--
 apps/px4io/px4io.h                     |  3 +++
 5 files changed, 27 insertions(+), 20 deletions(-)

(limited to 'apps')

diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c
index dc2e11c30..33a6707ce 100644
--- a/apps/drivers/boards/px4io/px4io_init.c
+++ b/apps/drivers/boards/px4io/px4io_init.c
@@ -80,11 +80,6 @@
 
 __EXPORT void stm32_boardinitialize(void)
 {
-	/* configure the high-resolution time/callout interface */
-#ifdef CONFIG_HRT_TIMER
-	hrt_init();
-#endif
-
 	/* configure GPIOs */
 	stm32_configgpio(GPIO_ACC1_PWR_EN);
 	stm32_configgpio(GPIO_ACC2_PWR_EN);
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index e7c968dcd..8fdf7d1c8 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -106,27 +106,29 @@ comms_check(void)
 	 * Check for bytes and feed them to the RX engine.  
 	 * Limit the number of bytes we actually process on any one iteration.
 	 */
-	struct pollfd fds[1];
-	fds[0].fd = fmu_fd;
-	fds[0].revents = POLLIN;
-	if (poll(fds, 1, 0) > 0) {
-		char buf[8];
-		ssize_t count = read(fmu_fd, buf, sizeof(buf));
-		for (int i = 0; i < count; i++)
-			hx_stream_rx(stream, buf[i]);
-	}
+	char buf[32];
+	ssize_t count = read(fmu_fd, buf, sizeof(buf));
+	for (int i = 0; i < count; i++)
+		hx_stream_rx(stream, buf[i]);
 }
 
+int frame_rx;
+int frame_bad;
+
 static void
 comms_handle_frame(void *arg, const void *buffer, size_t length)
 {
-	struct px4io_command *cmd;
+	struct px4io_command *cmd = (struct px4io_command *)buffer;
 
 	/* make sure it's what we are expecting */
-	if (length != sizeof(struct px4io_command))
+	if ((length != sizeof(struct px4io_command)) || 
+	    (cmd->f2i_magic != F2I_MAGIC)) {
+	    	frame_bad++;
 		return;
+	}
+	frame_rx++;
 
-	cmd = (struct px4io_command *)buffer;
+	irqstate_t flags = irqsave();
 
 	/* fetch new PWM output values */
 	for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++)
@@ -143,4 +145,7 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
 	/* XXX do relay changes here */	
 	for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
 		system_state.relays[i] = cmd->relay_state[i];
+
+out:
+	irqrestore(flags);
 }
diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c
index cbc03e2f8..94d10ef57 100644
--- a/apps/px4io/mixer.c
+++ b/apps/px4io/mixer.c
@@ -62,7 +62,7 @@ static unsigned mixer_input_drops;
  * Count of periodic calls in which we have no FMU input.
  */
 static unsigned fmu_input_drops;
-#define FMU_INPUT_DROP_LIMIT	10
+#define FMU_INPUT_DROP_LIMIT	20
 
 /*
  * HRT periodic call used to check for control input data.
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 33c28fc6c..90057c790 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -71,6 +71,9 @@ int user_start(int argc, char *argv[])
 	bool	heartbeat = false;
 	bool	failsafe = false;
 
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
 	/* configure the PWM outputs */
 	up_pwm_servo_init(0xff);
 
@@ -131,12 +134,13 @@ int user_start(int argc, char *argv[])
 		/* print some simple status */
 		if (timers[TIMER_STATUS_PRINT] == 0) {
 			timers[TIMER_STATUS_PRINT] = 1000;
-			lib_lowprintf("%c %s | %s | %s | C=%d    \r",
+			lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d   \r",
 				cursor[cycle++ & 3],
 				(system_state.armed         ? "ARMED"  : "SAFE"),
 				(system_state.rc_channels   ? "RC OK"  : "NO RC"),
 				(system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"),
-				system_state.rc_channels
+				system_state.rc_channels,
+				frame_rx, frame_bad
 			);
 		}
 	}
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 4889036b6..bbbe91865 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -95,6 +95,9 @@ struct sys_state_s
 
 extern struct sys_state_s system_state;
 
+extern int frame_rx;
+extern int frame_bad;
+
 /*
  * Software countdown timers.
  *
-- 
cgit v1.2.3