diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml
index 4c27817376..f979d5d023 100644
--- a/conf/airframes/twinstar2.xml
+++ b/conf/airframes/twinstar2.xml
@@ -55,7 +55,7 @@
diff --git a/conf/flight_plans/muret_follow.xml b/conf/flight_plans/muret_follow.xml
index 670c2790db..661c057e79 100644
--- a/conf/flight_plans/muret_follow.xml
+++ b/conf/flight_plans/muret_follow.xml
@@ -23,7 +23,7 @@
-
+
diff --git a/sw/airborne/autopilot/nav.c b/sw/airborne/autopilot/nav.c
index 894324934e..ce17cb6494 100644
--- a/sw/airborne/autopilot/nav.c
+++ b/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);
diff --git a/sw/airborne/autopilot/traffic_info.c b/sw/airborne/autopilot/traffic_info.c
index 1fd13baf6c..76830fc3b7 100644
--- a/sw/airborne/autopilot/traffic_info.c
+++ b/sw/airborne/autopilot/traffic_info.c
@@ -1,18 +1,25 @@
/* Informations relative to the other aircrafts */
+#include
#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];
}
diff --git a/sw/airborne/autopilot/traffic_info.h b/sw/airborne/autopilot/traffic_info.h
index 9e02c5197b..707706cf33 100644
--- a/sw/airborne/autopilot/traffic_info.h
+++ b/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
diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile
index b7cd3d2d82..42acb5dfd2 100644
--- a/sw/simulator/Makefile
+++ b/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
diff --git a/sw/simulator/sim_ap.c b/sw/simulator/sim_ap.c
index 93e0aed242..be6e852440 100644
--- a/sw/simulator/sim_ap.c
+++ b/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;
}
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 68ce0cdeed..9553d81eb0 100644
--- a/sw/simulator/sitl.ml
+++ b/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;