add datalink messages between AC in formation flight

conf/messages.xml

@@ -675,6 +675,20 @@
     <field name="ir_lp_last_out" type="float"/>
   </message>
 
+ <message name="FORMATION_SLOT_TM" id="82">
+	 <field name="ac_id" type="uint8"/>
+	 <field name="mode" type="uint8"/>
+	 <field name="slot_east" format="%.1f" type="float" unit="m"/>
+	 <field name="slot_north" format="%.1f" type="float" unit="m"/>
+	 <field name="slot_alt" format="%.1f" type="float" unit="m"/>
+ </message>
+
+ <message name="FORMATION_STATUS_TM" id="83">
+	 <field name="ac_id" type="uint8"/>
+	 <field name="leader_id" type="uint8"/>
+	 <field name="status" type="uint8"/>
+ </message>
+
 
   <message name="ENOSE_STATUS" ID="250">
       <field name="val1" type="uint16"/>
@@ -785,6 +799,21 @@
  <message name="PING" id="8">
  </message>
 
+ <message name="FORMATION_SLOT" id="9">
+	 <field name="ac_id" type="uint8"/>
+	 <field name="mode" type="uint8"/>
+	 <field name="slot_east" format="%.1f" type="float" unit="m"/>
+	 <field name="slot_north" format="%.1f" type="float" unit="m"/>
+	 <field name="slot_alt" format="%.1f" type="float" unit="m"/>
+ </message>
+
+ <message name="FORMATION_STATUS" id="10">
+	 <field name="ac_id" type="uint8"/>
+	 <field name="leader_id" type="uint8"/>
+	 <field name="status" type="uint8"/>
+ </message>
+
+
  <message name="SET_ACTUATOR" id="100">
    <field name="value" type="uint16"/>
    <field name="no" type="uint8"/>

sw/airborne/datalink.c

@@ -35,6 +35,10 @@
 #include "traffic_info.h"
 #endif // TRAFFIC_INFO
 
+#ifdef FORMATION
+#include "formation.h"
+#endif // FORMATION
+
 #include "common_nav.h"
 #include "settings.h"
 #include "latlong.h"
@@ -127,5 +131,22 @@ void dl_parse_msg(void) {
     DOWNLINK_SEND_DL_VALUE(&i, &val);
   } else
 #endif /** Else there is no dl_settings section in the flight plan */
-    { /* Last else */ }
+#ifdef FORMATION
+	if (msg_id == DL_FORMATION_SLOT) {
+    uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer);
+    uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer);
+    float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer);
+    float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer);
+    float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer);
+    UpdateSlot(ac_id, slot_east, slot_north, slot_alt);
+    if (ac_id == leader_id) form_mode = mode;
+  } else if (msg_id == DL_FORMATION_STATUS) {
+    uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer);
+    uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer);
+    uint8_t status = DL_FORMATION_STATUS_status(dl_buffer);
+    if (leader == leader_id) formation[ac_id].status = status;
+    else formation[ac_id].status = UNSET;
+  } else
+#endif
+  { /* Last else */ }
 }

sw/airborne/formation.c

@@ -6,6 +6,12 @@
 
 #include <math.h>
 
+#ifndef DOWNLINK_DEVICE
+#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
+#endif
+//#include "uart.h"
+#include "downlink.h"
+
 #include "formation.h"
 #include "estimator.h"
 #include "fw_h_ctl.h"
@@ -14,6 +20,7 @@
 #include "gps.h"
 #include "flight_plan.h"
 #include "airframe.h"
+#include "dl_protocol.h"
 
 #include <stdio.h>
 
@@ -28,6 +35,7 @@ float coef_form_course;
 float coef_form_alt;
 int form_mode;
 uint8_t leader_id;
+float old_cruise, old_alt;
 
 #ifndef FORM_CARROT
 #define FORM_CARROT 2.
