diff --git a/sw/ground_segment/tmtc/wavecard_connect.ml b/sw/ground_segment/tmtc/wavecard_connect.ml index 7b6c989a69..3477e10303 100644 --- a/sw/ground_segment/tmtc/wavecard_connect.ml +++ b/sw/ground_segment/tmtc/wavecard_connect.ml @@ -26,6 +26,7 @@ open Printf 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) @@ -45,15 +46,22 @@ let send_dl_msg = fun ac a -> -let broadcast_msg = fun (com, data) -> +let broadcast_msg = fun ac (com, data) -> match com with W.RECEIVED_FRAME -> Ivy.send (sprintf "FROM_WAVECARD %s" data) + | W.RES_READ_REMOTE_RSSI -> + Tm_Pprz.message_send (string_of_int ac.id) "WC_RSSI" ["raw_level", Pprz.Int (Char.code data.[0])] | _ -> Debug.call 'w' (fun f -> fprintf f "wv receiving: %x " (W.code_of_cmd com); for i = 0 to String.length data - 1 do fprintf f "%x " (Char.code data.[i]) done; fprintf f "\n"); Ivy.send (sprintf "RAW_FROM_WAVECARD %2x %s" (W.code_of_cmd com) data) +let rssi_period = 5000 (** ms *) +let req_rssi = fun ac -> + Wavecard.send_addressed ac.fd (W.REQ_READ_REMOTE_RSSI, ac.addr,"") + + let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f)) @@ -126,24 +134,27 @@ let _ = try let fd = Serial.opendev !port Serial.B9600 in - (* Listening *) - let listener = Wavecard.receive ~ack:(send_ack 10 fd) broadcast_msg in + let ac = { fd=fd; id= !id; addr= W.addr_of_string !distant_addr } in + (* Listening on wavecard *) + let listener = Wavecard.receive ~ack:(send_ack 10 fd) (broadcast_msg ac) in let cb = fun _ -> listener fd; true in ignore (Glib.Io.add_watch [`IN] cb (GMain.Io.channel_of_descr fd)); - let ac = { fd=fd; id= !id; addr= W.addr_of_string !distant_addr } in - (* Sending request from Ivy *) - (* For debug *) - ignore (Ivy.bind (fun _ a -> send_dl_msg ac a.(0)) "TO_WAVECARD +(.*)"); + (** Listening on Ivy *) ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp ac)); ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" (move_wp ac)); ignore (Ground_Pprz.message_bind "SEND_EVENT" (send_event ac)); + (* For debug *) + ignore (Ivy.bind (fun _ a -> send_dl_msg ac a.(0)) "TO_WAVECARD +(.*)"); + + + ignore (GMain.Timeout.add rssi_period (fun _ -> req_rssi ac; true)); (* Main Loop *) let loop = Glib.Main.create true in