Towards A/C following

conf/conf.xml

@@ -3,6 +3,7 @@
 <conf>
   <aircraft 
     name="Morue1"
+    ac_id="0"
     airframe="airframes/microjet1.xml" 
     radio="radios/mc3030.xml" 
     flight_plan="flight_plans/muret3.xml"
@@ -10,6 +11,7 @@
 
   <aircraft 
     name="Morue2"
+    ac_id="1"
     airframe="airframes/microjet2.xml" 
     radio="radios/mc3030.xml" 
     flight_plan="flight_plans/muret2.xml"
@@ -17,6 +19,7 @@
 
   <aircraft 
     name="Thon1"
+    ac_id="2"
     airframe="airframes/twinstar1.xml" 
     radio="radios/cockpitMM.xml" 
     flight_plan="flight_plans/muret_for.xml"
@@ -24,9 +27,10 @@
 
   <aircraft 
     name="Thon2"
+    ac_id="3"
     airframe="airframes/twinstar2.xml" 
     radio="radios/mc3030.xml" 
-    flight_plan="flight_plans/muret1.xml"
+    flight_plan="flight_plans/muret_follow.xml"
   />
   
   <map

conf/flight_plans/muret_follow.xml

@@ -0,0 +1,30 @@
+<!-- <!DOCTYPE flight_plan SYSTEM "flight_plan.dtd"> -->
+
+<flight_plan NAME="example - Muret for loops" LON0="1.27289" MAX_DIST_FROM_HOME="1000" GROUND_ALT="185" SECURITY_HEIGHT="25" QFU="270" ALT="250" LAT0="43.46223">
+  <rc_control>
+    <mode NAME="AUTO1">
+      <setting VAR="ir_pitch_neutral" RANGE="60." RC="gain_1_up" TYPE="int16"/>
+      <setting VAR="ir_roll_neutral" RANGE="-60." RC="gain_1_down" TYPE="int16"/>
+    </mode>
+    <mode NAME="AUTO2">
+      <setting VAR="course_pgain" RANGE="0.1" RC="gain_1_up" TYPE="float"/>
+      <setting VAR="pitch_of_roll" RANGE=".2" RC="gain_1_down" TYPE="float"/>
+    </mode>
+  </rc_control>
+  <waypoints utm_x0="360284.8" utm_y0="4813595.5">
+    <waypoint name="HOME" x="0.0" y="30.0" lat="43.4625000078" lon="1.27288231142" alt="250."/>
+     </waypoints>
+
+  <blocks>
+    <block NAME="init">
+      <while COND="(!launch)"/>
+      <heading VMODE="gaz" GAZ="0.8" PITCH="0.15" COURSE="QFU" UNTIL="(estimator_flight_time > 8)"/>
+      <heading CLIMB="3.0" VMODE="climb" PITCH="0.15" COURSE="QFU" UNTIL="(estimator_z > SECURITY_ALT)"/>
+    </block>
+
+    <block NAME="follow">
+      <follow ac_id="2" distance="200"/>
+    </block>
+  </blocks>
+</flight_plan>
+

conf/flight_plans/muret_for.xml

@@ -25,9 +25,9 @@
     <block NAME="for">
       <exception COND="(RcEvent1())" DEROUTE="for"/>
       <for var="i" from="0" to="3">
-        <go x="-200" y="$i*100"/>
+        <go hmode="route" x="-200" y="$i*100"/>
         <go x="-200" y="($i+3)*100"/>
-        <go x="200" y="($i+3)*100"/>
+        <go hmode="route" x="200" y="($i+3)*100"/>
         <go x="200" y="($i+1)*100"/>
       </for>
       <circle WP="HOME" alt="GROUND_ALT+50" radius="75"/>

conf/messages.xml

@@ -335,4 +335,15 @@ twinstar2 POSITION_UTM 10 10
   </message>
 </class>
 
+<class name="telecommand">
+  <message name="TRAFFIC_INFO" id="0x01">
+    <field name="ac_id"   type="int8"/>
+    <field name="east"    type="float" unit="m"/>
+    <field name="north"   type="float" unit="m"/>
+    <field name="speed"   type="float" unit="m/s"/>
+    <field name="heading" type="float" unit="deg"/>
+    <field name="alt"     type="float" unit="m"/>
+    <field name="climb"   type="float" unit="m/s"/>
+  </message>
+</class>
 </protocol>

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

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"
 
 

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;
+}

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

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 =

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));
 
@@ -217,12 +225,29 @@ let send_aircraft_msg = fun ac ->
   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 *)

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

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
 

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, int_of_string (Xml.attrib x "ac_id"))
-      | _x::xs -> loop (i+1) xs in
+      | _x::xs -> loop xs in
-    loop 0 (Xml.children conf_xml) in
+    loop (Xml.children conf_xml) in
   
   let airframe_file = user_conf_path // ExtXml.attrib aircraft_xml "airframe" in
 

sw/simulator/sim_ap.c

@@ -8,6 +8,7 @@
 #include "link_autopilot.h"
 #include "autopilot.h"
 #include "estimator.h"
+#include "traffic_info.h"
 
 #include <caml/mlvalues.h>
 #include <caml/memory.h>
@@ -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));
+}

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 *)

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 =