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;