[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

@@ -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>

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>

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>

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"/>

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
+  }
+}
+

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
+  }
+}
+