Add support for modified pprz protocol, called pprz2. Uses 0x98 as start byte, and 4 byte timestamp in the header. Used for telemetry only (uplink uses original pprz protocol for now). Enable with -transport pprz2 for link and UDP_TRANSPORT_TIMESTAMP for udp_transport.h

sw/airborne/fms/udp_transport.h

@@ -6,15 +6,15 @@
 #include "std.h"
 
 #ifdef UDP_TRANSPORT_TIMESTAMP
-#define STX_TS  0x98
-#define UdpTransportSizeOf(_payload) (_payload + 8)
+#define STX_UDP_TX  0x98
+#define STX_UDP_RX  0x99
+#define PPRZ_PROTOCOL_OVERHEAD 8
 #define UdpTransportPutTimestamp(x) UdpTransportPutUint32ByAddr(x)
-#define UdpTransportPutSTX() UdpTransportPut1Byte(STX_TS)
 #else
-#define STX  0x99
-#define UdpTransportSizeOf(_payload) (_payload + 4)
+#define STX_UDP_TX  0x99
+#define STX_UDP_RX  0x99
+#define PPRZ_PROTOCOL_OVERHEAD 4
 #define UdpTransportPutTimestamp(x) {} 
-#define UdpTransportPutSTX() UdpTransportPut1Byte(STX)
 #endif
 
 #ifndef MSG_TIMESTAMP
@@ -27,6 +27,10 @@ extern uint16_t udpt_tx_buf_idx;
 extern uint8_t udpt_ck_a, udpt_ck_b;
 #define  UDPT_TX_BUF_WATERMARK 1024
 
