From 789437978dfa28c12bfcea13e1b616b976b3f66d Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 20 Mar 2018 16:07:16 +0100
Subject: [PATCH] [swarm] add formation control for rotorcraft (#2246)

* [swarm] add formation control for rotorcraft

Based on Ewoud Smeur (INDI) and Hector Garcia de Marina (Formation
control) work. See
https://blog.paparazziuav.org/2017/12/02/pilot-a-super-rotorcraft
- several UAV can be control from the ground (using a joystick for
  instance) by sending proper acceleration setpoint to the INDI guidance
  controller
- configuration of the formation is done from a JSON file (2 example
  files provided)

Some extra changes:
- update accel from IMU in GPS passthrough INS
- add accel setpoint to ABI messages
- by to accel setpoint in INDI guidance
- send ground reference from natnet2ivy
- possibility to have a joystick labeled 'GCS' with input2ivy
---
 conf/abi.xml                                  |   5 +
 conf/joystick/T1600M_ground.xml               |  43 +++
 conf/modules/fc_rotor.xml                     |  24 ++
 conf/modules/guidance_indi.xml                |   1 +
 conf/swarm/conf_2uav_rot.json                 |  52 +++
 conf/swarm/conf_3uav_rot.json                 |  53 +++
 .../rotorcraft/guidance/guidance_indi.c       | 125 +++++--
 .../rotorcraft/guidance/guidance_indi.h       |   1 +
 sw/airborne/modules/fc_rotor/fc_rotor.c       |  53 +++
 sw/airborne/modules/fc_rotor/fc_rotor.h       |  35 ++
 sw/airborne/subsystems/abi_sender_ids.h       |   7 +
 .../subsystems/ins/ins_gps_passthrough.c      |  40 ++-
 sw/ground_segment/joystick/input2ivy.ml       |  12 +-
 sw/ground_segment/python/fc_rotor/fc_rotor.py | 306 ++++++++++++++++++
 .../python/fc_rotor/lib_rigid_formations.py   | 122 +++++++
 .../python/natnet3.x/NatNetClient.py          |   2 +-
 .../python/natnet3.x/natnet2ivy.py            |  15 +-
 17 files changed, 859 insertions(+), 37 deletions(-)
 create mode 100644 conf/joystick/T1600M_ground.xml
 create mode 100644 conf/modules/fc_rotor.xml
 create mode 100644 conf/swarm/conf_2uav_rot.json
 create mode 100644 conf/swarm/conf_3uav_rot.json
 create mode 100644 sw/airborne/modules/fc_rotor/fc_rotor.c
 create mode 100644 sw/airborne/modules/fc_rotor/fc_rotor.h
 create mode 100755 sw/ground_segment/python/fc_rotor/fc_rotor.py
 create mode 100644 sw/ground_segment/python/fc_rotor/lib_rigid_formations.py

diff --git a/conf/abi.xml b/conf/abi.xml
index 269d937287..5c5eba22d9 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -151,6 +151,11 @@
       <field name="distance" type="float" unit="m">Distance of object to track</field>
     </message>
 
+    <message name="ACCEL_SP" id="23">
+      <field name="flag" type="uint8_t">0: 2D accel setpoint, 1: 3D accel setpoint</field>
+      <field name="accel_sp" type="struct FloatVect3 *" unit="m/s^2"/>
+    </message>
+
   </msg_class>
 
 </protocol>
diff --git a/conf/joystick/T1600M_ground.xml b/conf/joystick/T1600M_ground.xml
new file mode 100644
index 0000000000..47deec91fb
--- /dev/null
+++ b/conf/joystick/T1600M_ground.xml
@@ -0,0 +1,43 @@
+<!-- Thrusmaster T1600 M
+
+  4 axis, 1 hat, 16 buttons
+
+  Only send ground JOYSTICK messages (4 axis, 4 buttons)
+
+-->
+
+<joystick>
+  <input>
+    <axis index="0" name="roll"/>
+    <axis index="1" name="pitch"/>
+    <axis index="2" name="yaw"/>
+    <axis index="3" name="thrust"/>
+    <button index="0" name="fire"/>
+    <button index="1" name="top_center"/>
+    <button index="2" name="top_left"/>
+    <button index="3" name="top_right"/>
+    <button index="4" name="b4"/>
+    <button index="5" name="b5"/>
+    <button index="6" name="b6"/>
+    <button index="7" name="b7"/>
+    <button index="8" name="b8"/>
+    <button index="9" name="b9"/>
+  </input>
+
+  <messages period="0.05">
+
+    <message class="ground" name="JOYSTICK" send_always="true">
+      <field name="id"      value="JoystickID()"/>
+      <field name="axis1"   value="roll"/>
+      <field name="axis2"   value="pitch"/>
+      <field name="axis3"   value="yaw"/>
+      <field name="axis4"   value="Fit(-thrust,-127,127,0,127)"/>
+      <field name="button1" value="top_left"/>
+      <field name="button2" value="top_right"/>
+      <field name="button3" value="fire"/>
+      <field name="button4" value="top_center"/>
+    </message>
+
+  </messages>
+
+</joystick>
diff --git a/conf/modules/fc_rotor.xml b/conf/modules/fc_rotor.xml
new file mode 100644
index 0000000000..54036215bf
--- /dev/null
+++ b/conf/modules/fc_rotor.xml
@@ -0,0 +1,24 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="fc_rotor" dir="fc_rotor">
+  <doc>
+    <description>
+      Algorithm for the formation control of a team of rotorcrafts.
+      The vehicles are are running the INDI control and the desired acceleration are given by script from ground.
+    </description>
+  </doc>
+
+  <header>
+    <file name="fc_rotor.h"/>
+  </header>
+
+  <init fun = "fc_rotor_init()"/>
+
+  <datalink message="DESIRED_SETPOINT" fun="fc_read_msg()"/>
+
+  <makefile firmware="rotorcraft">
+    <define name="FC_ROTOR"/>
+    <file name="fc_rotor.c"/>
+  </makefile>
+
+</module>
diff --git a/conf/modules/guidance_indi.xml b/conf/modules/guidance_indi.xml
index 6030837813..29873f38c5 100644
--- a/conf/modules/guidance_indi.xml
+++ b/conf/modules/guidance_indi.xml
@@ -17,6 +17,7 @@
   <header>
     <file name="guidance_indi.h"/>
   </header>
+  <init fun="guidance_indi_init()"/>
   <init fun="guidance_indi_enter()"/>
   <makefile target="ap|nps" firmware="rotorcraft">
     <file name="guidance_indi.c" dir="$(SRC_FIRMWARE)/guidance"/>
diff --git a/conf/swarm/conf_2uav_rot.json b/conf/swarm/conf_2uav_rot.json
new file mode 100644
index 0000000000..d59b1e2e85
--- /dev/null
+++ b/conf/swarm/conf_2uav_rot.json
@@ -0,0 +1,52 @@
+{
+  "ids": [ 211, 212],
+  "topology": [
+    [-1],
+    [ 1]
+  ],
+  "desired_distances": [2.8],
+  "motion":
+  {
+   "t1": [
+     [1.0],
+     [1.0]
+   ],
+   "t2": [
+     [1.0],
+     [1.0]
+   ],
+   "r1": [
+     [ 1.0],
+     [-1.0]
+   ],
+   "r2": [
+     [ 0],
+     [ 0]
+   ]
+  },
+  "gains": [ 0.17, 4.8 ],
+  "geo_fence":
+  {
+    "x_min": -4,
+    "x_max": 4,
+    "y_min": -4,
+    "y_max": 4,
+    "z_min": -5,
+    "z_max": 2
+  },
+  "3D": false,
+  "sensitivity":
+  {
+    "t1": 1.0,
+    "t2": 1.0,
+    "r1": 2.0,
+    "r2": 1.0,
+    "scale": 0.05,
+    "scale_min": 0.3,
+    "scale_max": 2.0,
+    "alt": 0.1,
+    "alt_min": 0.3,
+    "alt_max": 3.0
+  }
+}
+
diff --git a/conf/swarm/conf_3uav_rot.json b/conf/swarm/conf_3uav_rot.json
new file mode 100644
index 0000000000..78b67a3b7c
--- /dev/null
+++ b/conf/swarm/conf_3uav_rot.json
@@ -0,0 +1,53 @@
+{
+  "ids": [ 210, 211, 212 ],
+  "topology": [
+    [ 1,  0, -1],
+    [-1,  1,  0],
+    [ 0, -1,  1]
+  ],
+  "desired_distances": [2.8, 2.8, 2.8],
+  "motion":
+  {
+   "t1": [
+     [ 1.0,  -0.5,  0.0],
+     [ 0.0,  -0.5,  1.0]
+   ],
+   "t2": [
+     [-0.5, -0.5,  1],
+     [-1,  0.5,  0.5]
+   ],
+   "r1": [
+     [ 1,  1,  1],
+     [ 1,  1,  1]
+   ],
+   "r2": [
+     [ 0,  0,  0],
+     [ 0,  0,  0]
+   ]
+  },
+  "gains": [ 0.27, 4.0 ],
+  "geo_fence":
+  {
+    "x_min": -4,
+    "x_max": 4,
+    "y_min": -4,
+    "y_max": 4,
+    "z_min": -5,
+    "z_max": 5
+  },
+  "3D": false,
+  "sensitivity":
+  {
+    "t1": 1.0,
+    "t2": 1.0,
+    "r1": 1.0,
+    "r2": 1.0,
+    "scale": 0.05,
+    "scale_min": 0.3,
+    "scale_max": 2.0,
+    "alt": 0.1,
+    "alt_min": 0.3,
+    "alt_max": 3.0
+  }
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
index 0fe2359a55..737a762b75 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -31,7 +31,6 @@
  * Gust Disturbance Alleviation with Incremental Nonlinear Dynamic Inversion
  * https://www.researchgate.net/publication/309212603_Gust_Disturbance_Alleviation_with_Incremental_Nonlinear_Dynamic_Inversion
  */
-
 #include "generated/airframe.h"
 #include "firmwares/rotorcraft/guidance/guidance_indi.h"
 #include "subsystems/ins/ins_int.h"
@@ -46,7 +45,6 @@
 #include "autopilot.h"
 #include "stabilization/stabilization_attitude_ref_quat_int.h"
 #include "firmwares/rotorcraft/stabilization.h"
-#include "stdio.h"
 #include "filters/low_pass_filter.h"
 #include "subsystems/abi.h"
 
@@ -66,7 +64,16 @@ float guidance_indi_speed_gain = GUIDANCE_INDI_SPEED_GAIN;
 float guidance_indi_speed_gain = 1.8;
 #endif
 
-struct FloatVect3 sp_accel = {0.0,0.0,0.0};
+#ifndef GUIDANCE_INDI_ACCEL_SP_ID
+#define GUIDANCE_INDI_ACCEL_SP_ID ABI_BROADCAST
+#endif
+abi_event accel_sp_ev;
+static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp);
+struct FloatVect3 indi_accel_sp = {0.0, 0.0, 0.0};
+bool indi_accel_sp_set_2d = false;
+bool indi_accel_sp_set_3d = false;
+
+struct FloatVect3 sp_accel = {0.0, 0.0, 0.0};
 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
 float thrust_in_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
 static void guidance_indi_filter_thrust(void);
@@ -101,23 +108,35 @@ struct FloatVect3 euler_cmd;
 
 float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
 
+float time_of_accel_sp_2d = 0.0;
+float time_of_accel_sp_3d = 0.0;
+
 struct FloatEulers guidance_euler_cmd;
 float thrust_in;
 
 static void guidance_indi_propagate_filters(void);
 static void guidance_indi_calcG(struct FloatMat33 *Gmat);
 
+/**
+ * @brief Init function
+ */
+void guidance_indi_init(void)
+{
+  AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);
+}
+
 /**
  *
  * Call upon entering indi guidance
  */
