pprz messages nb error handling

sw/ground_segment/tmtc/hw_modem_listen.ml

@@ -53,6 +53,7 @@ let use_pprz_message = fun (msg_id, values) ->
     ac_id := Pprz.string_assoc "id" values;
   Tele_Pprz.message_send !ac_id msg.Pprz.name values
 
+(** Listen on a serial device or on multimon pipe *)
 let listen_pprz_modem = fun pprz_message_cb tty ->
   let fd = 
     if String.sub tty 0 4 = "/dev" then
@@ -60,21 +61,31 @@ let listen_pprz_modem = fun pprz_message_cb tty ->
     else
       Unix.descr_of_in_channel (open_in tty)
   in
+
+  (** Callback for a checksumed pprz message *)
   let use_pprz_buf = fun buf ->
     status.rx_byte <- status.rx_byte + String.length buf;
     Debug.call 'P' (fun f -> fprintf f "use_pprz: %s\n" (Debug.xprint buf));
     pprz_message_cb (Tele_Pprz.values_of_bin buf) in
-  let buffer = ref "" in
+  (** Callback for a modem message *)
-  let use_modem_message = fun msg ->
+  let use_modem_message =
-    Debug.call 'M' (fun f -> fprintf f "use_modem: %s\n" (Debug.xprint msg));
+    let buffer = ref "" in
-    match Modem.parse msg with
+    fun msg ->
-      None -> () (* Only internal modem data *)
+      Debug.call 'M' (fun f -> fprintf f "use_modem: %s\n" (Debug.xprint msg));
-    | Some data ->
+      match Modem.parse msg with
-	let b = !buffer ^ data in
+	None -> () (* Only internal modem data *)
-	Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b));
+      | Some data ->
-	let x = PprzTransport.parse use_pprz_buf b in
+	  (** Accumulate in a buffer *)
-	buffer := String.sub b x (String.length b - x)
+	  let b = !buffer ^ data in
+	  Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b));
+	  (** Parse as pprz message and ... *)
+	  let x = PprzTransport.parse use_pprz_buf b in
+	  status.rx_err <- !PprzTransport.nb_err;
+	  (** ... remove from the buffer the chars which have been used *)
+	  buffer := String.sub b x (String.length b - x)
   in
+
+  (** Callback for available chars on the channel *)
   let scanner = Serial.input (ModemTransport.parse use_modem_message) in
   let cb = fun _ ->
     begin
@@ -85,6 +96,7 @@ let listen_pprz_modem = fun pprz_message_cb tty ->
     end;
     true in
   
+  (** Attach the callback to the channel *)
   ignore (Glib.Io.add_watch [`IN] cb (Glib.Io.channel_of_descr fd))
 
 (** Modem monitoring messages *)

sw/lib/ocaml/serial.ml

@@ -92,12 +92,16 @@ module type PROTOCOL = sig
 end
 
 module Transport(Protocol:PROTOCOL) = struct
+  let nb_err = ref 0
+  let discarded_bytes = ref 0
   let rec parse = fun use buf ->
     Debug.call 'T' (fun f -> fprintf f "Transport.parse: %s\n" (Debug.xprint buf));
+    (** ref required due to Not_enough exception raised by Protocol.length *)
     let start = ref 0
     and n = String.length buf in
     try
       start := Protocol.index_start buf;
+      discarded_bytes := !discarded_bytes + !start;
       let length = Protocol.length buf !start in
       let end_ = !start + length in
       if n < end_ then
@@ -105,9 +109,11 @@ module Transport(Protocol:PROTOCOL) = struct
       let msg = String.sub buf !start length in
       if Protocol.checksum msg then begin
 	use msg
-      end else
+      end else begin
+	incr nb_err;
+	discarded_bytes := !discarded_bytes + length;
 	Debug.call 'T' (fun f -> fprintf f "Transport.chk: %s\n" (Debug.xprint msg))
-	;
+      end;
       end_ + parse use (String.sub buf end_ (String.length buf - end_))
     with
       Not_found -> String.length buf

sw/lib/ocaml/serial.mli

@@ -70,6 +70,8 @@ module type PROTOCOL =
 module Transport :
   functor (Protocol : PROTOCOL) ->
     sig 
+      val nb_err : int ref (* Errors on checksum *)
+      val discarded_bytes : int ref
       val parse : (string -> unit) -> string -> int
       (** [parse f buf] Scans [buf] according to [Protocol] and applies [f] on
        every recognised message. Returns the number of consumed bytes. *)