@@ -69,10 +77,13 @@ int formation_init(void) {
   coef_form_alt = FORM_ALTITUDE_PGAIN;
   form_prox = FORM_PROX;
   form_mode = FORM_MODE;
+  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
+  old_alt = GROUND_ALT + SECURITY_HEIGHT;
   return FALSE;
 }
 
 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) {
+  DOWNLINK_SEND_FORMATION_SLOT_TM(&_id, &form_mode, &slot_e, &slot_n, &slot_a);
   formation[_id].status = IDLE;
   formation[_id].east = slot_e;
   formation[_id].north = slot_n;
@@ -82,23 +93,36 @@ int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) {
 
 int start_formation(void) {
   int i;
+  uint8_t ac_id = AC_ID;
   for (i = 0; i < NB_ACS; ++i) {
     if (formation[i].status == IDLE) formation[i].status = ACTIVE;
   }
+  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&formation[AC_ID].status);
+  // store current cruise and alt
+  old_cruise = v_ctl_auto_throttle_cruise_throttle;
+  old_alt = nav_altitude;
   return FALSE;
 }
 
 int stop_formation(void) {
   int i;
+  uint8_t ac_id = AC_ID;
   for (i = 0; i < NB_ACS; ++i) {
     if (formation[i].status == ACTIVE) formation[i].status = IDLE;
   }
+  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&formation[AC_ID].status);
+  // restore cruise and alt
+  v_ctl_auto_throttle_cruise_throttle = old_cruise;
+  old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
+  nav_altitude = old_alt;
+  old_alt = GROUND_ALT + SECURITY_HEIGHT;
   return FALSE;
 }
 
 
 int formation_flight(void) {
 
+  static uint8_t _1Hz   = 0;
   int nb = 0, i;
   float ch = cos(estimator_hspeed_dir);
   float sh = sin(estimator_hspeed_dir);
@@ -109,17 +133,29 @@ int formation_flight(void) {
   form_speed_n = estimator_hspeed_mod * ch;
   form_speed_e = estimator_hspeed_mod * sh;
 
+  // broadcast info
+  uint8_t ac_id = AC_ID;
+  DOWNLINK_SEND_FORMATION_STATUS_TM(&ac_id,&leader_id,&formation[AC_ID].status);
+  if (++_1Hz>=4) {
+    _1Hz=0;
+    DOWNLINK_SEND_FORMATION_SLOT_TM(&ac_id, &form_mode,
+        &formation[AC_ID].east,
+        &formation[AC_ID].north,
+        &formation[AC_ID].alt);
+  }
+
   // set info for this AC
   SetAcInfo(AC_ID, estimator_x, estimator_y, estimator_hspeed_dir, estimator_z, estimator_hspeed_mod, gps_itow);
-  if (formation[AC_ID].status != ACTIVE) return TRUE; // AC not ready
+  if (formation[AC_ID].status != ACTIVE) return FALSE; // AC not ready
 
   // get leader info
   struct ac_info_ * leader = get_ac_info(leader_id);
-  if (formation[leader_id].status == UNSET) return TRUE; // leader not ready
-  else if (formation[leader_id].status == IDLE) {
-      if(Max(((int)gps_itow - (int)leader->itow) / 1000., 0.) > FORM_CARROT) return TRUE; // still not ready
-      else formation[leader_id].status = ACTIVE;
-  }
+  if (formation[leader_id].status != ACTIVE) return FALSE; // leader not ready
+  //if (formation[leader_id].status == UNSET) return FALSE; // leader not ready
+  //else if (formation[leader_id].status == IDLE) {
+  //    if(Max(((int)gps_itow - (int)leader->itow) / 1000., 0.) > FORM_CARROT) return TRUE; // still not ready
+  //    else formation[leader_id].status = ACTIVE;
+  //}
 
   // compute slots in the right reference frame
   struct slot_ form[NB_ACS];
@@ -189,6 +225,9 @@ int formation_flight(void) {
     h_ctl_course_pgain = -coef_form_course;
     // fly to desired
     fly_to_xy(desired_x, desired_y);
+    desired_x = leader->east  + dx;
+    desired_y = leader->north + dy;
+    //fly_to_xy(desired_x, desired_y);
     // lateral correction
     //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy));
     //float diff_course = leader->course - estimator_hspeed_dir;

sw/airborne/formation.h

@@ -10,6 +10,9 @@
 #include "nav.h"
 #include "traffic_info.h"
 
+#define FORM_MODE_GLOBAL 1
+#define FORM_MODE_COURSE 2
+
 extern float coef_form_alt,coef_form_pos,coef_form_speed,coef_form_course;
 extern float form_prox;
 extern int form_mode;
@@ -30,6 +33,12 @@ int formation_init(void);
 
 int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a);
 
+#define UpdateSlot(_id, _se, _sn, _sa) { \
+  formation[_id].east = _se; \
+  formation[_id].north = _sn; \
+  formation[_id].alt = _sa; \
+}
+
 int start_formation(void);
 
 int stop_formation(void);

sw/airborne/sim/sim_ap.c

@@ -24,6 +24,7 @@
 #include "ap_downlink.h"
 #include "sim_uart.h"
 #include "latlong.h"
+#include "formation.h"
 
 #include <caml/mlvalues.h>
 #include <caml/memory.h>
@@ -157,3 +158,19 @@ value set_wind(value east, value north) {
   datalink_time = 0;
   return Val_unit;
 }
+
+value set_formation_slot(value ac_id, value mode, value se, value sn, value sa) {
+  uint8_t id = Int_val(ac_id);
+  UpdateSlot(id, Double_val(se), Double_val(sn), Double_val(sa));
+  if (id == Int_val(leader_id)) form_mode = Int_val(mode);
+  datalink_time = 0;
+  return Val_unit;
+}
+
+value set_formation_status(value ac_id, value leader, value status) {
+  if (Int_val(leader) == leader_id) formation[Int_val(ac_id)].status = Int_val(status);
+  else formation[Int_val(ac_id)].status = UNSET;
+  datalink_time = 0;
+  return Val_unit;
+}
+

sw/ground_segment/tmtc/link.ml

@@ -320,16 +320,16 @@ let get_fp = fun device _sender vs ->
 	  and course = f "course"
 	  and alt = f "alt"
 	  and gspeed = f "speed"
-    and itow = i32 "itow" in
+          and itow = i32 "itow" in
 	  let utm = Latlong.utm_of WGS84 {posn_lat=lat; posn_long=long} in
 	  let vs = ["ac_id", Pprz.Int ac_id;
 		    "utm_east", cm_of_m utm.utm_x;
 		    "utm_north", cm_of_m utm.utm_y;
 		    "course", Pprz.Int (truncate (10. *. course));
 		    "alt", cm_of_m alt;
-        "speed", cm_of_m gspeed;
-        "itow", Pprz.Int32 itow] in
-	  let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in
+                    "speed", cm_of_m gspeed;
+                    "itow", Pprz.Int32 itow] in
+          let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in
 	  let s = Dl_Pprz.payload_of_values msg_id my_id vs in
 	  send dest_id device ac_device s Low
 	with
