[paparazzi-commits] [4936] adding test program for periodic on overo

[antoine drouin]

Revision: 4936
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4936
Author:   poine
Date:     2010-06-14 16:38:07 +0000 (Mon, 14 Jun 2010)
Log Message:
-----------
adding test program for periodic on overo

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/fms/overo_test_periodic.c

Added: paparazzi3/trunk/sw/airborne/fms/overo_test_periodic.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/overo_test_periodic.c                      
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/overo_test_periodic.c      2010-06-14 
16:38:07 UTC (rev 4936)
@@ -0,0 +1,95 @@
+
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <event.h>
+
+//#define LINK_HOST  "10.31.4.7"
+//#define LINK_HOST  "stripe"
+#define LINK_HOST  "192.168.1.0"
+#define LINK_PORT        4242
+#define DATALINK_PORT    4243
+
+#include "airframe.h"
+
+#include "fms_periodic.h"
+#include "downlink.h"
+#include "udp_transport.h"
+#include "fms_network.h"
+#include "fms_spi_link.h"
+#include "fms_autopilot_msg.h"
+
+static void main_periodic(int);
+static void main_send_to_stm(void);
+
+static void on_datalink_event(int fd, short event, void *arg);
+static struct FmsNetwork* network;
+
+int main(int argc, char** argv) {
+
+  /* Initalize event library */
+  event_init();
+  
+  if (fms_periodic_init(main_periodic)) {
+    TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
+    return -1; 
+  }
+  
+  if (spi_link_init()) {
+    TRACE(TRACE_ERROR, "%s", "failed to open SPI link\n");
+    return -1;
+  }
+
+  /* Add our datalink event */
+  network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, 
FMS_NETWORK_BROADCAST);
+  struct event datalink_event;
+  event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, 
&datalink_event);
+  event_add(&datalink_event, NULL);
+  
+  event_dispatch();
+
+  return 0;
+}
+
+
+static void main_periodic(int my_sig_num) {
+
+  DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+
+  main_send_to_stm();
+
+  UdpTransportPeriodic();
+
+}
+
+
+static void main_send_to_stm(void) {
+
+  union AutopilotMessageTW msg_in, msg_out;
+  spi_link_send(&msg_out.down, sizeof(union AutopilotMessageTW), &msg_in.up);
+
+  printf("spi telemetry got %d\n", msg_in.up.tw_len);
+  //  for (int i=0; i<msg_in.up.tw_len; i++)
+  //    printf("%02x ",  msg_in.up.data[i]);
+  //  printf("\n");
+}
+
+
+static void on_datalink_event(int fd, short event, void *arg) {
+  char buf[255];
+  int bytes_read;
+  bytes_read = read(fd, buf, sizeof(buf) - 1);
+  if (bytes_read == -1) {
+    perror("read");
+    return;
+  } else if (bytes_read == 0) {
+    fprintf(stderr, "Connection closed\n");
+    return;
+  }
+  printf("on_datalink_event, read %d\n", bytes_read);
+
+  struct event *ev = arg;
+  event_add(ev, NULL);
+}
+