[paparazzi-commits] [5129] Add udp_transport2, based on udp_transport, using struct DownlinkTransport interface

[Allen Ibara]

Revision: 5129
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5129
Author:   aibara
Date:     2010-07-22 20:36:24 +0000 (Thu, 22 Jul 2010)
Log Message:
-----------
Add udp_transport2, based on udp_transport, using struct DownlinkTransport 
interface

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

Added: paparazzi3/trunk/sw/airborne/fms/udp_transport2.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/udp_transport2.c                           
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/udp_transport2.c   2010-07-22 20:36:24 UTC 
(rev 5129)
@@ -0,0 +1,100 @@
+#include <stdlib.h>
+#include "udp_transport2.h"
+#include "fms_network.h"
+#include "downlink_transport.h"
+
+
+static void put_1byte(struct udp_transport *udp, const uint8_t x)
+{
+  udp->updt_tx_buf[udp->udpt_tx_buf_idx] = x;
+  udp->udpt_tx_buf_idx++;
+}
+
+static void put_uint8_t(struct udp_transport *udp, const uint8_t byte)
+{
+  udp->udpt_ck_a += byte;
+  udp->udpt_ck_b += udp->udpt_ck_a;
+  put_1byte(udp, byte);
+}
+
+static void put_named_uint8_t(struct udp_transport *udp, char *name, const 
uint8_t byte)
+{
+  put_uint8_t(udp, byte);
+}
+
+static void put_bytes(struct udp_transport *udp, uint16_t len, const uint8_t 
*bytes)
+{
+  for (int i = 0; i < len; i++) {
+    put_uint8_t(udp, bytes[i]);
+  }
+}
+
+static void header(struct udp_transport *udp, uint8_t payload_len)
+{
+  uint32_t msg_timestamp = MSG_TIMESTAMP;
+  /*udpt_tx_buf_idx = 0;*/
+  put_1byte(udp, STX_UDP_TX);
+  uint8_t msg_len = payload_len + PPRZ_PROTOCOL_OVERHEAD;
+  udp->udpt_ck_a = udp->udpt_ck_b = 0;
+  put_uint8_t(udp, msg_len);
+  put_bytes(udp, 4, (uint8_t *) &msg_timestamp);
+}
+
+static void start_message(struct udp_transport *udp, char *name, uint8_t 
msg_id, uint16_t payload_len)
+{
+  //downlink_nb_msgs++;
+  header(udp, 2 + payload_len);
+  put_uint8_t(udp, AC_ID);
+  put_named_uint8_t(udp, name, msg_id);
+}
+
+static void end_message(struct udp_transport *udp)
+{
+  put_1byte(udp, udp->udpt_ck_a);
+  put_1byte(udp, udp->udpt_ck_b);
+  if (udp->udpt_tx_buf_idx > UDPT_TX_BUF_WATERMARK) {
+    network_write(udp->network, udp->updt_tx_buf, udp->udpt_tx_buf_idx);
+    udp->udpt_tx_buf_idx = 0;
+  }
+}
+
+static void overrun(struct udp_transport *udp)
+{
+  
+}
+
+static void count_bytes(struct udp_transport *udp, uint8_t bytes)
+{
+       
+}
+
+static int check_free_space(struct udp_transport *udp, uint8_t bytes)
+{
+       return TRUE;
+}
+
+static uint8_t size_of(struct udp_transport *udp, uint8_t len)
+{
+       return len + 2;
+}
+
+struct DownlinkTransport *udp_transport_new(struct FmsNetwork *network)
+{
+  struct DownlinkTransport *tp = calloc(1, sizeof(struct DownlinkTransport));
+  struct udp_transport *udp = calloc(1, sizeof(struct udp_transport));
+
+  udp->udp_dl_status = UNINIT;
+  udp->network = network;
+  tp->impl = udp;
+
+  tp->StartMessage = start_message;
+  tp->EndMessage = end_message;
+  tp->PutBytes = put_bytes;
+
+  tp->Overrun = overrun;
+  tp->CountBytes = count_bytes;
+  tp->SizeOf = size_of;
+  tp->CheckFreeSpace = check_free_space;
+
+  return tp;
+}

