From 46a8bfb0a2a39e03cd07d1106858782abadf8b69 Mon Sep 17 00:00:00 2001
From: Hector Garcia de Marina <noeth3r@gmail.com>
Date: Tue, 29 Jan 2019 11:19:24 +0100
Subject: [PATCH] Futaba16SZ, units in circular formation, and no IMU in
 default msg cfg (#2382)

* fixed tab and IMU not necessary in default

* explicit units

* Futaba T16SZ conf file

* units

* fixed tab and IMU not necessary in default

* explicit units

* Futaba T16SZ conf file

* units
---
 conf/radios/T16SZ_SBUS.xml                    | 64 +++++++++++++++++++
 conf/telemetry/default_fixedwing_gvf.xml      |  1 -
 .../python/gvf/circularFormation.py           |  4 +-
 .../python/gvf/formation/three_aircraft.json  |  4 +-
 .../python/gvf/formation/two_aircraft.json    |  4 +-
 5 files changed, 70 insertions(+), 7 deletions(-)
 create mode 100644 conf/radios/T16SZ_SBUS.xml

diff --git a/conf/radios/T16SZ_SBUS.xml b/conf/radios/T16SZ_SBUS.xml
new file mode 100644
index 0000000000..a802cadefd
--- /dev/null
+++ b/conf/radios/T16SZ_SBUS.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<!-- $Id$
+--
+-- (c) 2013 Gautier Hattenberger
+--
+-- 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, write to
+-- the Free Software Foundation, 59 Temple Place - Suite 330,
+-- Boston, MA 02111-1307, USA.
+-->
+
+<!--
+-- Attributes of root (Radio) tag :
+-- name: name of RC
+-- data_min: min width of a pulse to be considered as a data pulse
+-- data_max: max width of a pulse to be considered as a data pulse
+-- sync_min: min width of a pulse to be considered as a synchro pulse
+-- sync_max: max width of a pulse to be considered as a synchro pulse
+-- pulse_type: POSITIVE ( Futaba and others) | NEGATIVE (JR)
+-- min, max and sync are expressed in micro-seconds
+-->
+
+<!--
+-- Attributes of channel tag :
+-- ctl: name of the command on the transmitter - only for displaying
+-- function: logical command
+-- average: (boolean) channel filtered through several frames (for discrete commands)
+-- min: minimum pulse length (micro-seconds)
+-- max: maximum pulse length (micro-seconds)
+-- neutral: neutral pulse length (micro-seconds)
+-- Note: a command may be reversed by exchanging min and max values
+-->
+
+<!DOCTYPE radio SYSTEM "radio.dtd">
+<radio name="Futaba T16SZ with SBUS" data_min="1000" data_max="2000" sync_min ="5000" sync_max ="15000" pulse_type="POSITIVE">
+ <channel ctl="A" function="ROLL"     min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="B" function="PITCH"    min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="C" function="THROTTLE" min="1000" neutral="1000" max="2000" average="0"/>
+ <channel ctl="D" function="YAW"      min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="E" function="MODE"     max="1000" neutral="1520" min="2000" average="1"/>
+ <channel ctl="F" function="GAIN1"    min="1000" neutral="1520" max="2000" average="0"/>
+ <channel ctl="G" function="GAIN2"    min="1000" neutral="1520" max="2000" average="0"/>
+ <channel ctl="H" function="GAIN3"    min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="I" function="GAIN4"    min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="J" function="GAIN5"    min="2000" neutral="2000" max="1000" average="0"/>
+ <channel ctl="K" function="GAIN6"    min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="N" function="GAIN7"    max="1000" neutral="1520" min="2000" average="1"/>
+ <channel ctl="M" function="GAIN8"    min="1000" neutral="1520" max="2000" average="0"/>
+ <channel ctl="L" function="GAIN9"    min="1000" neutral="1520" max="2000" average="0"/>
+ <channel ctl="O" function="GAIN10"   min="1000" neutral="1500" max="2000" average="0"/>
+ <channel ctl="P" function="GAIN11"   min="1000" neutral="1500" max="2000" average="0"/>
+</radio>
diff --git a/conf/telemetry/default_fixedwing_gvf.xml b/conf/telemetry/default_fixedwing_gvf.xml
index e08666d525..16c05b6129 100644
--- a/conf/telemetry/default_fixedwing_gvf.xml
+++ b/conf/telemetry/default_fixedwing_gvf.xml
@@ -23,7 +23,6 @@
       <message name="DATALINK_REPORT"     period="5.1"/>
       <message name="DL_VALUE"            period="1.5"/>
       <message name="IR_SENSORS"          period="1.2"/>
-      <message name="IMU_GYRO"            period="1.1"/>
       <message name="SURVEY"              period="2.1"/>
       <message name="GPS_SOL"             period="2.0"/>
       <message name="CAM"                 period="0.5"/>
diff --git a/sw/ground_segment/python/gvf/circularFormation.py b/sw/ground_segment/python/gvf/circularFormation.py
index bfb093e3d6..b909031b1d 100644
--- a/sw/ground_segment/python/gvf/circularFormation.py
+++ b/sw/ground_segment/python/gvf/circularFormation.py
@@ -61,9 +61,9 @@ class FormationControl:
         self.verbose = verbose
         self.ids = self.config['ids']
         self.B = np.array(self.config['topology'])
-        self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
+        self.Zdesired = np.array(self.config['desired_intervehicle_angles_degrees'])*np.pi/180
         self.k = np.array(self.config['gain'])
-        self.radius = np.array(self.config['desired_stationary_radius'])
+        self.radius = np.array(self.config['desired_stationary_radius_meters'])
         self.aircraft = [Aircraft(i) for i in self.ids]
         self.sigmas = np.zeros(len(self.aircraft))
 
diff --git a/sw/ground_segment/python/gvf/formation/three_aircraft.json b/sw/ground_segment/python/gvf/formation/three_aircraft.json
index 19a59ce321..ff5e38b510 100644
--- a/sw/ground_segment/python/gvf/formation/three_aircraft.json
+++ b/sw/ground_segment/python/gvf/formation/three_aircraft.json
@@ -5,7 +5,7 @@
     [-1, 1],
     [ 0,-1]
   ],
-  "desired_intervehicle_angles": [0, 0],
+  "desired_intervehicle_angles_degrees": [0, 0],
   "gain": 10,
-  "desired_stationary_radius": 80
+  "desired_stationary_radius_meters": 80
 }
diff --git a/sw/ground_segment/python/gvf/formation/two_aircraft.json b/sw/ground_segment/python/gvf/formation/two_aircraft.json
index bcdc31353f..34038f4909 100644
--- a/sw/ground_segment/python/gvf/formation/two_aircraft.json
+++ b/sw/ground_segment/python/gvf/formation/two_aircraft.json
@@ -4,7 +4,7 @@
     [ 1],
     [-1]
   ],
-  "desired_intervehicle_angles": [0],
+  "desired_intervehicle_angles_degrees": [0],
   "gain": 10,
-  "desired_stationary_radius": 80
+  "desired_stationary_radius_meters": 80
 }