From 126907b31deff6849b49021de2edfea2858523c2 Mon Sep 17 00:00:00 2001 From: Pascal Brisset Date: Mon, 3 Apr 2006 21:36:03 +0000 Subject: [PATCH] A/C id option removed: it's a connection for several A/Cs --- sw/ground_segment/tmtc/maxstream.ml | 109 +++++++++++++--------------- 1 file changed, 50 insertions(+), 59 deletions(-) diff --git a/sw/ground_segment/tmtc/maxstream.ml b/sw/ground_segment/tmtc/maxstream.ml index 457e19c408..6b52d8039a 100644 --- a/sw/ground_segment/tmtc/maxstream.ml +++ b/sw/ground_segment/tmtc/maxstream.ml @@ -30,11 +30,10 @@ 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) -(** Hack: id of the sender is used in uplink to select the receiver *) -let ground_ac_id = ref 4 +let ground_id = 0 -type aircraft = { fd : Unix.file_descr; id : int; addr : W.addr } +type modem = { fd : Unix.file_descr; } (*let send_ack = fun delay fd () -> ignore (GMain.Timeout.add delay (fun _ -> W.send fd (W.ACK, ""); false)) *) @@ -93,7 +92,7 @@ let send = fun ac s -> let send_dl_msg = fun ac a -> let (id, values) = Dl_Pprz.values_of_string a in - let s = Dl_Pprz.payload_of_values id !ground_ac_id values in + let s = Dl_Pprz.payload_of_values id ground_id values in send ac s @@ -102,85 +101,77 @@ let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f)) (** Got a FLIGHT_PARAM message and dispatch a ACINFO *) let get_fp = fun ac _sender vs -> let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in - if ac_id <> ac.id then - let f = fun a -> Pprz.float_assoc a vs in - let ux = f "east" - and uy = f "north" - and course = f "course" - and alt = f "alt" - and gspeed = f "speed" in - let vs = ["ac_id", Pprz.Int ac_id; - "utm_east", cm_of_m ux; - "utm_north", cm_of_m uy; - "course", Pprz.Int (truncate (10. *. course)); - "alt", cm_of_m alt; - "speed", cm_of_m gspeed] in - let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in - let s = Dl_Pprz.payload_of_values msg_id !ground_ac_id vs in - send ac s + let f = fun a -> Pprz.float_assoc a vs in + let ux = f "east" + and uy = f "north" + and course = f "course" + and alt = f "alt" + and gspeed = f "speed" in + let vs = ["ac_id", Pprz.Int ac_id; + "utm_east", cm_of_m ux; + "utm_north", cm_of_m uy; + "course", Pprz.Int (truncate (10. *. course)); + "alt", cm_of_m alt; + "speed", cm_of_m gspeed] in + let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in + let s = Dl_Pprz.payload_of_values msg_id ac_id vs in + send ac s (** Got a MOVE_WAYPOINT and send a MOVE_WP *) let move_wp = fun ac _sender vs -> Debug.call 'm' (fun f -> fprintf f "mm MOVE WAYPOINT\n"); let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in - if ac_id = ac.id then - let f = fun a -> Pprz.float_assoc a vs in - let lat = f "lat" - and long = f "long" - and alt = f "alt" - and wp_id = Pprz.int_assoc "wp_id" vs in - let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in - let utm = Latlong.utm_of WGS84 wgs84 in - let vs = ["wp_id", Pprz.Int wp_id; - "utm_east", cm_of_m utm.utm_x; - "utm_north", cm_of_m utm.utm_y; - "alt", cm_of_m alt] in - let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in - let s = Dl_Pprz.payload_of_values msg_id !ground_ac_id vs in - send ac s + let f = fun a -> Pprz.float_assoc a vs in + let lat = f "lat" + and long = f "long" + and alt = f "alt" + and wp_id = Pprz.int_assoc "wp_id" vs in + let wgs84 = {posn_lat=(Deg>>Rad)lat;posn_long=(Deg>>Rad)long} in + let utm = Latlong.utm_of WGS84 wgs84 in + let vs = ["wp_id", Pprz.Int wp_id; + "utm_east", cm_of_m utm.utm_x; + "utm_north", cm_of_m utm.utm_y; + "alt", cm_of_m alt] in + let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in + let s = Dl_Pprz.payload_of_values msg_id ac_id vs in + send ac s (** Got a SEND_EVENT, and send an EVENT *) let send_event = fun ac _sender vs -> let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in - if ac_id = ac.id then - let ev_id = Pprz.int_assoc "event_id" vs in - let vs = ["event", Pprz.Int ev_id] in - let msg_id, _ = Dl_Pprz.message_of_name "EVENT" in - let s = Dl_Pprz.payload_of_values msg_id !ground_ac_id vs in - send ac s + let ev_id = Pprz.int_assoc "event_id" vs in + let vs = ["event", Pprz.Int ev_id] in + let msg_id, _ = Dl_Pprz.message_of_name "EVENT" in + let s = Dl_Pprz.payload_of_values msg_id ac_id vs in + send ac s (** Got a DL_SETTING, and send an SETTING *) let setting = fun ac _sender vs -> let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in - if ac_id = ac.id then - let idx = Pprz.int_assoc "index" vs in - let vs = ["event", Pprz.Int idx; "value", List.assoc "value" vs] in - let msg_id, _ = Dl_Pprz.message_of_name "SETTING" in - let s = Dl_Pprz.payload_of_values msg_id !ground_ac_id vs in - send ac s + let idx = Pprz.int_assoc "index" vs in + let vs = ["event", Pprz.Int idx; "value", List.assoc "value" vs] in + let msg_id, _ = Dl_Pprz.message_of_name "SETTING" in + let s = Dl_Pprz.payload_of_values msg_id ac_id vs in + send ac s (** Got a JUMP_TO_BLOCK, and send an BLOCK *) let jump_block = fun ac _sender vs -> Debug.call 'm' (fun f -> fprintf f "mm JUMP_TO_BLOCK\n"); let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in - if ac_id = ac.id then - let block_id = Pprz.int_assoc "block_id" vs in - let vs = ["block_id", Pprz.Int block_id] in - let msg_id, _ = Dl_Pprz.message_of_name "BLOCK" in - let s = Dl_Pprz.payload_of_values msg_id !ground_ac_id vs in - send ac s - + let block_id = Pprz.int_assoc "block_id" vs in + let vs = ["block_id", Pprz.Int block_id] in + let msg_id, _ = Dl_Pprz.message_of_name "BLOCK" in + let s = Dl_Pprz.payload_of_values msg_id ac_id vs in + send ac s + let _ = let ivy_bus = ref "127.255.255.255:2010" in let port = ref "/dev/ttyS0" in - let distant_addr = ref "0x011804c0012d" in let options = [ "-b", Arg.Set_string ivy_bus, (sprintf "Ivy bus (%s)" !ivy_bus); - "-id", Arg.Set_int ground_ac_id, (sprintf "A/C Id (%d)" !ground_ac_id); - "-a", Arg.Set_string distant_addr, (sprintf "Distant address (%s)" !distant_addr); "-d", Arg.Set_string port, (sprintf "Port (%s)" !port)] in Arg.parse options @@ -192,7 +183,7 @@ let _ = try let fd = Serial.opendev !port Serial.B9600 in - let ac = { fd=fd; id= !ground_ac_id; addr= W.addr_of_string !distant_addr } in + let ac = { fd=fd; } in (* Listening on wavecard *) let cb = fun _ -> maxstream_receive fd; @@ -204,7 +195,7 @@ let _ = (** Listening on Ivy *) -(* ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp ac)); *) +(** 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)); ignore (Ground_Pprz.message_bind "DL_SETTING" (setting ac));