Add gps passthrough and spistream files

sw/airborne/fms/fms_spistream.h

@@ -0,0 +1,39 @@
+#ifndef FMS_SPISTREAM_H__
+#define FMS_SPISTREAM_H__
+
+#define CLIENT_SOCKET_PATH "./spistreamc.socket"
+#define DAEMON_SOCKET_PATH "./spistreamd.socket"
+
+#define min(a,b) ((a>b)? (b) : (a))
+
+void print_message(char prefix[], uint8_t msg_id, uint8_t * data, uint16_t num_bytes); 
+void print_message(char prefix[], uint8_t msg_id, uint8_t * data, uint16_t num_bytes) {
+/*
+	struct tm * timeinfo; 
+	time_t c_time;
+	char time_str[30];
+*/
+  uint8_t cnt; 
+	uint8_t log_bytes = num_bytes; 
+	if(log_bytes > 16) { log_bytes = 16; }
+/*
+	time(&c_time); 
+	timeinfo = localtime(&c_time);
+	strftime(time_str, 30, " %X ", timeinfo); 
+	
+  printf("%s %s bytes: %3d | id: %3d | UART%d | ", 
+				 prefix, time_str, num_bytes, msg_id, data[0]);
+*/
+  printf("%s bytes: %3d | id: %3d | UART%d | ", 
+				 prefix, num_bytes, msg_id, data[0]);
+  for(cnt = 1; cnt < log_bytes; cnt++) { 
+    printf("%02X ", data[cnt]);
+		if(cnt >= 24 && cnt % 24 == 0 && cnt+1 < log_bytes) { 
+			printf("\n                                                    ");
+		}
+  }
+  printf("\n");
+}
+
+#endif
+

sw/airborne/fms/fms_spistream_client.c

@@ -0,0 +1,225 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+// Socket stuff
+#include <sys/types.h>
+#include <sys/un.h>
+#include <signal.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+// End socket stuff
+
+#include <event.h>
+
+#include "std.h"
+#include "fms_debug.h"
+#include "fms_periodic.h"
+
+/* stuff for io processor link */
+#include "fms_spi_link.h"
+#include "fms_autopilot_msg.h"
+
+#define OVERO_ENV
+#include "lisa/lisa_spistream.h"
+#include "fms_spistream.h"
+
+
+static void parse_command_line(int argc, char** argv);
+static void main_init(void);
+static void main_exit(void);
+static void main_periodic(int my_sig_num);
+
+static int open_stream(void);
+
+static void on_kill(int signum);
+
+static int dfifo[4]; 
+static int cfifo[4]; 
+static char dfifo_files[4][40];
+static char cfifo_files[4][40];
+
+
+
+int main(int argc, char *argv[]) {
+
+  parse_command_line(argc, argv);
+  
+  main_init();
+  TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n");
+
+  /* Enter our mainloop */
+	event_dispatch();
+	while(1) {
+		sleep(100);
+	}
+  
+	main_exit(); 
+
+  TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n");
+
+  return 0;
+}
+
+static void main_periodic(int my_sig_num) {
+	uint8_t fifo_idx; 
+	uint8_t msg_id; 
+	uint16_t num_bytes; 
+	int16_t ret; 
+	static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH*10];
+
+	for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { 
+		// The periodic is triggered before fifo 
+		// connections have been initialized, so 
+		// check for a valid fd first: 
+		if(dfifo[fifo_idx] > 0) { 
+			ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2);
+			ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1);
+
+			memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH); 
+			if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { 
+				fprintf(stderr, "Warning: Message has length %d, but limit "
+												"is %d\n", 
+								num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
+				num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
+			}
+			ret = read(dfifo[fifo_idx], &buf, num_bytes);
+			if(ret > 0 && ret == num_bytes) { 
+				// Message received
+				print_message(">> Client", msg_id, buf, num_bytes);
+			}
+			else if(ret > 0 && ret < num_bytes) { 
+				fprintf(stderr, "Tried to read %d bytes, but only got %d\n", 
+								num_bytes, ret); 
+			}
+		}
+		else { 
+			// FIFO file descriptor is invalid, 
+			// retry to open it: 
+			dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
+		}
+	}
+
+}
+
+static void main_init(void) {
+
+  TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
+
+  /* Initalize the event library */
+  event_init();
+  
+  /* Initalize our ô so accurate periodic timer */
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return; 
+  }
+
+	signal(SIGKILL, on_kill);
+	signal(SIGINT,  on_kill);
+	signal(SIGILL,  on_kill);
+	signal(SIGHUP,  on_kill);
+	signal(SIGQUIT, on_kill);
+	signal(SIGTERM, on_kill);
+	signal(SIGSEGV, on_kill);
+
+	if(!open_stream()) { 
+		fprintf(stderr, "Could not open stream, sorry\n");
+		exit(1);
+	}
+
+  TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
+}
+
+/**
+ * For every FIFO, a non-blocking connection try is called 
+ * via open(..., O_NONBLOCK). 
+ * This immediately returns a file descriptor or 0 if 
+ * the other end of the fifo is closed. 
+ * In the transmission, we check the FIFO file descriptors 
+ * and retry to open them, in case they are invalid. 
+ * You can also just open() them without O_NONBLOCK in 
+ * the client app, but the daemon should be running before 
+ * starting the client then, otherwise open() would block. 
+ *
+ * When using this strategy, we get connection 
+ * recovery for free when either daemon or client die. 
+ *
+ * After the connections are established, you can use them 
+ * for read() and write(), as well as register an event 
+ * trigger on them, like select() or libevent. 
+ * 
+ */
+static int open_stream(void) { 
+	uint8_t fifo_idx; 
+
+	strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data
+	strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client)
+	strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo"); 
+	strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
+	strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands
+	strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM)
+	strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo"); 
+	strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
+	
+	for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { 
+		fprintf(stderr, "Open data stream %s ... \n", dfifo_files[fifo_idx]);
+		dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
+		fprintf(stderr, " ...\n");
+	}
+
+	return 1; 
+
+	for(fifo_idx = 0; fifo_idx < 3; fifo_idx++) { 
+		fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]);
+		cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY);
+		if(cfifo[fifo_idx] < 0) { 
+			fprintf(stderr, " failed\n");
+			return 0; 
+		}
+	}
+	return 1; 
+}
+
+static void main_exit(void) 
+{
+	fprintf(stderr, "Bye!\n");
+}
+
+static void parse_command_line(int argc, char** argv) {
+}
+
+static void on_kill(int signum)
+{
+	fprintf(stderr, "Exiting, got signal %d\n", signum);
+	main_exit(); 
+	exit(1);
+}

