adding test program for periodic on overo

2026-06-07 17:49:49 +08:00 · 2010-06-14 16:38:07 +00:00
parent dd0f707410
commit c4c0cfb4be
1 changed files with 95 additions and 0 deletions
@@ -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);
+}
+