diff --git a/conf/conf.xml b/conf/conf.xml index 5f14098aa0..5911bd527e 100644 --- a/conf/conf.xml +++ b/conf/conf.xml @@ -3,6 +3,7 @@ --> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/flight_plans/muret_for.xml b/conf/flight_plans/muret_for.xml index 4e5b2f5b38..da7432ee3b 100644 --- a/conf/flight_plans/muret_for.xml +++ b/conf/flight_plans/muret_for.xml @@ -25,9 +25,9 @@ - + - + diff --git a/conf/messages.xml b/conf/messages.xml index e6749e3f23..296f760a29 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -335,4 +335,15 @@ twinstar2 POSITION_UTM 10 10 + + + + + + + + + + + diff --git a/sw/airborne/autopilot/Makefile b/sw/airborne/autopilot/Makefile index 44940293d8..93f54f21fa 100644 --- a/sw/airborne/autopilot/Makefile +++ b/sw/airborne/autopilot/Makefile @@ -65,6 +65,7 @@ $(TARGET).srcs = \ uart.c \ estimator.c \ if_calib.c \ + traffic_info.c \ mainloop.c include ../../../conf/Makefile.local diff --git a/sw/airborne/autopilot/nav.c b/sw/airborne/autopilot/nav.c index 3c0227bb55..894324934e 100644 --- a/sw/airborne/autopilot/nav.c +++ b/sw/airborne/autopilot/nav.c @@ -32,6 +32,7 @@ #include "autopilot.h" #include "link_fbw.h" #include "airframe.h" +#include "traffic_info.h" uint8_t nav_stage, nav_block; uint8_t excpt_stage; /*To save the current stage when an exception is raised */ @@ -109,6 +110,19 @@ static float carrot_x, carrot_y; #define Max(x,y) (x > y ? x : y) #define Qdr(x) (Min(x, 350) < qdr && qdr < x+10) +void Follow(uint8_t _ac_id, float distance); + + +void +Follow(uint8_t _ac_id, float distance) { + struct ac_info_ * ac = get_the_other(); + vertical_mode = VERTICAL_MODE_AUTO_ALT; + desired_altitude = ac->alt; + float alpha = M_PI/2 - RadOfDeg(ac->heading); + fly_to_xy(ac->east - distance*cos(alpha), ac->north - distance*sin(alpha)); +} + + #include "flight_plan.h" diff --git a/sw/airborne/autopilot/traffic_info.c b/sw/airborne/autopilot/traffic_info.c new file mode 100644 index 0000000000..1fd13baf6c --- /dev/null +++ b/sw/airborne/autopilot/traffic_info.c @@ -0,0 +1,18 @@ +/* Informations relative to the other aircrafts */ + +#include "flight_plan.h" + +struct ac_info_ {float east, north, heading, alt;}; + +struct ac_info_ the_other; + +void set_the_other(float utm_x, float utm_y, float heading, float alt) { + the_other.east = utm_x - NAV_UTM_EAST0; + the_other.north = utm_y - NAV_UTM_NORTH0; + the_other.heading = heading; + the_other.alt = alt; +} + +struct ac_info_ * get_the_other(void) { + return &the_other; +} diff --git a/sw/airborne/autopilot/traffic_info.h b/sw/airborne/autopilot/traffic_info.h new file mode 100644 index 0000000000..9e02c5197b --- /dev/null +++ b/sw/airborne/autopilot/traffic_info.h @@ -0,0 +1,9 @@ +#ifndef TI_H +#define TI_H + +struct ac_info_ {float east, north, heading, alt;}; + +void set_the_other(float utm_x, float utm_y, float heading, float alt); +struct ac_info_ * get_the_other(void); + +#endif diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index 1e9c8108be..3ff248eafd 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -115,11 +115,11 @@ let aircraft_pos_msg = fun track utm_x utm_y heading -> track#add_point en; track#move_icon en heading -let carrot_pos_msg = fun track x y -> +let carrot_pos_msg = fun track utm_x utm_y -> match !map_ref with None -> () | Some utm0 -> - let en = {G.east = x; north = y } in + let en = {G.east = utm_x -. utm0.utm_x; north = utm_y -. utm0.utm_y } in track#move_carrot en let new_color = diff --git a/sw/ground_segment/tmtc/receive.ml b/sw/ground_segment/tmtc/receive.ml index 5c61dbc27c..28b646173b 100644 --- a/sw/ground_segment/tmtc/receive.ml +++ b/sw/ground_segment/tmtc/receive.ml @@ -29,8 +29,10 @@ module U = Unix module ModemTransport = Serial.Transport(Modem.Protocol) module Tele_Class = struct let name = "telemetry_ap" end +module Tc_Class = struct let name = "telecommand" end module AcInfo = struct let name = "aircraft_info" end module Tele_Pprz = Pprz.Protocol(Tele_Class) +module Tc_Pprz = Pprz.Protocol(Tc_Class) module AcInfo_Pprz = Pprz.Protocol(AcInfo) module PprzTransport = Serial.Transport(Tele_Pprz) @@ -78,6 +80,8 @@ type aircraft = { mutable pitch : float; mutable east : float; mutable north : float; + mutable nav_ref_east : float; + mutable nav_ref_north : float; mutable desired_east : float; mutable desired_north : float; mutable gspeed : float; @@ -106,6 +110,7 @@ let aircrafts = Hashtbl.create 3 (** Broadcast of the received aircrafts *) let aircrafts_msg_period = 5000 (* ms *) let aircraft_msg_period = 1000 (* ms *) +let traffic_info_period = 2000 (* ms *) let send_aircrafts_msg = fun () -> let t = U.gettimeofday () in let names = String.concat "," (Hashtbl.fold (fun k v r -> k::r) aircrafts []) in @@ -155,6 +160,9 @@ let log_and_parse = fun log ac_name a msg values -> | "DESIRED" -> a.desired_east <- fvalue "desired_x"; a.desired_north <- fvalue "desired_y" + | "NAVIGATION_REF" -> + a.nav_ref_east <- fvalue "utm_east"; + a.nav_ref_north <- fvalue "utm_north" | "ATTITUDE" -> a.roll <- fvalue "phi"; a.pitch <- fvalue "theta" @@ -203,7 +211,7 @@ let send_aircraft_msg = fun ac -> let _, fp_msg = AcInfo_Pprz.message_of_name "FLIGHT_PARAM" in Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message fp_msg values)); - let values = ["cur_block", Pprz.Int a.cur_block;"cur_stage", Pprz.Int a.cur_stage; "target_east", f a.desired_east; "target_north", f a.desired_north] + let values = ["cur_block", Pprz.Int a.cur_block;"cur_stage", Pprz.Int a.cur_stage; "target_east", f (a.nav_ref_east+.a.desired_east); "target_north", f (a.nav_ref_north+.a.desired_north)] and _, ns_msg = AcInfo_Pprz.message_of_name "NAV_STATUS" in Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message ns_msg values)); @@ -216,13 +224,30 @@ let send_aircraft_msg = fun ac -> Ivy.send (sprintf "%s %s" ac (AcInfo_Pprz.string_of_message as_msg values)) with Not_found -> prerr_endline ac + +let send_traffic_info = fun ac -> + (* TODO: should send up on the datalink *) + (* Sending on the Ivy bus for the simulators *) + let a = Hashtbl.find aircrafts ac in + let f = fun x -> Pprz.Float x in + let conf = ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "name" = ac) in + let values = ["ac_id", Pprz.Int (int_of_string (ExtXml.attrib conf "ac_id")); + "east", f a.east; + "north", f a.north; + "speed", f a.gspeed; + "heading", f (Geometry_2d.rad2deg a.course); + "alt", f a.alt; + "climb", f a.climb] in + let _, fp_msg = Tc_Pprz.message_of_name "TRAFFIC_INFO" in + Ivy.send (sprintf "%s %s" ac (Tc_Pprz.string_of_message fp_msg values)) let new_aircraft = fun id -> - { port = id ; roll = 0.; pitch = 0.; east = 0.; north = 0.; desired_east = 0.; desired_north = 0.; gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode=0; ap_altitude=0; if_calib_mode=0; mcu1_status=0; lls_calib=0 } + { port = id ; roll = 0.; pitch = 0.; east = 0.; north = 0.; nav_ref_east = 0.; nav_ref_north = 0.; desired_east = 0.; desired_north = 0.; gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode=0; ap_altitude=0; if_calib_mode=0; mcu1_status=0; lls_calib=0 } let register_aircraft = fun name a -> Hashtbl.add aircrafts name a; - ignore (Glib.Timeout.add aircraft_msg_period (fun () -> send_aircraft_msg name; true)) + ignore (Glib.Timeout.add aircraft_msg_period (fun () -> send_aircraft_msg name; true)); + ignore (Glib.Timeout.add traffic_info_period (fun () -> send_traffic_info name; true)) (** Callback of an identifying message from a soft simulator *) diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml index beb3d65fa2..238af0cfa7 100644 --- a/sw/lib/ocaml/mapTrack.ml +++ b/sw/lib/ocaml/mapTrack.ml @@ -48,7 +48,7 @@ class track = fun ?(name="coucou") ?(size = 50) ?(color="red") (geomap:MapCanvas GnoCanvas.text group ~props:[`TEXT name; `X 25.; `Y 25.; `ANCHOR `SW; `FILL_COLOR color] in let carrot = GnoCanvas.group group in let _ac_carrot = - ignore (GnoCanvas.polygon ~points:[|0.;0.;-2.5;5.;2.5;5.|] ~props:[`WIDTH_UNITS 1.;`FILL_COLOR "orange"; `OUTLINE_COLOR "orange"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] carrot) in + ignore (GnoCanvas.polygon ~points:[|0.;0.;-5.;-10.;5.;-10.|] ~props:[`WIDTH_UNITS 1.;`FILL_COLOR "orange"; `OUTLINE_COLOR "orange"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] carrot) in object (self) val mutable segments = Array.create size empty diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 00331baa07..b7cd3d2d82 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -32,7 +32,7 @@ SIMHCMO=$(SIMHML:%.ml=%.cmo) SIMSML = stdlib.ml data.ml flightModel.ml gps.ml sitl.ml sim.ml SIMSCMO=$(SIMSML:%.ml=%.cmo) SIMSCMX=$(SIMSML:%.ml=%.cmx) -SIMSC = sim_ir.c sim_gps.c sim_ap.c estimator.c pid.c nav.c main.c +SIMSC = sim_ir.c sim_gps.c sim_ap.c estimator.c pid.c traffic_info.c nav.c main.c SIMSO=$(SIMSC:%.c=$(OBJDIR)/%.o) SIMSA=sims.cma diff --git a/sw/simulator/data.ml b/sw/simulator/data.ml index f90e8292b6..49970ae257 100644 --- a/sw/simulator/data.ml +++ b/sw/simulator/data.ml @@ -51,12 +51,12 @@ type aircraft = { let aircraft = fun name -> let aircraft_xml, id = - let rec loop i = function + let rec loop = function [] -> failwith ("Aicraft not found : "^name) | x::_ when Xml.tag x = "aircraft" && Xml.attrib x "name" = name -> - (x, i) - | _x::xs -> loop (i+1) xs in - loop 0 (Xml.children conf_xml) in + (x, int_of_string (Xml.attrib x "ac_id")) + | _x::xs -> loop xs in + loop (Xml.children conf_xml) in let airframe_file = user_conf_path // ExtXml.attrib aircraft_xml "airframe" in diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c index e7a175088d..93e0aed242 100644 --- a/sw/simulator/sim_ap.c +++ b/sw/simulator/sim_ap.c @@ -8,6 +8,7 @@ #include "link_autopilot.h" #include "autopilot.h" #include "estimator.h" +#include "traffic_info.h" #include #include @@ -91,3 +92,8 @@ value set_servos(value servos) { return Val_int(servo_widths[SERVO_GAZ]); } + +value +sim_set_the_other(value east, value north, value heading, value alt) { + set_the_other(Double_val(east), Double_val(north), Double_val(heading), Double_val(alt)); +} diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index 51f9959031..68ce0cdeed 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -26,6 +26,9 @@ open Printf +let ios = int_of_string +let fos = float_of_string + let ivy_bus = ref "127.255.255.255:2010" module Make(A:Data.MISSION) = struct @@ -113,11 +116,20 @@ module Make(A:Data.MISSION) = struct ignore (adj_bat#connect#value_changed update); update () + external set_the_other : float -> float -> float -> float -> unit = "sim_set_the_other" + + let traffic_info = fun ac_id east north heading alt -> + if ac_id <> A.ac.Data.id then (* Only ONE other A/C for the time being *) + set_the_other east north heading alt + let boot = fun () -> periodic servos_period update_servos; periodic periodic_period periodic_task; periodic rc_period rc_task; - periodic 10000 update_adj_bat + periodic 10000 update_adj_bat; + ignore (Ivy.bind + (fun _ a -> traffic_info (ios a.(0)) (fos a.(1)) (fos a.(2)) (fos a.(4)) (fos a.(5)) ) + "TRAFFIC_INFO +([^ ]+) +([^ ]+) +([^ ]+) +([^ ]+) +([^ ]+) +([^ ]+) +([^ ]+)") (* Functions called by the simulator *) diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 4c0e3ca79a..012a77eef9 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -194,7 +194,7 @@ let rec compile_stage = fun block x -> | "for" -> List.iter (compile_stage block) (Xml.children x); incr stage (* To count the loop stage *) - | "return_from_excpt" | "goto" | "deroute" | "exit_block" + | "return_from_excpt" | "goto" | "deroute" | "exit_block" | "follow" | "heading" | "go" | "stay" | "xyz" | "circle" -> () | s -> failwith (sprintf "Unknown stage: %s\n" s) end @@ -250,6 +250,12 @@ let rec print_stage = fun index_of_waypoints x -> ignore (output_vmode x "" ""); left (); lprintf "}\n"; lprintf "return;\n" + | "follow" -> + stage (); + let id = ExtXml.attrib x "ac_id" in + let d = ExtXml.attrib x "distance" in + lprintf "Follow(%s, %s);\n" id d; + lprintf "return;\n" | "go" -> stage (); let wp =