sw/airborne/fms/fms_spistream_daemon.c

@@ -0,0 +1,347 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+// Socket stuff
+#include <sys/types.h>
+#include <sys/un.h>
+#include <signal.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+// End socket stuff
+
+#include <event.h>
+
+#include "std.h"
+#include "fms_debug.h"
+#include "fms_periodic.h"
+
+/* stuff for io processor link */
+#include "fms_spi_link.h"
+#include "fms_autopilot_msg.h"
+
+/* stuff for telemetry/datalink */
+
+#define OVERO_ENV
+#include "lisa/lisa_spistream.h"
+#include "fms_spistream.h"
+
+#define LOG_OUT stdout
+
+
+static void parse_command_line(int argc, char** argv);
+static void main_init(void);
+static void main_exit(void);
+static void main_periodic(int my_sig_num);
+static void spistream_event(void);
+
+static int open_stream(void);
+static void close_stream(void);
+
+static void on_timeout(int signum);
+static void on_kill(int signum);
+static void on_dead_pipe(int signum);
+
+static void on_spistream_msg_received(uint8_t msg_id, uint8_t * data, uint16_t num_bytes);
+static void on_spistream_msg_sent(uint8_t msg_id);
+
+static void send_to_client(uint8_t * data, uint16_t num_bytes, uint8_t fifo_idx);
+
+static uint8_t spistream_msg[123];
+
+static int dfifo[4]; 
+static int cfifo[4]; 
+static char dfifo_files[4][40];
+static char cfifo_files[4][40];
+
+
+
+int main(int argc, char *argv[]) {
+
+  parse_command_line(argc, argv);
+  
+  main_init();
+  TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n");
+
+  /* Enter our mainloop */
+	event_dispatch();
+	while(1) {
+		sleep(100);
+	}
+  
+	main_exit(); 
+
+  TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n");
+
+  return 0;
+}
+
+static void main_periodic(int my_sig_num) {
+
+	static int32_t every_100 = 1000;
+	if(every_100-- == 0) { 
+		every_100 = 1000;
+		spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
+	}
+
+	spistream_event(); 
+}
+
+static void spistream_event() {
+
+  static struct AutopilotMessagePTStream msg_in;
+  static struct AutopilotMessagePTStream msg_out;
+  static uint8_t crc_valid; 
+
+  spistream_read_pkg(&msg_in); 
+/*
+  uint8_t cnt; 
+  static uint8_t pkg_size = sizeof(msg_in.pkg_data);
+	if(msg_out.message_cnt != 0) { 
+		printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ", 
+				pkg_size, msg_out.message_cnt, msg_out.package_cntd);
+		for(cnt = 0; cnt < pkg_size; cnt++) { 
+			printf("%3d ", msg_out.pkg_data[cnt]);
+		}
+		printf("\n");
+	}
+*/
+  spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
+/*
+	if(msg_in.message_cnt != 0) { 
+		printf("PKG in (spi trx: %d): Size: %3d MID: %3d PCNTD: %3d | ", SPISTREAM_PACKAGE_SIZE, 
+           pkg_size, msg_in.message_cnt, msg_in.package_cntd);
+		for(cnt = 0; cnt < pkg_size; cnt++) { 
+			printf("%02X ", msg_in.pkg_data[cnt]);
+		}
+		printf("\n");
+	}
+*/
+  spistream_write_pkg(&msg_out); 
+}
+
+static void on_spistream_msg_received(uint8_t msg_id, 
+                                      uint8_t * data, 
+                                      uint16_t num_bytes) {
+	uint8_t uart; 
+	uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH+3];
+
+	print_message("<< Daemon", msg_id, data, num_bytes); 
+
+	uart = data[0]; 
+	// Check for valid uart ID
+	if(uart >= 0 && uart <= 3) { 
+		if(msg_id > 0) {
+			buf[0] = (uint8_t)(num_bytes & 0x00ff);
+			buf[1] = (uint8_t)((num_bytes << 8) & 0x00ff);
+			buf[2] = msg_id; 
+			if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { 
+				fprintf(LOG_OUT, "Warning: Message has length %d, but limit "
+                         "is %d - truncating message\n", 
+								num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
+				num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
+			}
+			memcpy(buf+3, data, num_bytes); 
+			send_to_client(buf, num_bytes+3, uart); 
+		}
+	}
+}
+
+static void send_to_client(uint8_t * data, uint16_t num_bytes, uint8_t fifo_idx)
+{
+	if(dfifo[fifo_idx] <= 0) { 
+		// No client connected to this fifo, yet
+		dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
+		if(dfifo[fifo_idx] <= 0) { 
+			fprintf(LOG_OUT, "No client for data fifo %d (%s)\n", 
+							fifo_idx, dfifo_files[fifo_idx]); 
+			return;
+		}
+	}
+	else { 
+		// Client connected to this fifo
+		if(write(dfifo[fifo_idx], data, num_bytes) == -1) { 
+			fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx);
+		}
+	}	
+}
+
+static void on_spistream_msg_sent(uint8_t msg_id) { 
+//  TRACE(TRACE_DEBUG, "%s", "SPI message sent \n");
+}
+
+static void main_init(void) {
+  uint8_t byte_idx; 
+
+  TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
+
+  /* Initalize our SPI link to IO processor */
+  if (spi_link_init()) {
+    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
+    return;
+  }
+
+  spistream_init(&on_spistream_msg_received, 
+                 &on_spistream_msg_sent); 
+
+  for(byte_idx=1; byte_idx < 123; byte_idx++) { 
+    spistream_msg[byte_idx] = byte_idx; 
+  }
+  /* Initalize the event library */
+  event_init();
+  
+  /* Initalize our ô so accurate periodic timer */
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return; 
+  }
+
+	signal(SIGKILL, on_kill);
+	signal(SIGINT,  on_kill);
+	signal(SIGILL,  on_kill);
+	signal(SIGHUP,  on_kill);
+	signal(SIGQUIT, on_kill);
+	signal(SIGTERM, on_kill);
+	signal(SIGSEGV, on_kill);
+	signal(SIGPIPE, on_dead_pipe);
+
+	if(!open_stream()) { 
+		fprintf(LOG_OUT, "Could not open stream, sorry\n");
+		exit(1);
+	}
+
+  TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
+}
+
+static void main_exit(void) 
+{
+	fprintf(LOG_OUT, "Closing socket\n");
+	close_stream(); 
+}
+
+static void parse_command_line(int argc, char** argv) {
+/*
+	while ((ch = getopt(argc, argv, "d:")) != -1) {
+		switch (ch) {
+		case 'd':
+			daemon_mode = 1;
+		  break;
+		}
+	}
+*/
+}
+
+static int open_stream(void) { 
+	uint8_t fifo_idx; 
+	int ret; 
+
+	strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data
+	strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client)
+	strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo"); 
+	strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
+	strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands
+	strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM)
+	strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo"); 
+	strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
+	
+	for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { 
+		fprintf(LOG_OUT, "Creating data stream %s ...", dfifo_files[fifo_idx]);
+		if((ret = mkfifo(dfifo_files[fifo_idx], 0777)) < 0) {
+			fprintf(LOG_OUT, " failed\n");
+			fprintf(LOG_OUT, "Could not create data fifo %d: %s\n", 
+						 fifo_idx, dfifo_files[fifo_idx]);
+			close_stream(); 
+			return 0; 
+		}
+		else { 
+			fprintf(LOG_OUT, " ok\n");
+			dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
+		}
+	}
+
+	for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { 
+		fprintf(LOG_OUT, "Creating command stream %s ... ", cfifo_files[fifo_idx]);
+		if((ret = mkfifo(cfifo_files[fifo_idx], 0777)) < 0) {
+			fprintf(LOG_OUT, " failed\n");
+			fprintf(LOG_OUT, "Could not create command fifo %d: %s\n", 
+						 fifo_idx, cfifo_files[fifo_idx]);
+			close_stream(); 
+			return 0; 
+		}
+		else { 
+			fprintf(LOG_OUT, " ok\n");
+			cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
+		}
+	}
+	return 1; 
+}
+
+static void close_stream(void) { 
+	uint8_t fifo_idx; 
+	fprintf(LOG_OUT, "Closing streams\n");
+	for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) 
+	{ 
+		if(dfifo[fifo_idx] >= 0) { 
+			close(dfifo[fifo_idx]); 
+		}
+		unlink(dfifo_files[fifo_idx]); 
+		if(cfifo[fifo_idx] >= 0) { 
+			close(cfifo[fifo_idx]); 
+		}
+		unlink(cfifo_files[fifo_idx]); 
+	}
+}
+
+static void on_timeout(int signum) 
+{
+	fprintf(LOG_OUT, "Timeout, stopping spistream daemon\n");
+	exit(6);
+}
+
+static void on_kill(int signum)
+{
+	fprintf(LOG_OUT, "Exiting, got signal %d\n", signum);
+	main_exit(); 
+	exit(1);
+}
+
+static void on_dead_pipe(int signum)
+{
+	uint8_t fifo_idx; 
+	fprintf(LOG_OUT, "Got SIGPIPE (signal %d)\n", signum);
+	// *Pop* goes the pipe. Looks like our client got AWOL. 
+	// Let's be nice and invalidate the file descriptors: 
+	for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
+		close(dfifo[fifo_idx]); 
+		dfifo[fifo_idx] = -1; 
+	}
+}
+

