diff --git a/conf/messages.xml b/conf/messages.xml
index 04307f9269..84b6494a1c 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -675,6 +675,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -785,6 +799,21 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index d3c3f35e56..e3d599b66c 100644
--- a/sw/airborne/datalink.c
+++ b/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 */ }
}
diff --git a/sw/airborne/formation.c b/sw/airborne/formation.c
index 5cfa0105a9..bd904dddec 100644
--- a/sw/airborne/formation.c
+++ b/sw/airborne/formation.c
@@ -6,6 +6,12 @@
#include
+#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
@@ -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;
diff --git a/sw/airborne/formation.h b/sw/airborne/formation.h
index 17c7227796..0c6d5ba5c7 100644
--- a/sw/airborne/formation.h
+++ b/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);
diff --git a/sw/airborne/sim/sim_ap.c b/sw/airborne/sim/sim_ap.c
index d05f3b9dff..df8bd0857d 100644
--- a/sw/airborne/sim/sim_ap.c
+++ b/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
#include
@@ -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;
+}
+
diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml
index fe3d9451d1..e8dd4dfef6 100644
--- a/sw/ground_segment/tmtc/link.ml
+++ b/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 ->
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 7dcab58c23..a14b0a35a6 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/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 *)
diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml
index 0a263d4be5..09501efbe7 100644
--- a/sw/simulator/sitl.ml
+++ b/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