Thon2 follows Thon1 !

conf/airframes/twinstar2.xml

@@ -55,7 +55,7 @@
     <define name="LOW_BATTERY" value="93" unit="1e-1V"/>
   </section>
   <section name="MISC">
-    <define name="NOMINAL_AIRSPEED" value="10." unit="m/s"/>
+    <define name="NOMINAL_AIRSPEED" value="12." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
   </section>
 </airframe>

conf/flight_plans/muret_follow.xml

@@ -23,7 +23,7 @@
     </block>
 
     <block NAME="follow">
-      <follow ac_id="2" distance="200"/>
+      <follow ac_id="2" distance="50"/>
     </block>
   </blocks>
 </flight_plan>

sw/airborne/autopilot/nav.c

@@ -115,7 +115,7 @@ void Follow(uint8_t _ac_id, float distance);
 
 void
 Follow(uint8_t _ac_id, float distance) { 
-  struct ac_info_ * ac = get_the_other();
+  struct ac_info_ * ac = get_the_other(_ac_id);
   vertical_mode = VERTICAL_MODE_AUTO_ALT;
   desired_altitude = ac->alt;
   float alpha = M_PI/2 - RadOfDeg(ac->heading);

sw/airborne/autopilot/traffic_info.c

@@ -1,18 +1,25 @@
 /* Informations relative to the other aircrafts */
 
+#include <inttypes.h>
 #include "flight_plan.h"
 
+#define NB_OTHERS 4
+
 struct ac_info_ {float east, north, heading, alt;};
 
-struct ac_info_ the_other;
+struct ac_info_ the_others[NB_OTHERS];
 
-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;
+void
+set_the_other(uint8_t id, float utm_x, float utm_y, float heading, float alt) {
+  if (id < NB_OTHERS) {
+    the_others[id].east = utm_x -  NAV_UTM_EAST0;
+    the_others[id].north = utm_y - NAV_UTM_NORTH0;
+    the_others[id].heading = heading;
+    the_others[id].alt = alt;
+  }
 }
 
-struct ac_info_ * get_the_other(void) {
-  return &the_other;
+struct ac_info_ * get_the_other(uint8_t id) {
+  id = (id < NB_OTHERS ? id : NB_OTHERS - 1);
+  return &the_others[id];
 }

sw/airborne/autopilot/traffic_info.h

@@ -3,7 +3,10 @@
 
 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);
+void 
+set_the_other(uint8_t id, float utm_x, float utm_y, float heading, float alt);
+
+struct ac_info_ *
+get_the_other(uint8_t id);
 
 #endif

sw/simulator/Makefile

@@ -117,7 +117,7 @@ $(OBJDIR)/simsitl.ml : simsitl.ml
 	$(OCAMLC) $(INCLUDES) -c $<
 
 clean :
-	\rm -f *.cm* *~ *.out .depend *.o
+	\rm -f *.cm* *~ *.out .depend *.o *.a *.so
 
 flightModel.cmo: flightModel.cmi 
 hitl.cmo: hitl.cmi 

sw/simulator/sim_ap.c

@@ -94,6 +94,7 @@ value set_servos(value servos) {
 }
 
 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));
+sim_set_the_other(value id, value east, value north, value heading, value alt) {
+  set_the_other(Int_val(id), Double_val(east), Double_val(north), Double_val(heading), Double_val(alt));
+  return Val_unit;
 }

sw/simulator/sitl.ml

@@ -116,11 +116,11 @@ 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"
+   external set_the_other : int -> 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
+      set_the_other ac_id east north heading alt
 
   let boot = fun () ->
     periodic servos_period update_servos;