@@ -385,6 +385,30 @@ let forward_uplink = fun device ->
   set_forwarder "BLOCK";
   set_forwarder "WIND_INFO"
 
+let broadcast_uplink = fun device ->
+  let broadcast = fun name sender vs ->
+    Debug.call 'f' (fun f -> fprintf f "broadcast %s\n" name);
+    let ac_id = Pprz.int_assoc "ac_id" vs in
+    List.iter 
+    (fun (dest_id, _) ->
+      if dest_id <> ac_id then (** Do not send to itself *)
+        try
+          Debug.trace 'b' (sprintf "Broadcast %d for %d" ac_id dest_id);
+          let ac_device = airborne_device dest_id airframes device.transport in
+          let msg_id, _ = Dl_Pprz.message_of_name name in
+          let s = Dl_Pprz.payload_of_values msg_id my_id vs in
+          send dest_id device ac_device s Low
+        with
+        _NotSendingToThis -> ())
+    airframes in
+
+  let set_broadcast = fun name ->
+    ignore (Dl_Pprz.message_bind name (broadcast name)) in
+
+  set_broadcast "FORMATION_SLOT";
+  set_broadcast "FORMATION_STATUS"
+
+
 let send_ping_msg = fun device ->
   Hashtbl.iter
     (fun ac_id status ->

sw/ground_segment/tmtc/server.ml

@@ -368,6 +368,10 @@ let log_and_parse = fun logging ac_name (a:Aircraft.aircraft) msg values ->
 	    update_waypoint a (ivalue "wp_id") p (fvalue "alt")
 	| None -> () (** Can't use this message  *)
       end
+  | "FORMATION_SLOT_TM" ->
+      Dl_Pprz.message_send "ground_dl" "FORMATION_SLOT" values
+  | "FORMATION_STATUS_TM" ->
+      Dl_Pprz.message_send "ground_dl" "FORMATION_STATUS" values
   | _ -> ()
 
 (** Callback for a message from a registered A/C *)

sw/simulator/sitl.ml

@@ -195,6 +195,24 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
       | x::_ -> fprintf stderr "Sim: Warning, ingoring RAW_DATALINK '%s' message" x
       | [] -> ()
 
+  external set_formation_slot : int -> int -> float -> float -> float -> unit = "set_formation_slot"
+  let get_formation_slot = fun _sender vs ->
+    let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+    if ac_id <> !my_id then
+      let f = fun a -> Pprz.float_assoc a vs in
+      let mode = Pprz.int_assoc "mode" vs
+      and se = f "slot_east"
+      and sn = f "slot_north"
+      and sa = f "slot_alt" in
+      set_formation_slot ac_id mode se sn sa
+
+  external set_formation_status : int -> int -> int -> unit = "set_formation_status"
+  let get_formation_status = fun _sender vs ->
+    let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in
+    if ac_id <> !my_id then
+      let leader = Pprz.int_assoc "leader_id" vs
+      and status = Pprz.int_assoc "status" vs in
+      set_formation_status ac_id leader status
 
 
   let boot = fun time_scale ->
@@ -204,7 +222,9 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct
     ignore (Dl_Pprz.message_bind "MOVE_WP" get_move_wp);
     ignore (Dl_Pprz.message_bind "BLOCK" get_block);
     ignore (Dl_Pprz.message_bind "SETTING" get_setting);
-    ignore (Ground_Pprz.message_bind "RAW_DATALINK" get_raw_datalink)
+    ignore (Ground_Pprz.message_bind "RAW_DATALINK" get_raw_datalink);
+    ignore (Dl_Pprz.message_bind "FORMATION_SLOT" get_formation_slot);
+    ignore (Dl_Pprz.message_bind "FORMATION_STATUS" get_formation_status)
 
 (* Functions called by the simulator *)
   let commands = fun s -> rcommands := s