sw/airborne/fms/overo_test_gps_passthrough.c

@@ -0,0 +1,192 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+#include <event.h>
+
+#include "std.h"
+#include "fms_debug.h"
+#include "fms_periodic.h"
+
+/* stuff for io processor link */
+#include "fms_spi_link.h"
+#include "fms_autopilot_msg.h"
+
+/* stuff for telemetry/datalink */
+
+#define OVERO_ENV
+#include "lisa/lisa_spistream.h"
+
+static void parse_command_line(int argc, char** argv);
+static void main_init(void);
+static void main_periodic(int my_sig_num);
+static void spistream_event(void);
+
+static void on_spistream_msg_received(uint8_t msg_id, uint8_t * data, uint16_t num_bytes);
+static void on_spistream_msg_sent(void);
+
+static uint8_t spistream_msg[123];
+
+int main(int argc, char *argv[]) {
+
+  parse_command_line(argc, argv);
+  
+  main_init();
+  TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n");
+
+  /* Enter our mainloop */
+	event_dispatch();
+	while(1) {
+		sleep(100);
+	}
+  
+  TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n");
+
+  return 0;
+}
+
+static void main_periodic(int my_sig_num) {
+#if 0
+	static int32_t every_100 = 1000;
+	if(every_100-- == 0) { 
+		every_100 = 1000;
+		spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
+/*
+		spistream_send_msg(spistream_msg, 15);
+		spistream_send_msg(spistream_msg, 25);
+*/
+	}
+#endif
+	spistream_event(); 
+}
+
+static void spistream_event() {
+
+  struct AutopilotMessagePTStream msg_in;
+  struct AutopilotMessagePTStream msg_out;
+  static uint8_t pkg_size = sizeof(msg_in.pkg_data);
+  uint8_t crc_valid; 
+  uint8_t cnt; 
+
+  spistream_write_pkg(&msg_out); 
+
+	if(msg_out.message_cnt != 0) { 
+		printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ", 
+				pkg_size, msg_out.message_cnt, msg_out.package_cntd);
+		for(cnt = 0; cnt < pkg_size; cnt++) { 
+			printf("%3d ", msg_out.pkg_data[cnt]);
+		}
+		printf("\n");
+	}
+
+  spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
+/*
+	if(msg_in.message_cnt != 0) { 
+		printf("Package in:  Size: %3d MID: %3d PCNTD: %3d | ", 
+				pkg_size, msg_in.message_cnt, msg_in.package_cntd);
+		for(cnt = 0; cnt < pkg_size; cnt++) { 
+			printf("%3d ", msg_in.pkg_data[cnt]);
+		}
+		printf("\n");
+	}
+*/
+  spistream_read_pkg(&msg_in); 
+}
+
+static void on_spistream_msg_received(uint8_t msg_id, 
+                                      uint8_t * data, 
+                                      uint16_t num_bytes) {
+	static uint16_t plot_freq = 100; 
+
+	uint16_t log_bytes; 
+  uint8_t cnt; 
+	struct tm * timeinfo; 
+	time_t c_time;
+	char time_str[30];
+
+	plot_freq = 100; 
+	time(&c_time); 
+
+	timeinfo = localtime(&c_time);
+	strftime(time_str, 30, " %X ", timeinfo); 
+
+	log_bytes = num_bytes; 
+	if(log_bytes > 48) { log_bytes = 48; } 
+	printf("SPI message received: ");
+	printf("%s | Length: %3d | id: %3d | UART%d | ", time_str, num_bytes, msg_id, data[0]);
+	for(cnt = 1; cnt < log_bytes; cnt++) { 
+		printf("%02X ", data[cnt]);
+	}
+	printf("\n");
+}
+
+static void on_spistream_msg_sent(void) { 
+//  TRACE(TRACE_DEBUG, "%s", "SPI message sent \n");
+}
+
+static void main_init(void) {
+  uint8_t byte_idx; 
+
+  TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
+
+  /* Initalize our SPI link to IO processor */
+  if (spi_link_init()) {
+    TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
+    return;
+  }
+
+  spistream_init(&on_spistream_msg_received, 
+                 &on_spistream_msg_sent); 
+/*
+	spistream_msg[0] = 0; 
+  for(byte_idx=1; byte_idx < 123; byte_idx += 4) { 
+    spistream_msg[byte_idx]   = 0xDE;
+    spistream_msg[byte_idx+1] = 0xAD;
+    spistream_msg[byte_idx+2] = 0xBE;
+    spistream_msg[byte_idx+3] = 0xEF;
+  }
+*/
+  for(byte_idx=1; byte_idx < 123; byte_idx++) { 
+    spistream_msg[byte_idx] = byte_idx; 
+  }
+  /* Initalize the event library */
+  event_init();
+  
+  /* Initalize our ô so accurate periodic timer */
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return; 
+  }
+  
+  TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
+}
+
+static void parse_command_line(int argc, char** argv) {
+}

