[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6085] Add gps passthrough and spistream files
|
From: |
Allen Ibara |
|
Subject: |
[paparazzi-commits] [6085] Add gps passthrough and spistream files |
|
Date: |
Wed, 06 Oct 2010 05:08:38 +0000 |
Revision: 6085
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6085
Author: aibara
Date: 2010-10-06 05:08:38 +0000 (Wed, 06 Oct 2010)
Log Message:
-----------
Add gps passthrough and spistream files
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/fms/fms_spistream.h
paparazzi3/trunk/sw/airborne/fms/fms_spistream_client.c
paparazzi3/trunk/sw/airborne/fms/fms_spistream_daemon.c
paparazzi3/trunk/sw/airborne/fms/overo_test_gps_passthrough.c
paparazzi3/trunk/sw/airborne/lisa/lisa_spistream.h
paparazzi3/trunk/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c
Added: paparazzi3/trunk/sw/airborne/fms/fms_spistream.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_spistream.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_spistream.h 2010-10-06 05:08:38 UTC
(rev 6085)
@@ -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
+
Added: paparazzi3/trunk/sw/airborne/fms/fms_spistream_client.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_spistream_client.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_spistream_client.c 2010-10-06
05:08:38 UTC (rev 6085)
@@ -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 \xF4 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);
+}
Added: paparazzi3/trunk/sw/airborne/fms/fms_spistream_daemon.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/fms_spistream_daemon.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/fms_spistream_daemon.c 2010-10-06
05:08:38 UTC (rev 6085)
@@ -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 \xF4 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;
+ }
+}
+
Added: paparazzi3/trunk/sw/airborne/fms/overo_test_gps_passthrough.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/overo_test_gps_passthrough.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/overo_test_gps_passthrough.c
2010-10-06 05:08:38 UTC (rev 6085)
@@ -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 \xF4 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) {
+}
Added: paparazzi3/trunk/sw/airborne/lisa/lisa_spistream.h
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_spistream.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_spistream.h 2010-10-06 05:08:38 UTC
(rev 6085)
@@ -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__ */
+
Added: paparazzi3/trunk/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c
2010-10-06 05:08:38 UTC (rev 6085)
@@ -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;
+ }
+ }
+}
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6085] Add gps passthrough and spistream files,
Allen Ibara <=