+#define UdpTransportSizeOf(_payload) (_payload + PPRZ_PROTOCOL_OVERHEAD)
+
+#define UdpTransportPutSTX() UdpTransportPut1Byte(STX_UDP_TX)
+
 #define UdpTransportPeriodic() {				\
     if (udpt_tx_buf_idx) {					\
       int len;	  						\
@@ -137,7 +141,7 @@ static inline void parse_udp_dl( uint8_t c ) {
 
   switch (udp_dl_status) {
   case UNINIT:
-    if (c == STX)
+    if (c == STX_UDP_RX)
       udp_dl_status++;
     break;
   case GOT_STX:

sw/ground_segment/tmtc/link.ml

@@ -34,13 +34,16 @@ module Tm_Pprz = Pprz.Messages (struct let name = "telemetry" end)
 module Ground_Pprz = Pprz.Messages (struct let name = "ground" end)
 module Dl_Pprz = Pprz.Messages (struct let name = "datalink" end)
 module PprzTransport = Serial.Transport (Pprz.Transport)
+module PprzTransportExtended = Serial.Transport (Pprz.TransportExtended)
 
 (* Modem transport layer *)
 type transport =
     Pprz  (* Paparazzi protocol, with A/C id, message id and CRC *)
+  | Pprz2  (* Paparazzi protocol, with timestamp, A/C id, message id and CRC *)
   | XBee  (* Maxstream protocol, API mode *)
 let transport_of_string = function
     "pprz" -> Pprz
+  | "pprz2" -> Pprz2
   | "xbee" -> XBee
   | x -> invalid_arg (sprintf "transport_of_string: %s" x)
 
@@ -325,6 +328,11 @@ let broadcast = fun device payload _priority ->
         let buf = Pprz.Transport.packet payload in
 	Printf.fprintf o "%s" buf; flush o;
         Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
+    | Pprz2 ->
+        let o = Unix.out_channel_of_descr device.fd in
+        let buf = Pprz.TransportExtended.packet payload in
+	Printf.fprintf o "%s" buf; flush o;
+        Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf));
     | XBee ->
         XB.send device payload
 
@@ -345,7 +353,7 @@ let parser_of_device = fun device ->
   match device.transport with
     Pprz ->
       let use = fun s ->
-	let raw_data_size = String.length (Serial.string_of_payload s) + 4(*stx,len,ck_a, ck_b*) in
+	let raw_data_size = String.length (Serial.string_of_payload s) + 4 (*stx,len,ck_a, ck_b*) in
 	let udp_peername =
 	  if !udp then
 	    Some !last_udp_peername
@@ -353,6 +361,16 @@ let parser_of_device = fun device ->
 	    None in
 	use_tele_message ?udp_peername ~raw_data_size s in
       PprzTransport.parse use
+  | Pprz2 ->
+      let use = fun s ->
+	let raw_data_size = String.length (Serial.string_of_payload s) + 8 (*stx,len, timestamp, ck_a, ck_b*) in
+	let udp_peername =
+	  if !udp then
+	    Some !last_udp_peername
+	  else
+	    None in
+	use_tele_message ?udp_peername ~raw_data_size s in
+      PprzTransportExtended.parse use
   | XBee ->
       let module XbeeTransport = Serial.Transport (Xbee.Protocol) in
       XbeeTransport.parse (XB.use_message device)
@@ -424,8 +442,8 @@ let () =
       "-noac_info", Arg.Clear ac_info, (sprintf "Disables AC traffic info (uplink).");
       "-nouplink", Arg.Clear uplink, (sprintf "Disables the uplink (from the ground to the aircraft).");
       "-s", Arg.Set_string baudrate, (sprintf "<baudrate>  Default is %s" !baudrate);
-      "-timestamp", Arg.Unit (fun () -> add_timestamp := Some (Unix.gettimeofday ())), "Add timestamp to messages sent over ivy";
-      "-transport", Arg.Set_string transport, (sprintf "<transport> Available protocols are modem,pprz and xbee. Default is %s" !transport);
+      "-local_timestamp", Arg.Unit (fun () -> add_timestamp := Some (Unix.gettimeofday ())), "Add local timestamp to messages sent over ivy";
+      "-transport", Arg.Set_string transport, (sprintf "<transport> Available protocols are modem,pprz,pprz2 and xbee. Default is %s" !transport);
       "-udp", Arg.Set udp, "Listen a UDP connection on <udp_port>";
       "-udp_port", Arg.Set_int udp_port, (sprintf "<UDP port> Default is %d" !udp_port);
       "-udp_uplink_port", Arg.Set_int udp_uplink_port, (sprintf "<UDP uplink port> Default is %d" !udp_uplink_port);

sw/lib/ocaml/pprz.ml

@@ -324,20 +324,24 @@ let hex_of_int_array = function
 
 exception Unknown_msg_name of string * string
 
-module Transport = struct
-  let stx = Char.chr 0x99 (** sw/airborne/pprz_transport.h *)
-  let offset_length = 1
-  let offset_payload = 2
+module type TRANSPORT_TYPE = sig
+  val stx : char
+  val offset_length : int
+  val offset_payload : int
+  val offset_timestamp : int
+  val overhead_length : int
+end
 
+module PprzTransportBase(SubType:TRANSPORT_TYPE) = struct
   let index_start = fun buf ->
-    String.index buf stx
+    String.index buf SubType.stx
 
   let length = fun buf start ->
     let len = String.length buf - start in
-    if len > offset_length then
-      let l = Char.code buf.[start+offset_length] in
+    if len > SubType.offset_length then
+      let l = Char.code buf.[start+SubType.offset_length] in
       Debug.call 'T' (fun f -> fprintf f "Pprz len=%d\n" l);
-      max l 4 (** if l<4 (4=stx+length+ck_a+ck_b), it's not a valid length *)
+      max l SubType.overhead_length (** if l<4 (4=stx+length+ck_a+ck_b), it's not a valid length *)
     else
       raise Serial.Not_enough
 
@@ -359,26 +363,45 @@ module Transport = struct
 
   let payload = fun msg ->
     let l = String.length msg in
-    assert(Char.code msg.[offset_length] = l);
-    assert(l >= 4);
-    Serial.payload_of_string (String.sub msg offset_payload (l-4))
+    assert(Char.code msg.[SubType.offset_length] = l);
+    assert(l >= SubType.overhead_length);
+    Serial.payload_of_string (String.sub msg SubType.offset_payload (l-SubType.overhead_length))
 
   let packet = fun payload ->
     let payload = Serial.string_of_payload payload in
     let n = String.length payload in
-    let msg_length = n + 4 in (** + stx, len, ck_a and ck_b *)
+    let msg_length = n + SubType.overhead_length in (** + stx, len, ck_a and ck_b *)
     if msg_length >= 256 then
       invalid_arg "Pprz.Transport.packet";
     let m = String.create msg_length in
-    String.blit payload 0 m offset_payload n;
-    m.[0] <- stx;
-    m.[offset_length] <- Char.chr msg_length;
+    String.blit payload 0 m SubType.offset_payload n;
+    m.[0] <- SubType.stx;
+    m.[SubType.offset_length] <- Char.chr msg_length;
     let (ck_a, ck_b) = compute_checksum m in
     m.[msg_length-2] <- Char.chr ck_a;
     m.[msg_length-1] <- Char.chr ck_b;
     m
 end
 
+module PprzTypeStandard = struct
+  let stx = Char.chr 0x99
+  let offset_length = 1
+  let offset_timestamp = -1
+  let offset_payload = 2
+  let overhead_length = 4
+end
+
+module PprzTypeTimestamp = struct
+  let stx = Char.chr 0x98
+  let offset_length = 1
+  let offset_timestamp = 2
+  let offset_payload = 6
+  let overhead_length = 8
+end
+
+module Transport = PprzTransportBase (PprzTypeStandard)
+module TransportExtended = PprzTransportBase (PprzTypeTimestamp)
+
 let offset_ac_id = 0
 let offset_msg_id = 1
 let offset_fields = 2

sw/lib/ocaml/pprz.mli

@@ -102,6 +102,18 @@ module Transport : Serial.PROTOCOL
     [packet] raises Invalid_Argument if length >= 256
  *)
 
+module TransportExtended : Serial.PROTOCOL
+(** Pprz frame (sw/airborne/pprz_transport.h):
+    |STX|length|timestamp|... payload=(length-4) bytes ...|Checksum A|Checksum B|
+    Where checksum is computed over length and payload:
+    ck_A = ck_B = 0;
+    for all byte b in payload
+      ck_A += b; ck_b += ck_A
+
+    STX = 0x98
+    [packet] raises Invalid_Argument if length >= 256
+ *)
+
 val offset_fields : int
 
 module type CLASS = sig