sw/airborne/lisa/lisa_spistream.h

@@ -0,0 +1,271 @@
+#ifndef SPISTREAM_PROTOCOL_H__
+#define SPISTREAM_PROTOCOL_H__
+
+#include <string.h>
+
+#ifndef SPISTREAM_MAX_MESSAGE_LENGTH
+#define SPISTREAM_MAX_MESSAGE_LENGTH 720
+#endif
+
+#ifndef SPISTREAM_MAX_RX_MESSAGE_LENGTH
+#define SPISTREAM_MAX_RX_MESSAGE_LENGTH SPISTREAM_MAX_MESSAGE_LENGTH
+#endif
+#ifndef SPISTREAM_RX_BUFFER_SIZE
+#define SPISTREAM_RX_BUFFER_SIZE SPISTREAM_MAX_RX_MESSAGE_LENGTH
+#endif
+
+#ifndef SPISTREAM_MAX_TX_MESSAGE_LENGTH
+#define SPISTREAM_MAX_TX_MESSAGE_LENGTH SPISTREAM_MAX_MESSAGE_LENGTH
+#endif
+#ifndef SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS
+#define SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS 4
+#endif
+#ifndef SPISTREAM_TX_MAX_BUFFER_PACKAGES
+#define SPISTREAM_TX_MAX_BUFFER_PACKAGES ( \
+		(SPISTREAM_MAX_TX_MESSAGE_LENGTH / \
+			SPISTREAM_PACKAGE_SIZE) * \
+		SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS)
+#endif
+
+#define SPISTREAM_INVALID_MESSAGE_ID 0 
+
+struct spistream_state_t { 
+    uint8_t tx_message_cnt;     // message cnt of next message to be sent
+    uint8_t rx_package_cntd; 
+};
+
+struct spistream_message_range_t { 
+	uint8_t index; 
+	uint8_t size; 
+};
+
+struct spistream_buffers_t { 
+		uint16_t rx_num_packages; // number of packages in buffer
+
+		uint16_t tx_insert;       // next index for package insertion
+		uint16_t tx_read;         // next index to read package from
+		uint16_t tx_num_packages; // number of packages in buffer
+
+// RX stores data as array
+    uint8_t  rx[SPISTREAM_RX_BUFFER_SIZE]; 
+// TX stores packages
+    struct AutopilotMessagePTStream tx[SPISTREAM_TX_MAX_BUFFER_PACKAGES]; 
+};
+
+typedef void (*spistream_message_rx_handler_t)(uint8_t msg_id, uint8_t * data, uint16_t num_bytes);
+typedef void (*spistream_message_tx_handler_t)(uint8_t msg_id);
+struct spistream_config_t { 
+// Handler to call for processing received message
+    spistream_message_rx_handler_t message_rx_handler; 
+// Handler to call after message transmission
+    spistream_message_tx_handler_t message_tx_handler; 
+};
+
+enum spistream_flag { SPISTREAM_NO_WAIT=0, SPISTREAM_WAIT_FOR_READ };
+
+/* Function declarations */
+
+static inline void spistream_init(spistream_message_rx_handler_t message_rx_handler, 
+                                  spistream_message_tx_handler_t message_tx_handler);
+static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in);
+static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out);
+static inline uint8_t spistream_send_msg(uint8_t * data, uint16_t num_bytes, enum spistream_flag);
+
+/* Definitions */
+
+static struct spistream_state_t   spistream_state; 
+static struct spistream_buffers_t spistream_buffers; 
+static struct spistream_config_t  spistream; 
+
+static inline void spistream_init(spistream_message_rx_handler_t message_rx_handler, 
+                                  spistream_message_tx_handler_t message_tx_handler)
+{ 
+    memset(&spistream_buffers, 0, sizeof(struct spistream_buffers_t)); 
+    memset(&spistream_state,   0, sizeof(struct spistream_state_t)); 
+    spistream.message_rx_handler = message_rx_handler; 
+    spistream.message_tx_handler = message_tx_handler; 
+    spistream_buffers.rx_num_packages = 0; 
+    spistream_state.rx_package_cntd   = 0; 
+    spistream_buffers.tx_num_packages = 0; 
+}
+
+/**
+ * Read a single package into internal RX buffer. 
+ * Converts data from package domain to byte array. 
+ * After receiving a full message, the registered spistream.message_rx_handler 
+ * function is called. 
+ * Called on every SPI event. 
+ */
+static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in) 
+{ 
+		uint8_t package_cntd;
+
+    if(pkg_in->message_cnt == SPISTREAM_INVALID_MESSAGE_ID) { 
+        return; 
+    } 
+
+		// In the last package of every message, the package_cntd is expected to be 
+		// negative or 0. It indicates the number of zero-bytes that are padded to 
+		// the end of the message to fill a package. 
+		if(pkg_in->package_cntd <= 0) { package_cntd = 1; } 
+		else { package_cntd = pkg_in->package_cntd; } 
+
+		if(pkg_in->package_cntd >= spistream_buffers.rx_num_packages) { 
+			spistream_buffers.rx_num_packages = pkg_in->package_cntd;
+		}
+    
+    if(spistream_state.rx_package_cntd == 0) { // Beginning of new message
+        // Message length is first value of package countdown: 
+        spistream_buffers.rx_num_packages = package_cntd; 
+				spistream_state.rx_package_cntd   = package_cntd;
+    }
+    memcpy(spistream_buffers.rx + 
+						((spistream_buffers.rx_num_packages - package_cntd) * 
+						 SPISTREAM_PACKAGE_SIZE), 
+           pkg_in->pkg_data, 
+           SPISTREAM_PACKAGE_SIZE);
+
+    if(pkg_in->package_cntd <= 0) { 
+      // Message is ready, pass to handler: 
+			spistream.message_rx_handler(pkg_in->message_cnt,
+																	 (uint8_t *)(spistream_buffers.rx), 
+																	 (spistream_buffers.rx_num_packages * 
+																		SPISTREAM_PACKAGE_SIZE) + 
+																	 pkg_in->package_cntd);
+			spistream_state.rx_package_cntd = 0; 
+   } 
+}
+
+/**
+ * Fill given SPI package with next package from TX buffer. 
+ * Called on every SPI event. 
+ */
+static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out) 
+{ 
+    if(spistream_buffers.tx_num_packages == 0) { 
+			memset(pkg_out, 0, sizeof(struct AutopilotMessagePTStream));
+      pkg_out->message_cnt = SPISTREAM_INVALID_MESSAGE_ID;
+      return; 
+    } 
+
+		memcpy(pkg_out, 
+				   spistream_buffers.tx + spistream_buffers.tx_read, 
+				   sizeof(struct AutopilotMessagePTStream));
+		if(pkg_out->package_cntd <= 0) { 
+			spistream.message_tx_handler(pkg_out->message_cnt); 
+		}
+
+		spistream_buffers.tx_read++; 
+		if(spistream_buffers.tx_read >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) { 
+			spistream_buffers.tx_read = 0; 
+		}
+
+		spistream_buffers.tx_num_packages--;
+}
+
+/**
+ * Enqueue given message in TX buffer. 
+ * This function is directly wrapped by spistream_send_message 
+ * at the moment. 
+ */
+static inline uint8_t spistream_enqueue_msg(uint8_t * data, 
+																						uint16_t num_bytes, 
+																						enum spistream_flag wait_for_read) 
+{ 
+	uint16_t pkg_idx, num_packages, num_padding; 
+	uint16_t idx; 
+	// Enough space in buffer?
+
+	if(wait_for_read == SPISTREAM_NO_WAIT || 
+		 spistream_buffers.tx_num_packages+1 < SPISTREAM_TX_MAX_BUFFER_PACKAGES) 
+	{ 
+		spistream_state.tx_message_cnt++;
+		// Message id 0 is reserved for invalid packages: 
+		if(spistream_state.tx_message_cnt == SPISTREAM_INVALID_MESSAGE_ID) { 
+			spistream_state.tx_message_cnt = 1; 
+		}
+		// How many packages we need for this message: 
+		num_packages = (num_bytes / SPISTREAM_PACKAGE_SIZE); 
+		if(num_bytes % SPISTREAM_PACKAGE_SIZE != 0) { 
+			num_packages++; 
+		} 
+		// How many zero-bytes we will have at the end of the last package: 
+		if(num_bytes > SPISTREAM_PACKAGE_SIZE) { 
+			num_padding = (num_packages * SPISTREAM_PACKAGE_SIZE) - num_bytes;
+		}
+		else { 
+			num_padding = SPISTREAM_PACKAGE_SIZE - num_bytes; 
+		}
+
+		pkg_idx = spistream_buffers.tx_insert; 
+
+		// Convert data to packages and add them to TX buffer: 
+		for(idx = 0; num_packages > 0; idx++) { 
+			if(idx < num_bytes) { 
+				spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = data[idx];
+			}
+			else { // padding
+				spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = 0; 
+			}
+			// Last byte in current package: 
+			if((idx % SPISTREAM_PACKAGE_SIZE) == SPISTREAM_PACKAGE_SIZE-1) { 
+
+			// Finish configuration of current package
+				// Last package uses field package_cntd to indicate the number 
+				// of padding bytes it contains, as negative number: 
+				if(num_packages == 1) { 
+					spistream_buffers.tx[pkg_idx].package_cntd = -num_padding; 
+				}
+				else { 
+					spistream_buffers.tx[pkg_idx].package_cntd = num_packages; 
+				}
+				spistream_buffers.tx[pkg_idx].message_cnt = spistream_state.tx_message_cnt; 
+
+			// Prepare next package: 
+				num_packages--; 
+				// Increment insert pointer with ring buffer overflow: 
+				spistream_buffers.tx_insert++;
+				if(spistream_buffers.tx_insert >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) { 
+					spistream_buffers.tx_insert = 0; 
+				}
+				// Continue with next package: 
+				pkg_idx = spistream_buffers.tx_insert; 
+				spistream_buffers.tx_num_packages++;
+			}
+		}
+#if 0
+printf("Enqueue finished. Buffer: \n");
+		for(pkg_idx = 0; pkg_idx < spistream_buffers.tx_num_packages; pkg_idx++) { 
+			printf("Package %2d | %3d |: ", pkg_idx, spistream_buffers.tx[pkg_idx].package_cntd); 
+			for(idx = 0; idx < SPISTREAM_PACKAGE_SIZE; idx++) { 
+				printf("%3d ", spistream_buffers.tx[pkg_idx].pkg_data[idx]);
+			}
+			printf("\n");
+		}
+#endif
+
+		return 1; 
+	}
+	return 0; 
+}
+
+static inline void spistream_dequeue_msg(uint8_t message_id) { 
+}
+
+/**
+ * Used from userland: Send num_bytes bytes from buffer over spistream. 
+ * Flags are: 
+ * - SPISTREAM_WAIT_FOR_READ:  Reject packages when TX buffer is full
+ *   and return 0, otherwise enqueue message and return 1. 
+ * - SPISTREAM_NO_WAIT: Overwrite data if TX buffer is full, enqueue 
+ *   message and always return 1. 
+ */
+static inline uint8_t spistream_send_msg(uint8_t * data, 
+																			   uint16_t num_bytes, 
+																				 enum spistream_flag wait_for_read) 
+{
+    return spistream_enqueue_msg(data, num_bytes, wait_for_read);
+}
+
+#endif /* SPISTREAM_PROTOCOL_H__ */
+

