diff --git a/sw/ground_segment/tmtc/hw_modem_listen.ml b/sw/ground_segment/tmtc/hw_modem_listen.ml index e3d2171140..e62d268220 100644 --- a/sw/ground_segment/tmtc/hw_modem_listen.ml +++ b/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 - let use_modem_message = fun msg -> - Debug.call 'M' (fun f -> fprintf f "use_modem: %s\n" (Debug.xprint msg)); - match Modem.parse msg with - None -> () (* Only internal modem data *) - | Some data -> - let b = !buffer ^ data in - Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b)); - let x = PprzTransport.parse use_pprz_buf b in - buffer := String.sub b x (String.length b - x) + (** Callback for a modem message *) + let use_modem_message = + let buffer = ref "" in + fun msg -> + Debug.call 'M' (fun f -> fprintf f "use_modem: %s\n" (Debug.xprint msg)); + match Modem.parse msg with + None -> () (* Only internal modem data *) + | Some data -> + (** Accumulate in a buffer *) + 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 *) diff --git a/sw/lib/ocaml/serial.ml b/sw/lib/ocaml/serial.ml index 679b2e07d0..ab1953c419 100644 --- a/sw/lib/ocaml/serial.ml +++ b/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 diff --git a/sw/lib/ocaml/serial.mli b/sw/lib/ocaml/serial.mli index 93e0f6a9f2..6dba9f4216 100644 --- a/sw/lib/ocaml/serial.mli +++ b/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. *)