Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

A/C id option removed: it's a connection for several A/Cs

Browse Source
This commit is contained in:
Pascal Brisset
2006-04-03 21:36:03 +00:00
parent d18d21b7a7
commit 126907b31d
1 changed files with 50 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/tmtc/maxstream.ml
+50 -59
View File
@@ -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));