Added: paparazzi3/trunk/sw/airborne/fms/udp_transport2.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/udp_transport2.h                           
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/udp_transport2.h   2010-07-22 20:36:24 UTC 
(rev 5129)
@@ -0,0 +1,104 @@
+#ifndef UDP_TRANSPORT2_H
+#define UDP_TRANSPORT2_H
+
+#include "fms_network.h"
+#include "fms_debug.h"
+#include "std.h"
+#include "airframe.h"
+
+#ifdef UDP_TRANSPORT_TIMESTAMP
+#define STX_UDP_TX  0x98
+#define STX_UDP_RX  0x99
+#define PPRZ_PROTOCOL_OVERHEAD 8
+#else
+#define STX_UDP_TX  0x99
+#define STX_UDP_RX  0x99
+#define PPRZ_PROTOCOL_OVERHEAD 4
+#endif
+
+#ifndef MSG_TIMESTAMP
+#define MSG_TIMESTAMP 0
+#endif
+
+struct DownlinkTransport *udp_transport_new(struct FmsNetwork *network);
+
+#define UDPT_TX_BUF_LEN 1496
+#define UDPT_TX_BUF_WATERMARK 1024
+#define UDP_DL_PAYLOAD_LEN 256
+
+struct udp_transport {
+  /*
+   * Downlink
+   */
+  char updt_tx_buf[UDPT_TX_BUF_LEN];
+  uint16_t udpt_tx_buf_idx;
+  uint8_t udpt_ck_a, udpt_ck_b;
+  
+  /*
+   * Uplink
+   */
+  uint8_t udp_dl_payload[UDP_DL_PAYLOAD_LEN];
+  volatile uint8_t udp_dl_payload_len;
+  volatile bool_t udp_dl_msg_received;
+  uint8_t udp_dl_ovrn, udp_dl_nb_err;
+  uint8_t udp_dl_status;
+  uint8_t _ck_a, _ck_b, payload_idx;
+
+  struct FmsNetwork *network;
+};
+
+
+/*
+ * Parsing of uplink msg
+ */
+#define UNINIT 0
+#define GOT_STX 1
+#define GOT_LENGTH 2
+#define GOT_PAYLOAD 3
+#define GOT_CRC1 4
+
+static inline void parse_udp_dl( struct udp_transport *tp, uint8_t c ) {
+
+  switch (tp->udp_dl_status) {
+  case UNINIT:
+    if (c == STX_UDP_RX)
+      tp->udp_dl_status++;
+    break;
+  case GOT_STX:
+    if (tp->udp_dl_msg_received) {
+      tp->udp_dl_ovrn++;
+      goto error;
+    }
+    tp->udp_dl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
+    tp->_ck_a = tp->_ck_b = c;
+    tp->udp_dl_status++;
+    tp->payload_idx = 0;
+    break;
+  case GOT_LENGTH:
+    tp->udp_dl_payload[tp->payload_idx] = c;
+    tp->_ck_a += c; tp->_ck_b += tp->_ck_a;
+    tp->payload_idx++;
+    if (tp->payload_idx == tp->udp_dl_payload_len)
+      tp->udp_dl_status++;
+    break;
+  case GOT_PAYLOAD:
+    if (c != tp->_ck_a)
+      goto error;
+    tp->udp_dl_status++;
+    break;
+  case GOT_CRC1:
+    if (c != tp->_ck_b)
+      goto error;
+    tp->udp_dl_msg_received = TRUE;
+    goto restart;
+  }
+  return;
+ error:
+  tp->udp_dl_nb_err++;
+ restart:
+  tp->udp_dl_status = UNINIT;
+  return;
+}
+
+#endif /* UDP_TRANSPORT2_H */
+