diff --git a/sw/ground_segment/tmtc/maxstream.ml b/sw/ground_segment/tmtc/maxstream.ml index 1933e79500..c79857f9c1 100644 --- a/sw/ground_segment/tmtc/maxstream.ml +++ b/sw/ground_segment/tmtc/maxstream.ml @@ -29,6 +29,7 @@ module W = Wavecard module Tm_Pprz = Pprz.Protocol(struct let name = "telemetry_ap" end) module Ground_Pprz = Pprz.Protocol(struct let name = "ground" end) module Dl_Pprz = Pprz.Protocol(struct let name = "datalink" end) +module PprzTransport = Serial.Transport(Tm_Pprz) let ground_id = 0 @@ -67,13 +68,25 @@ let maxstream_send = fun fd data -> Debug.call 'm' (fun f -> fprintf f "mm sending: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); flush o +let use_tele_message = fun buf -> + Debug.call 'm' (fun f -> fprintf f "mm receiving: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); + let (msg_id, ac_id, values) = Tm_Pprz.values_of_bin buf in + let msg = Tm_Pprz.message_of_id msg_id in + Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values + +let buffer = ref "" + let maxstream_parse = fun buf f -> - 10 + let b = !buffer ^ buf in + let nb_used = PprzTransport.parse use_tele_message b in + buffer := String.sub b nb_used (String.length b - nb_used); + String.length buf let maxstream_receive = fun f -> Serial.input (fun b -> maxstream_parse b f) + let send = fun ac s -> (* Wavecard.send_addressed ac.fd (W.REQ_SEND_MESSAGE,ac.addr,s) *) maxstream_send ac.fd s