-void guidance_indi_enter(void) {
+void guidance_indi_enter(void)
+{
   thrust_in = stabilization_cmd[COMMAND_THRUST];
   thrust_act = thrust_in;
 
-  float tau = 1.0/(2.0*M_PI*filter_cutoff);
-  float sample_time = 1.0/PERIODIC_FREQUENCY;
-  for(int8_t i=0; i<3; i++) {
+  float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
+  float sample_time = 1.0 / PERIODIC_FREQUENCY;
+  for (int8_t i = 0; i < 3; i++) {
     init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
   }
   init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi);
@@ -131,7 +150,8 @@ void guidance_indi_enter(void) {
  *
  * main indi guidance function
  */
-void guidance_indi_run(bool in_flight, float heading_sp) {
+void guidance_indi_run(bool in_flight, float heading_sp)
+{
 
   //filter accel to get rid of noise and filter attitude to synchronize with accel
   guidance_indi_propagate_filters();
@@ -145,21 +165,43 @@ void guidance_indi_run(bool in_flight, float heading_sp) {
   float speed_sp_y = pos_y_err * guidance_indi_pos_gain;
   float speed_sp_z = pos_z_err * guidance_indi_pos_gain;
 
-  sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
-  sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
-  sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
+  // If the acceleration setpoint is set over ABI message
+  if (indi_accel_sp_set_2d) {
+    sp_accel.x = indi_accel_sp.x;
+    sp_accel.y = indi_accel_sp.y;
+    // In 2D the vertical motion is derived from the flight plan
+    sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
+    float dt = get_sys_time_float() - time_of_accel_sp_2d;
+    // If the input command is not updated after a timeout, switch back to flight plan control
+    if (dt > 0.5) {
+      indi_accel_sp_set_2d = false;
+    }
+  } else if (indi_accel_sp_set_3d) {
+    sp_accel.x = indi_accel_sp.x;
+    sp_accel.y = indi_accel_sp.y;
+    sp_accel.z = indi_accel_sp.z;
+    float dt = get_sys_time_float() - time_of_accel_sp_3d;
+    // If the input command is not updated after a timeout, switch back to flight plan control
+    if (dt > 0.5) {
+      indi_accel_sp_set_3d = false;
+    }
+  } else {
+    sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
+    sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
+    sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
+  }
 
 #if GUIDANCE_INDI_RC_DEBUG
 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
   //for rc control horizontal, rotate from body axes to NED
   float psi = stateGetNedToBodyEulers_f()->psi;
-  float rc_x = -(radio_control.values[RADIO_PITCH]/9600.0)*8.0;
-  float rc_y = (radio_control.values[RADIO_ROLL]/9600.0)*8.0;
+  float rc_x = -(radio_control.values[RADIO_PITCH] / 9600.0) * 8.0;
+  float rc_y = (radio_control.values[RADIO_ROLL] / 9600.0) * 8.0;
   sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
   sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
 
   //for rc vertical control
-  sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
+  sp_accel.z = -(radio_control.values[RADIO_THROTTLE] - 4500) * 8.0 / 9600.0;
 #endif
 
   //Calculate matrix of partial derivatives
@@ -167,7 +209,7 @@ void guidance_indi_run(bool in_flight, float heading_sp) {
   //Invert this matrix
   MAT33_INV(Ga_inv, Ga);
 
-  struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned[0].o[0], sp_accel.y -filt_accel_ned[1].o[0], sp_accel.z -filt_accel_ned[2].o[0]};
+  struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned[0].o[0], sp_accel.y - filt_accel_ned[1].o[0], sp_accel.z - filt_accel_ned[2].o[0]};
 
   //Bound the acceleration error so that the linearization still holds
   Bound(a_diff.x, -6.0, 6.0);
@@ -196,11 +238,11 @@ void guidance_indi_run(bool in_flight, float heading_sp) {
   guidance_indi_filter_thrust();
 
   //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
-  thrust_in = thrust_filt.o[0] + euler_cmd.z*thrust_in_specific_force_gain;
+  thrust_in = thrust_filt.o[0] + euler_cmd.z * thrust_in_specific_force_gain;
   Bound(thrust_in, 0, 9600);
 
 #if GUIDANCE_INDI_RC_DEBUG
-  if(radio_control.values[RADIO_THROTTLE]<300) {
+  if (radio_control.values[RADIO_THROTTLE] < 300) {
     thrust_in = 0;
   }
 #endif
@@ -236,7 +278,8 @@ void guidance_indi_filter_thrust(void)
  * The roll and pitch also need to be filtered to synchronize them with the
  * acceleration
  */
-void guidance_indi_propagate_filters(void) {
+void guidance_indi_propagate_filters(void)
+{
   struct NedCoor_f *accel = stateGetAccelNed_f();
   update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
   update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
@@ -252,7 +295,8 @@ void guidance_indi_propagate_filters(void) {
  * Calculate the matrix of partial derivatives of the roll, pitch and thrust
  * w.r.t. the NED accelerations
  */
-void guidance_indi_calcG(struct FloatMat33 *Gmat) {
+void guidance_indi_calcG(struct FloatMat33 *Gmat)
+{
 
   struct FloatEulers *euler = stateGetNedToBodyEulers_f();
 
@@ -265,15 +309,15 @@ void guidance_indi_calcG(struct FloatMat33 *Gmat) {
   //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
   float T = -9.81;
 
-  RMAT_ELMT(*Gmat, 0, 0) = (cphi*spsi - sphi*cpsi*stheta)*T;
-  RMAT_ELMT(*Gmat, 1, 0) = (-sphi*spsi*stheta - cpsi*cphi)*T;
-  RMAT_ELMT(*Gmat, 2, 0) = -ctheta*sphi*T;
-  RMAT_ELMT(*Gmat, 0, 1) = (cphi*cpsi*ctheta)*T;
-  RMAT_ELMT(*Gmat, 1, 1) = (cphi*spsi*ctheta)*T;
-  RMAT_ELMT(*Gmat, 2, 1) = -stheta*cphi*T;
-  RMAT_ELMT(*Gmat, 0, 2) = sphi*spsi + cphi*cpsi*stheta;
-  RMAT_ELMT(*Gmat, 1, 2) = cphi*spsi*stheta - cpsi*sphi;
-  RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
+  RMAT_ELMT(*Gmat, 0, 0) = (cphi * spsi - sphi * cpsi * stheta) * T;
+  RMAT_ELMT(*Gmat, 1, 0) = (-sphi * spsi * stheta - cpsi * cphi) * T;
+  RMAT_ELMT(*Gmat, 2, 0) = -ctheta * sphi * T;
+  RMAT_ELMT(*Gmat, 0, 1) = (cphi * cpsi * ctheta) * T;
+  RMAT_ELMT(*Gmat, 1, 1) = (cphi * spsi * ctheta) * T;
+  RMAT_ELMT(*Gmat, 2, 1) = -stheta * cphi * T;
+  RMAT_ELMT(*Gmat, 0, 2) = sphi * spsi + cphi * cpsi * stheta;
+  RMAT_ELMT(*Gmat, 1, 2) = cphi * spsi * stheta - cpsi * sphi;
+  RMAT_ELMT(*Gmat, 2, 2) = cphi * ctheta;
 }
 
 /**
@@ -283,7 +327,7 @@ void guidance_indi_calcG(struct FloatMat33 *Gmat) {
  *
  * function that creates a quaternion from a roll, pitch and yaw setpoint
  */
-void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, float heading)
+void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers *indi_rp_cmd, bool in_flight, float heading)
 {
   struct FloatQuat q_rp_cmd;
   //this is a quaternion without yaw! add the desired yaw before you use it!
@@ -318,5 +362,26 @@ void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_c
     QUAT_COPY(q_sp, q_rp_sp);
   }
 
-  QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp);
+  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
 }
+
+/**
+ * ABI callback that obtains the acceleration setpoint from telemetry
+ * flag: 0 -> 2D, 1 -> 3D
+ */
+static void accel_sp_cb(uint8_t sender_id __attribute__((unused)), uint8_t flag, struct FloatVect3 *accel_sp)
+{
+  if (flag == 0) {
+    indi_accel_sp.x = accel_sp->x;
+    indi_accel_sp.y = accel_sp->y;
+    indi_accel_sp_set_2d = true;
+    time_of_accel_sp_2d = get_sys_time_float();
+  } else if (flag == 1) {
+    indi_accel_sp.x = accel_sp->x;
+    indi_accel_sp.y = accel_sp->y;
+    indi_accel_sp.z = accel_sp->z;
+    indi_accel_sp_set_3d = true;
+    time_of_accel_sp_3d = get_sys_time_float();
+  }
+}
+
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
index e09f2040cb..e88578382d 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.h
@@ -35,6 +35,7 @@
 extern void guidance_indi_enter(void);
 extern void guidance_indi_run(bool in_flight, float heading_sp);
 extern void stabilization_attitude_set_setpoint_rp_quat_f(struct FloatEulers* indi_rp_cmd, bool in_flight, float heading);
+extern void guidance_indi_init(void);
 
 extern float guidance_indi_thrust_specific_force_gain;
 extern struct FloatVect3 euler_cmd;
diff --git a/sw/airborne/modules/fc_rotor/fc_rotor.c b/sw/airborne/modules/fc_rotor/fc_rotor.c
new file mode 100644
index 0000000000..899b78d77e
--- /dev/null
+++ b/sw/airborne/modules/fc_rotor/fc_rotor.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+#include "math/pprz_algebra_float.h"
+#include "subsystems/abi.h"
+#include "subsystems/datalink/datalink.h" // dl_buffer
+#include "autopilot.h"
+#include "modules/fc_rotor/fc_rotor.h"
+#include "firmwares/rotorcraft/navigation.h"
+
+void fc_rotor_init(void)
+{
+}
+
+void fc_read_msg(void)
+{
+  struct FloatVect3 u;
+  uint8_t ac_id = DL_DESIRED_SETPOINT_ac_id(dl_buffer);
+
+  if (ac_id == AC_ID) {
+    // 0: 2D control, 1: 3D control
+    uint8_t flag = DL_DESIRED_SETPOINT_flag(dl_buffer);
+
+    u.x = DL_DESIRED_SETPOINT_ux(dl_buffer);
+    u.y = DL_DESIRED_SETPOINT_uy(dl_buffer);
+    u.z = DL_DESIRED_SETPOINT_uz(dl_buffer);
+
+    AbiSendMsgACCEL_SP(ACCEL_SP_FCR_ID, flag, &u);
+
+    if (flag == 0) {
+      // with 2D control, set flight altitude in integer ENU LTP frame
+      nav_flight_altitude = POS_BFP_OF_REAL(u.z);
+    }
+  }
+}
+
diff --git a/sw/airborne/modules/fc_rotor/fc_rotor.h b/sw/airborne/modules/fc_rotor/fc_rotor.h
new file mode 100644
index 0000000000..d1ec3b8575
--- /dev/null
+++ b/sw/airborne/modules/fc_rotor/fc_rotor.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (C) 2016  Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/** \file fc_rotor.h
+ *
+ *  Formation control algorithm for rotorcrafts
+ */
+
+#include <std.h>
+
+#ifndef FC_ROTOR_H
+#define FC_ROTOR_H
+
+extern void fc_rotor_init(void);
+extern void fc_read_msg(void);
+
+#endif // FC_ROTOR_H
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index c578b67750..81e8e31019 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -370,5 +370,12 @@
 #define CAM_JEVOIS_ID 1
 #endif
 
+/*
+ * IDs of ACCEL_SP senders (message 21)
+ */
+
+#ifndef ACCEL_SP_FCR_ID
+#define ACCEL_SP_FCR_ID 1 // Formation Control Rotorcraft
+#endif
 
 #endif /* ABI_SENDER_IDS_H */
diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
index fb6ca83c6d..0fd6670461 100644
--- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c
+++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
@@ -58,6 +58,18 @@ struct InsGpsPassthrough {
 
 struct InsGpsPassthrough ins_gp;
 
+/** ABI bindings on ACCEL data
+ */
+#ifndef INS_PT_IMU_ID
+#define INS_PT_IMU_ID ABI_BROADCAST
+#endif
+static abi_event accel_ev;
+static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
+
+static abi_event body_to_imu_ev;
+static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
+static struct OrientationReps body_to_imu;
+
 
 /** ABI binding for gps data.
  * Used for GPS ABI messages.
@@ -107,7 +119,7 @@ static void send_ins(struct transport_tx *trans, struct link_device *dev)
 
 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
 {
-  static const float fake_baro_z = 0.0;
+  static float fake_baro_z = 0.0;
   pprz_msg_send_INS_Z(trans, dev, AC_ID,
                       (float *)&fake_baro_z, &ins_gp.ltp_pos.z,
                       &ins_gp.ltp_speed.z, &ins_gp.ltp_accel.z);
@@ -115,7 +127,7 @@ static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
 
 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
 {
-  static const float fake_qfe = 0.0;
+  static float fake_qfe = 0.0;
   if (ins_gp.ltp_initialized) {
     pprz_msg_send_INS_REF(trans, dev, AC_ID,
                           &ins_gp.ltp_def.ecef.x, &ins_gp.ltp_def.ecef.y, &ins_gp.ltp_def.ecef.z,
@@ -158,6 +170,8 @@ void ins_gps_passthrough_init(void)
 #endif
 
   AbiBindMsgGPS(INS_PT_GPS_ID, &gps_ev, gps_cb);
+  AbiBindMsgIMU_ACCEL_INT32(INS_PT_IMU_ID, &accel_ev, accel_cb);
+  AbiBindMsgBODY_TO_IMU_QUAT(INS_PT_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
 }
 
 void ins_reset_local_origin(void)
@@ -180,3 +194,25 @@ void ins_reset_altitude_ref(void)
   ins_gp.ltp_def.hmsl = gps.hmsl;
   stateSetLocalOrigin_i(&ins_gp.ltp_def);
 }
+
+static void accel_cb(uint8_t sender_id __attribute__((unused)),
+                     uint32_t stamp __attribute__((unused)),
+                     struct Int32Vect3 *accel)
+{
+  // untilt accel and remove gravity
+  struct Int32Vect3 accel_body, accel_ned;
+  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
+  int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
+  struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
+  int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
+  accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
+  stateSetAccelNed_i((struct NedCoor_i *)&accel_ned);
+  VECT3_COPY(ins_gp.ltp_accel, accel_ned);
+}
+
+static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
+                           struct FloatQuat *q_b2i_f)
+{
+  orientationSetQuat_f(&body_to_imu, q_b2i_f);
+}
+
diff --git a/sw/ground_segment/joystick/input2ivy.ml b/sw/ground_segment/joystick/input2ivy.ml
index 9ac204acea..20919d194c 100644
--- a/sw/ground_segment/joystick/input2ivy.ml
+++ b/sw/ground_segment/joystick/input2ivy.ml
@@ -135,7 +135,10 @@ let ac_id_of_name = fun ac_name ->
     ExtXml.int_attrib aircraft "ac_id"
   with
       Not_found ->
-        failwith (sprintf "A/C '%s' not found" ac_name)
+        if ac_name = "GCS" then
+          0 (* return GCS id *)
+        else
+          failwith (sprintf "A/C '%s' not found" ac_name)
 
 (** Fill the index_of_settings table from var/AC/settings.xml *)
 let hash_index_of_settings = fun ac_name ->
@@ -602,8 +605,11 @@ let () =
 
   let ac_id = ac_id_of_name !ac_name in
 
-  hash_index_of_settings !ac_name;
-  hash_index_of_blocks !ac_name;
+  if ac_id > 0 then begin
+    (* build hash only for real AC, not for GCS *)
+    hash_index_of_settings !ac_name;
+    hash_index_of_blocks !ac_name;
+  end;
 
   Printf.printf "Joystick ID (option -id): %u\n" !joystick_id;
   Printf.printf "Joystick SDL device index (option -d): %u\n" !device_index;
diff --git a/sw/ground_segment/python/fc_rotor/fc_rotor.py b/sw/ground_segment/python/fc_rotor/fc_rotor.py
new file mode 100755
index 0000000000..4f9368214e
--- /dev/null
+++ b/sw/ground_segment/python/fc_rotor/fc_rotor.py
@@ -0,0 +1,306 @@
+#!/usr/bin/env python
+#
+# Copyright (C) 2017 Hector Garcia de Marina <hgdemarina@gmail.com>
+#                    Gautier Hattenberger <gautier.hattenberger@enac.fr>
+#
+# This file is part of paparazzi.
+#
+# paparazzi is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2, or (at your option)
+# any later version.
+#
+# paparazzi is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with paparazzi; see the file COPYING.  If not, see
+# <http://www.gnu.org/licenses/>.
+#
+
+'''
+Formation Control for Rotorcraft, remotly piloted from ground with a Joystick
+'''
+
+import sys
+import numpy as np
+import json
+from time import sleep
+from os import path, getenv
+PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
+sys.path.append(PPRZ_HOME + "/var/lib/python/")
+from pprzlink.ivy import IvyMessagesInterface
+from pprzlink.message import PprzMessage 
+
+import lib_rigid_formations as rf
+from scipy import linalg as la
+
+class FC_Rotorcraft:
+    def __init__(self, ac_id):
+        self.initialized = False
+        self.id = ac_id
+        self.X = np.zeros(3)
+        self.V = np.zeros(3)
+        self.timeout = 0
+
+class FC_Joystick:
+    def __init__(self):
+        self.trans = 0.
+        self.trans2 = 0.
+        self.rot = 0.
+        self.rot2 = 0.
+        self.button1 = False
+        self.button2 = False
+        self.button3 = False
+        self.button4 = False
+
+class FormationControl:
+    def __init__(self, config, freq=10., use_ground_ref=False, verbose=False):
+        self.config = config
+        self.step = 1. / freq
+        self.sens = self.config['sensitivity']
+        self.use_ground_ref = use_ground_ref
+        self.enabled = True # run control by default
+        self.verbose = verbose
+        self.ids = self.config['ids']
+        self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids]
+        self.joystick = FC_Joystick()
+        self.altitude = 2.0 # starts from 1 m high
+        self.scale = 1.0
+        self.B = np.array(self.config['topology'])
+        self.d = np.array(self.config['desired_distances'])
+        self.t1 = np.array(self.config['motion']['t1'])
+        self.t2 = np.array(self.config['motion']['t2'])
+        self.r1 = np.array(self.config['motion']['r1'])
+        self.r2 = np.array(self.config['motion']['r2'])
+        self.k = np.array(self.config['gains'])
+
+        if self.B.size == 2:
+            self.B.shape = (2,1)
+
+        # Check formation settings
+        if len(self.ids) != np.size(self.B, 0):
+            print("The number of rotorcrafts in the topology and ids do not match")
+            return
+
+        if np.size(self.d) != np.size(self.B, 1):
+            print("The number of links in the topology and desired distances do not match")
+            return
+
+        #if np.size(self.d) != np.size(self.m,1):
+        #    print("The number of (columns) motion parameters and relative vectors do not match")
+        #    return
+
+        #if np.size(self.m, 0) != 8:
+        #    print("The number of (rows) motion parameters must be eight")
+        #    return
+
+        if self.config['3D'] == True:
+            print("3D formation is not supported yet")
+            return
+
+        # Start IVY interface
+        self._interface = IvyMessagesInterface("Formation Control Rotorcrafts")
+
+        # bind to INS message
+        def ins_cb(ac_id, msg):
+            if ac_id in self.ids and msg.name == "INS":
+                rc = self.rotorcrafts[self.ids.index(ac_id)]
+                i2p = 1. / 2**8     # integer to position
+                i2v = 1. / 2**19    # integer to velocity
+                rc.X[0] = float(msg['ins_x']) * i2p
+                rc.X[1] = float(msg['ins_y']) * i2p
+                rc.X[2] = float(msg['ins_z']) * i2p
+                rc.V[0] = float(msg['ins_xd']) * i2v
+                rc.V[1] = float(msg['ins_yd']) * i2v
+                rc.V[2] = float(msg['ins_zd']) * i2v
+                rc.timeout = 0
+                rc.initialized = True
+        if not self.use_ground_ref:
+            self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
+
+        # bind to GROUND_REF message
+        def ground_ref_cb(ground_id, msg):
+            ac_id = int(msg['ac_id'])
+            if ac_id in self.ids:
+                rc = self.rotorcrafts[self.ids.index(ac_id)]
+                # X and V in NED
+                rc.X[0] = float(msg['pos'][1])
+                rc.X[1] = float(msg['pos'][0])
+                rc.X[2] = -float(msg['pos'][2])
+                rc.V[0] = float(msg['speed'][1])
+                rc.V[1] = float(msg['speed'][0])
+                rc.V[2] = -float(msg['speed'][2])
+                rc.timeout = 0
+                rc.initialized = True
+        if self.use_ground_ref:
+            self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF"))
+
+        # bind to JOYSTICK message
+        def joystick_cb(ac_id, msg):
+            self.joystick.trans = float(msg['axis1']) * self.sens['t1'] / 127.
+            self.joystick.trans2 = float(msg['axis2']) * self.sens['t2'] / 127.
+            self.joystick.rot = float(msg['axis3']) * self.sens['r1'] / 127.
+            self.altitude = self.sens['alt_min'] + float(msg['axis4']) * (self.sens['alt_max'] - self.sens['alt_min']) / 127.
+            if msg['button1'] == '1' and not self.joystick.button1:
+                self.scale = min(self.scale + self.sens['scale'], self.sens['scale_max'])
+            if msg['button2'] == '1' and not self.joystick.button2:
+                self.scale = max(self.scale - self.sens['scale'], self.sens['scale_min'])
+            if msg['button4'] == '1' and not self.joystick.button4:
+                self.enabled = False
+            if msg['button3'] == '1' and not self.joystick.button3:
+                self.enabled = True
+            self.joystick.button1 = (msg['button1'] == '1')
+            self.joystick.button2 = (msg['button2'] == '1')
+            self.joystick.button3 = (msg['button3'] == '1')
+            self.joystick.button4 = (msg['button4'] == '1')
+        self._interface.subscribe(joystick_cb, PprzMessage("ground", "JOYSTICK"))
+
+    def __del__(self):
+        self.stop()
+
+    def stop(self):
+        # Stop IVY interface
+        if self._interface is not None:
+            self._interface.shutdown()
+
+    def formation(self):
+        '''
+        formation control algorithm
+        '''
+        ready = True
+        for rc in self.rotorcrafts:
+            if not rc.initialized:
+                if self.verbose:
+                    print("Waiting for state of rotorcraft ", rc.id)
+                    sys.stdout.flush()
+                ready = False
+            if rc.timeout > 0.5:
+                if self.verbose:
+                    print("The state msg of rotorcraft ", rc.id, " stopped")
+                    sys.stdout.flush()
+                ready = False
+            if rc.initialized and 'geo_fence' in self.config.keys():
+                geo_fence = self.config['geo_fence']
+                if (rc.X[0] < geo_fence['x_min'] or rc.X[0] > geo_fence['x_max']
+                        or rc.X[1] < geo_fence['y_min'] or rc.X[1] > geo_fence['y_max']
+                        or rc.X[2] < geo_fence['z_min'] or rc.X[2] > geo_fence['z_max']):
+                    if self.verbose:
+                        print("The rotorcraft", rc.id, " is out of the fence (", rc.X, ", ", geo_fence, ")")
+                        sys.stdout.flush()
+                    ready = False
+
+        if not ready:
+            return
+
+        dim = 2 * len(self.rotorcrafts)
+        X = np.zeros(dim)
+        V = np.zeros(dim)
+        U = np.zeros(dim)
+
+        i = 0
+        for rc in self.rotorcrafts:
+            X[i] = rc.X[0]
+            X[i+1] = rc.X[1]
+            V[i] = rc.V[0]
+            V[i+1] = rc.V[1]
+            i = i + 2
+
+
+        Bb = la.kron(self.B, np.eye(2))
+        Z = Bb.T.dot(X)
+        Dz = rf.make_Dz(Z, 2)
+        Dzt = rf.make_Dzt(Z, 2, 1)
+        Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
+        Zh = rf.make_Zh(Z, 2)
+        E = rf.make_E(Z, self.d * self.scale, 2, 1)
+
+        # Shape and motion control
+        jmu_t1 = self.joystick.trans * self.t1[0,:]
+        jtilde_mu_t1 = self.joystick.trans * self.t1[1,:]
+        jmu_r1 = self.joystick.rot * self.r1[0,:]
+        jtilde_mu_r1 = self.joystick.rot * self.r1[1,:]
+        jmu_t2 = self.joystick.trans2 * self.t2[0,:]
+        jtilde_mu_t2 = self.joystick.trans2 * self.t2[1,:]
+        jmu_r2 = self.joystick.rot2 * self.r2[0,:]
+        jtilde_mu_r2 = self.joystick.rot2 * self.r2[1,:]
+
+        Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
+        Avt1b = la.kron(Avt1, np.eye(2))
+        Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
+        Avr1b = la.kron(Avr1, np.eye(2))
+        Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
+        Avt2b = la.kron(Avt2, np.eye(2))
+        Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
+        Avr2b = la.kron(Avr2, np.eye(2))
+
+        Avb = Avt1b + Avr1b + Avt2b + Avr2b
+        Avr = Avr1 + Avr2
+
+        if self.B.size == 2:
+            Zhr = np.array([-Zh[1],Zh[0]])
+            U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*(Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(jmu_r1[0])*la.kron(Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1), np.eye(2)).dot(Zhr)
+        else:
+            U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*Avb.dot(Zh) + la.kron(Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
+
+        if self.verbose:
+            #print "Positions: " + str(X).replace('[','').replace(']','')
+            #print "Velocities: " + str(V).replace('[','').replace(']','')
+            #print "Acceleration command: " + str(U).replace('[','').replace(']','')
+            print "Error distances: " + str(E).replace('[','').replace(']','')
+            sys.stdout.flush()
+
+        i = 0
+        for ac in self.rotorcrafts:
+            msg = PprzMessage("datalink", "DESIRED_SETPOINT")
+            msg['ac_id'] = ac.id
+            msg['flag'] = 0 # full 3D not supported yet
+            msg['ux'] = U[i]
+            msg['uy'] = U[i+1]
+            msg['uz'] = self.altitude
+            self._interface.send(msg)
+            i = i+2
+
+
+    def run(self):
+        try:
+            # The main loop
+            while True:
+                # TODO: make better frequency managing
+                sleep(self.step)
+
+                for rc in self.rotorcrafts:
+                    rc.timeout = rc.timeout + self.step
+
+                # Run the formation algorithm
+                if self.enabled:
+                    self.formation()
+
+        except KeyboardInterrupt:
+            self.stop()
+
+
+if __name__ == '__main__':
+    import argparse
+
+    parser = argparse.ArgumentParser(description="Formation Control for Rotorcrafts")
+    parser.add_argument('config_file', help="JSON configuration file")
+    parser.add_argument('-f', '--freq', dest='freq', default=10, type=int, help="control frequency")
+    parser.add_argument('-gr', '--use_ground_ref', dest='use_ground_ref', default=False, action='store_true', help="use GROUND_REF messages instead of INS messages")
+    parser.add_argument('-v', '--verbose', dest='verbose', default=False, action='store_true', help="display debug messages")
+    args = parser.parse_args()
+
+    # if config file is not a full path, look for files in conf/swarm folder of PAPARAZI_HOME
+    if not path.isabs(args.config_file):
+        args.config_file = path.join(PPRZ_HOME, path.normpath("conf/swarm"), args.config_file)
+
+    with open(args.config_file, 'r') as f:
+        conf = json.load(f)
+        if args.verbose:
+            print(json.dumps(conf))
+
+        fc = FormationControl(conf, freq=args.freq, use_ground_ref=args.use_ground_ref, verbose=args.verbose)
+        fc.run()
+
diff --git a/sw/ground_segment/python/fc_rotor/lib_rigid_formations.py b/sw/ground_segment/python/fc_rotor/lib_rigid_formations.py
new file mode 100644
index 0000000000..3a6b6e1cb9
--- /dev/null
+++ b/sw/ground_segment/python/fc_rotor/lib_rigid_formations.py
@@ -0,0 +1,122 @@
+"Helpful functions for the rigid motion control"
+
+import numpy as np
+from scipy import linalg as la
+
+def make_S1(B):
+    r, c = B.shape
+    S1 = np.zeros_like(B)
+
+    for i in range(0, r):
+        for j in range(0, c):
+            if B[i,j] == 1:
+                S1[i,j] = 1
+
+    return S1
+
+def make_S2(B):
+    S2 = make_S1(-1*B)
+    return S2
+
+def make_Bd(B):
+    r, c = B.shape
+    Bd = np.copy(B)
+
+    for i in range(0, c):
+        Bd[0, i] = 0
+
+    return Bd
+
+def make_Zh(Z, m):
+    edges = Z.size/m
+    Zh = np.zeros(Z.size)
+
+    for i in range(0, edges):
+        norm = la.norm(Z[(i*m):(i*m+m)])
+        if norm > 0.05:
+            Zh[(i*m):(i*m+m)] = Z[(i*m):(i*m+m)]/norm
+
+    return Zh
+
+def make_Dz(Z, m):
+    edges = Z.size/m
+    Dz = np.zeros((Z.size, edges))
+    
+    j = 0
+
+    for i in range(0, edges):
+        Dz[j:j+m, i] =  Z[j:j+m]
+        j+=m
+
+    return Dz
+
+def make_Dzt(Z, m, l):
+    edges = Z.size/m
+    if l == 2:
+        return np.eye(edges)
+
+    Zt = np.zeros(edges)
+    for i in range(0, edges):
+        norm = la.norm(Z[(i*m):(i*m+m)])
+        if norm > 0.01:
+            Zt[i] = (norm)**(l-2)
+    
+    return np.diag(Zt)
+
+def make_Dztstar(d, m, l):
+    edges = d.size
+    if l == 2:
+        return np.eyes(edges)
+    
+    Ztstar = np.zeros(edges)
+    if edges == 1:
+        Ztstar[0] = d**(l-2)
+    else:
+        for i in range(0, edges):
+            Ztstar[i] = d[i]**(l-2)
+
+    return np.diag(Ztstar)
+
+def make_DPzh(Z, m):
+    edges = Z.size/m
+    DPzh = np.zeros((2*edges, 2*edges))
+    
+    Zt = np.diag(make_Dzt(Z, m, 1))
+    j = 0
+
+    for i in range(0, edges):
+        z = Z[j:j+m] * Zt[i]
+        DPzh[j:j+m, j:j+m] = np.identity(m) - \
+        np.dot(z[:,np.newaxis], z[np.newaxis, :])
+        j+=m
+
+    return DPzh
+
+def make_E(Z, d, m, l):
+    edges = Z.size/m
+    E = np.zeros(edges)
+    if edges == 1:
+        E[0] =  la.norm(Z)**l - d**l
+    else:
+        for i in range(0, edges):
+            E[i] = (la.norm(Z[(i*m):(i*m+m)]))**l - d[i]**l
+
+    return E
+
+def make_Av(B, mu, tilde_mu):
+    agents, edges = B.shape
+    Av = np.zeros(B.shape)
+    for i in range(0, agents):
+        for j in range(0, edges):
+            if B[i,j] == 1:
+                Av[i,j] = mu[j]
+            elif B[i,j] == -1:
+                Av[i,j] = tilde_mu[j]
+
+    return Av
+
+def make_Aa(B, mu, tilde_mu):
+    Av = make_Av(B, mu, tilde_mu)
+    Aa = Av.dot(B.T).dot(Av)
+
+    return Aa
diff --git a/sw/ground_segment/python/natnet3.x/NatNetClient.py b/sw/ground_segment/python/natnet3.x/NatNetClient.py
index d169dca247..60475d3c8c 100644
--- a/sw/ground_segment/python/natnet3.x/NatNetClient.py
+++ b/sw/ground_segment/python/natnet3.x/NatNetClient.py
@@ -421,7 +421,7 @@ class NatNetClient:
                 offset += self.__unpackSkeletonDescription( data[offset:] )
             
     def __dataThreadFunction( self, sock ):
-        sock.settimeout(0.5)
+        sock.settimeout(0.01)
         while self.running:
             # Block for input
             try:
diff --git a/sw/ground_segment/python/natnet3.x/natnet2ivy.py b/sw/ground_segment/python/natnet3.x/natnet2ivy.py
index 099624b5df..726722f0f4 100755
--- a/sw/ground_segment/python/natnet3.x/natnet2ivy.py
+++ b/sw/ground_segment/python/natnet3.x/natnet2ivy.py
@@ -68,6 +68,7 @@ parser.add_argument('-dp', '--data_port', dest='data_port', type=int, default=15
 parser.add_argument('-cp', '--command_port', dest='command_port', type=int, default=1510, help="NatNet server command socket UDP port")
 parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages")
 parser.add_argument('-f', '--freq', dest='freq', default=10, type=int, help="transmit frequency")
+parser.add_argument('-gr', '--ground_ref', dest='ground_ref', action='store_true', help="also send the GROUND_REF message")
 parser.add_argument('-vs', '--vel_samples', dest='vel_samples', default=4, type=int, help="amount of samples to compute velocity (should be greater than 2)")
 args = parser.parse_args()
 
@@ -124,7 +125,6 @@ def compute_velocity(ac_id):
             vel[2] / nb
     return vel
 
-
 def receiveRigidBodyList( rigidBodyList, stamp ):
     for (ac_id, pos, quat) in rigidBodyList:
         i = str(ac_id)
@@ -155,6 +155,19 @@ def receiveRigidBodyList( rigidBodyList, stamp ):
         msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
         ivy.send(msg)
 
+        # send GROUND_REF message if needed
+        if args.ground_ref:
+            gr = PprzMessage("ground", "GROUND_REF")
+            gr['ac_id'] = str(id_dict[i])
+            gr['frame'] = "LTP_ENU"
+            gr['pos'] = pos
+            gr['speed'] = vel
+            gr['quat'] = [quat[3], quat[0], quat[1], quat[2]]
+            gr['rate'] = [ 0., 0., 0. ]
+            gr['timestamp'] = stamp
+            ivy.send(gr)
+
+
 
 # start natnet interface
 natnet = NatNetClient(