sw/airborne/lisa/lisa_stm_gps_passthrough_main.c

@@ -0,0 +1,349 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include "init_hw.h"
+#include "sys_time.h"
+#include "lisa/lisa_overo_link.h"
+#include "lisa/lisa_spistream.h"
+#include "airframe.h"
+#include "uart.h"
+
+static inline void main_init(void);
+static inline void main_periodic(void);
+static inline void main_event(void);
+static inline void uart_transfer_event(void);
+
+static inline void on_spistream_msg_received(uint8_t msg_id, uint8_t * data, uint16_t num_bytes);
+static inline void on_spistream_msg_sent(uint8_t msg_id);
+
+static inline void on_overo_link_msg_received(void);
+static inline void on_overo_link_lost(void);
+static inline void on_overo_link_crc_failed(void);
+
+#ifdef SPISTREAM_DEBUG
+static inline void uart_debug_transfer_event(void); 
+static inline void uart_debug_transfer_init(void); 
+#endif
+
+struct __attribute__ ((packed)) spistream_uart_msg {
+  uint8_t uart_id; 
+  uint8_t uart_data[SPISTREAM_MAX_MESSAGE_LENGTH];
+};
+
+// TODO 
+// Use 3 static instances of this struct in uart_transfer_event 
+// instead of a myriad of repetitive static vars:
+struct uart_state { 
+	struct spistream_uart_msg * msg; 
+	uint32_t timeout; 
+	uint32_t num_rx_bytes; 
+	uint8_t enabled; 
+	uint8_t has_data; 
+	uint8_t sent; 
+};
+
+static struct spistream_uart_msg spistream_uart1_msg;
+static struct spistream_uart_msg spistream_uart2_msg;
+static struct spistream_uart_msg spistream_uart3_msg;
+#ifdef SPISTREAM_DEBUG
+static struct spistream_uart_msg spistream_debug_msg;
+#endif
+
+// The number of current, unfinished spistream transfers. 
+// Incremented after sending an spistream message, decremented 
+// in spistream TX completion handler (here: on_spistream_msg_sent). 
+static uint8_t spistream_wait_for_num_transfers = 0; 
+
+int main(void) {
+
+  main_init();
+
+  while (1) {
+    if (sys_time_periodic()) {
+      main_periodic();
+    }
+    main_event();
+  }
+  
+  return 0;
+}
+
+static inline void main_init(void) {
+
+  hw_init();
+  sys_time_init();
+  overo_link_init();
+
+  uart1_init(); 
+  uart2_init(); 
+  uart3_init(); 
+
+  spistream_uart1_msg.uart_id = 1; 
+  spistream_uart2_msg.uart_id = 2; 
+  spistream_uart3_msg.uart_id = 3; 
+
+#ifdef SPISTREAM_DEBUG
+	uart_debug_transfer_init(); 
+#endif
+
+  spistream_init(&on_spistream_msg_received, 
+                 &on_spistream_msg_sent);
+
+}
+
+/**
+ * Handler for commands (messages from Overo->STM). 
+ * Right now, it is just sending the command back for 
+ * debugging purposes. 
+ */
+static inline void on_spistream_msg_received(uint8_t msg_id, 
+                                             uint8_t * data, 
+                                             uint16_t num_bytes) 
+{ 
+	spistream_send_msg(data, num_bytes, SPISTREAM_NO_WAIT); 
+}
+
+static inline void on_spistream_msg_sent(uint8_t msg_id) { 
+	if(spistream_wait_for_num_transfers > 0) { 
+  	spistream_wait_for_num_transfers--;
+	}
+}
+
+static inline void main_periodic(void) 
+{
+  OveroLinkPeriodic(on_overo_link_lost);
+
+  RunOnceEvery(1, {
+      LED_PERIODIC(); 
+  });
+}
+
+/**
+ * Every SPI transfer contains exactly two packages of type 
+ * AutopilotMessagePTStream, one for each direction 
+ * (up: STM->Overo, down: Overo->STM). 
+ * As we delegate SPI message handling to spistream, the SPI 
+ * event just passes both packages to 
+ * spistream_read_pkg(down_pkg) and spistream_write_pkg(up_pkg). 
+ * Apart from that, we just don't care about the SPI driver 
+ * itself. 
+ */
+static inline void on_overo_link_msg_received(void) 
+{
+  spistream_read_pkg(&overo_link.down.msg);
+  spistream_write_pkg(&overo_link.up.msg); 
+}
+
+static inline void on_overo_link_lost(void) {
+}
+
+static inline void on_overo_link_crc_failed(void) {
+}
+
+static inline void main_event(void) 
+{
+  OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed);
+
+#ifdef SPISTREAM_DEBUG
+	uart_debug_transfer_event(); 
+#else
+	uart_transfer_event(); 
+#endif
+}
+
+#ifdef SPISTREAM_DEBUG
+static inline void uart_debug_transfer_init(void) { 
+	uint16_t idx; 
+	for(idx = 1; idx < 700; idx++) { 
+		spistream_debug_msg.uart_data[idx] = idx % 40; 
+	}
+}
+#endif
+
+/**
+ * spistream stress test: Send big (500-700 bytes) messages 
+ * with different message lengths for every channel and 
+ * length varying in every message. 
+ * Fool around with timeout to increase/decrease message 
+ * rate to see when it can't keep up any more. 
+ */
+#ifdef SPISTREAM_DEBUG
+static inline void uart_debug_transfer_event(void) { 
+	static uint16_t len = 0; 
+	static uint16_t timeout = 1; 
+
+	if(timeout-- == 0) {  
+		timeout = 8000; 
+		if(spistream_wait_for_num_transfers == 0) 
+		{ 
+			LED_OFF(6);
+			len++; 
+			if(len > 700) { len = 500; } 
+
+			spistream_debug_msg.uart_id = 1; 
+			if(spistream_send_msg((uint8_t *)&spistream_debug_msg, 
+														len+1-20, 
+														SPISTREAM_WAIT_FOR_READ)) { // +1 for UART id byte
+				spistream_wait_for_num_transfers++; 
+			}
+			spistream_debug_msg.uart_id = 2; 
+			if(spistream_send_msg((uint8_t *)&spistream_debug_msg, 
+														len+1, 
+														SPISTREAM_WAIT_FOR_READ)) { // +1 for UART id byte
+				spistream_wait_for_num_transfers++; 
+			}
+			spistream_debug_msg.uart_id = 3; 
+			if(spistream_send_msg((uint8_t *)&spistream_debug_msg, 
+														len+1+20, 
+														SPISTREAM_WAIT_FOR_READ)) { // +1 for UART id byte
+				spistream_wait_for_num_transfers++; 
+			}
+		}
+		else { 
+			LED_ON(6);
+		}
+	}
+}
+#endif
+
+static inline void uart_transfer_event(void) { 
+
+  static uint16_t uart1_num_rx_bytes = 0; 
+  static uint16_t uart2_num_rx_bytes = 0; 
+  static uint16_t uart3_num_rx_bytes = 0; 
+  static uint32_t timeout_trig  = 2; 
+  static uint32_t timeout_uart1 = 0; 
+  static uint32_t timeout_uart2 = 0; 
+  static uint32_t timeout_uart3 = 0; 
+  static uint8_t uart1_sent = 0; 
+  static uint8_t uart2_sent = 0; 
+  static uint8_t uart3_sent = 0; 
+  static uint8_t uart1_has_data = 0; 
+  static uint8_t uart2_has_data = 0; 
+  static uint8_t uart3_has_data = 0; 
+  static uint8_t trigger_send = 0; 
+
+	static uint8_t uart1_enabled = 1; 
+	static uint8_t uart2_enabled = 1; 
+	static uint8_t uart3_enabled = 1; 
+
+  // We cache data availability, so it doesn't change between checks: 
+  uart1_has_data = Uart1ChAvailable();
+  uart2_has_data = Uart2ChAvailable();
+  uart3_has_data = Uart3ChAvailable();
+
+	// Fill stage: Read data from UARTs into buffers, or increment 
+	// their timeouts if no data is available: 
+	if(!uart1_sent && uart1_has_data) { 
+		spistream_uart1_msg.uart_data[uart1_num_rx_bytes] = Uart1Getch();
+		timeout_uart1 = 0; 
+		if(uart1_num_rx_bytes < SPISTREAM_MAX_MESSAGE_LENGTH) 
+			{ uart1_num_rx_bytes++; }
+	} else { if(timeout_uart1 < timeout_trig) { timeout_uart1++; } }
+	
+	if(!uart2_sent && uart2_has_data) { 
+		spistream_uart2_msg.uart_data[uart2_num_rx_bytes] = Uart2Getch();
+		timeout_uart2 = 0; 
+		if(uart2_num_rx_bytes < SPISTREAM_MAX_MESSAGE_LENGTH) 
+			{ uart2_num_rx_bytes++; }
+	} else { if(timeout_uart2 < timeout_trig) { timeout_uart2++; } }
+
+	if(!uart3_sent && uart3_has_data) { 
+		spistream_uart3_msg.uart_data[uart3_num_rx_bytes] = Uart3Getch();
+		timeout_uart3 = 0; 
+		if(uart3_num_rx_bytes < SPISTREAM_MAX_MESSAGE_LENGTH) 
+			{ uart3_num_rx_bytes++; }
+	} else { if(timeout_uart3 < timeout_trig) { timeout_uart3++; } }
+
+	trigger_send = ((!uart1_enabled || 
+									(timeout_uart1 >= timeout_trig)) &&
+								 (!uart2_enabled || 
+									(timeout_uart2 >= timeout_trig)) &&
+								 (!uart3_enabled || 
+									(timeout_uart3 >= timeout_trig)));
+
+	// Send stage: If all UART timeouts reach the timeout 
+	// trigger value and have accumulated data to send
+	if(trigger_send) {
+
+    // If there was no new data on any UART for some time 
+    // and there is data in every rx buffer: 
+
+		if(spistream_wait_for_num_transfers > 0) 
+		{ 
+			// Warning LED: Could not finish all transactions 
+			// from last call. 
+			LED_ON(6); 
+		} 
+		else 
+		{
+			LED_OFF(6);
+
+			uart1_sent = !uart1_enabled; // If we set uartX_sent to 1 here, it 
+			uart2_sent = !uart2_enabled; // is just ignored for every read poll
+			uart3_sent = !uart3_enabled; // as it seems to have been read already. 
+
+      if(!uart1_sent && uart1_num_rx_bytes > 0) { 
+				if(spistream_send_msg((uint8_t *)&spistream_uart1_msg, 
+					    							  uart1_num_rx_bytes+1, // +1 for UART id
+						 									SPISTREAM_WAIT_FOR_READ)) { 
+					 uart1_sent = 1; 
+					 spistream_wait_for_num_transfers++; 
+				 }
+			}
+
+      if(!uart2_sent && uart1_num_rx_bytes > 0) { 
+				if(spistream_send_msg((uint8_t *)&spistream_uart2_msg, 
+						    							uart1_num_rx_bytes+1, // +1 for UART id
+						 									SPISTREAM_WAIT_FOR_READ)) { 
+					 uart2_sent = 1; 
+					 spistream_wait_for_num_transfers++; 
+				}
+			}
+
+      if(!uart3_sent && uart3_num_rx_bytes > 0) { 
+				if(spistream_send_msg((uint8_t *)&spistream_uart3_msg, 
+															uart3_num_rx_bytes+1, // +1 for UART id
+															SPISTREAM_WAIT_FOR_READ)) { 
+					uart3_sent = 1; 
+					spistream_wait_for_num_transfers++; 
+				}
+			}
+
+			// Transaction completed, reset state. 
+			// Note: Only reset when all uart buffers have been transmitted, 
+			// otherwise the timeout would start from the beginning and the 
+			// loop phase shifts (aka "you're in the deep"). 
+			uart1_num_rx_bytes = 0; 
+			uart2_num_rx_bytes = 0; 
+			uart3_num_rx_bytes = 0; 
+			timeout_uart1 = 0; 
+			timeout_uart2 = 0; 
+			timeout_uart3 = 0; 
+			uart1_sent = 0; 
+			uart2_sent = 0; 
+			uart3_sent = 0; 
+    }
+  } 
+}