diff --git a/sw/airborne/fms/fms_spistream.h b/sw/airborne/fms/fms_spistream.h new file mode 100644 index 0000000000..f3a964bca7 --- /dev/null +++ b/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 + diff --git a/sw/airborne/fms/fms_spistream_client.c b/sw/airborne/fms/fms_spistream_client.c new file mode 100644 index 0000000000..afa45a7e5e --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +// Socket stuff +#include +#include +#include +#include +#include +#include +// End socket stuff + +#include + +#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); +} diff --git a/sw/airborne/fms/fms_spistream_daemon.c b/sw/airborne/fms/fms_spistream_daemon.c new file mode 100644 index 0000000000..bc03f97a48 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +// Socket stuff +#include +#include +#include +#include +#include +#include +// End socket stuff + +#include + +#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; + } +} + diff --git a/sw/airborne/fms/overo_test_gps_passthrough.c b/sw/airborne/fms/overo_test_gps_passthrough.c new file mode 100644 index 0000000000..b61f87149b --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +#include + +#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) { +} diff --git a/sw/airborne/lisa/lisa_spistream.h b/sw/airborne/lisa/lisa_spistream.h new file mode 100644 index 0000000000..20384e97d4 --- /dev/null +++ b/sw/airborne/lisa/lisa_spistream.h @@ -0,0 +1,271 @@ +#ifndef SPISTREAM_PROTOCOL_H__ +#define SPISTREAM_PROTOCOL_H__ + +#include + +#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__ */ + diff --git a/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c b/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c new file mode 100644 index 0000000000..c82e7f7331 --- /dev/null +++ b/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; + } + } +}