From 4adee28825ad8f345e5ddd277038d382c0ee76db Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Mon, 2 Jun 2014 14:25:24 +0200
Subject: [PATCH] [conf] enable mag for quadshot and add it to tests/examples

---
 .../examples/quadshot_asp21_spektrum.xml      | 171 ++++++++----------
 conf/conf_example.xml                         |  10 +
 conf/conf_tests.xml                           |  10 +
 3 files changed, 99 insertions(+), 92 deletions(-)

diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml
index 93d0fafb490..b5afe0d6e70 100644
--- a/conf/airframes/examples/quadshot_asp21_spektrum.xml
+++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml
@@ -1,5 +1,5 @@
 <!-- this is a quadshot vehicle equiped with Lisa/m2.0 and an aspirin 2.1
-More information on the Quadshot can be found at transition-robotics.com -->
+     More information on the Quadshot can be found at transition-robotics.com -->
 
 <airframe name="quadshot aspirin 2.1 spektrum">
 
@@ -26,22 +26,22 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <set servo="B1" value="motor_mixing.commands[2]"/>
     <set servo="B2"  value="motor_mixing.commands[3]"/>
 
- <!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
- <!-- First the correct feedback is stored in variables -->
+    <!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
+    <!-- First the correct feedback is stored in variables -->
     <let var="aileron_feedback_left"  value="@YAW"/>
     <let var="aileron_feedback_right" value="@YAW"/>
 
     <let var="elevator_feedback_left"  value="+@PITCH"/>
     <let var="elevator_feedback_right" value="-@PITCH"/>
 
- <!-- Here the gains are defined for the two feedback cases, hover and forward-->
+    <!-- Here the gains are defined for the two feedback cases, hover and forward-->
     <let var="hover_left"  value="3*$aileron_feedback_left"/>
     <let var="hover_right" value="3*$aileron_feedback_right"/>
 
     <let var="forward_left"  value="6*$aileron_feedback_left+4*$elevator_feedback_left"/>
     <let var="forward_right" value="6*$aileron_feedback_right+4*$elevator_feedback_right"/>
 
- <!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
+    <!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
     <set servo="ELEVON_LEFT"    value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left  : $forward_left" />
     <set servo="ELEVON_RIGHT"   value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_right  : $forward_right" />
   </command_laws>
@@ -58,15 +58,15 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
   </section>
 
-<!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->
+  <!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->
 
-<!-- Note that is better to have *no* caliberation file at all, than a one with incorrect caliberation values.
-  The default factory values are most of the time perfectly acceptable, so by default we will not use the caliberation file -->
+  <!-- Note that is better to have *no* caliberation file at all, than a one with incorrect caliberation values.
+       The default factory values are most of the time perfectly acceptable, so by default we will not use the caliberation file -->
 
   <section name="IMU" prefix="IMU_">
     <!-- Use default driver values for gyro -->
 
- <!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
+    <!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
     <define name="ACCEL_X_NEUTRAL" value="0"/>
     <define name="ACCEL_Y_NEUTRAL" value="0"/>
     <define name="ACCEL_Z_NEUTRAL" value="0"/>
@@ -88,18 +88,18 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
   </section>
 
- <section name="AUTOPILOT">
-   <define name="MODE_MANUAL"  value="AP_MODE_ATTITUDE_DIRECT"/>
-   <define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
-   <define name="MODE_AUTO2"  value="AP_MODE_RATE_DIRECT"/>
+  <section name="AUTOPILOT">
+    <define name="MODE_MANUAL"  value="AP_MODE_ATTITUDE_DIRECT"/>
+    <define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
+    <define name="MODE_AUTO2"  value="AP_MODE_RATE_DIRECT"/>
 
-   <define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
- </section>
+    <define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
+  </section>
 
- <section name="BAT">
+  <section name="BAT">
     <define name="MIN_BAT_LEVEL" value="10.4" units="V"/>
-   <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
- </section>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
+  </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
     <define name="SP_MAX_P" value="10000"/>
@@ -115,7 +115,7 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <define name="IGAIN_R" value="200"/>
   </section>
 
- <!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
+  <!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
   <section name ="GAIN_SETS">
     <define name="NUMBER_OF_GAINSETS" value="2"/>
     <define name="SCHEDULING_VARIABLE" value="(radio.values[COMMAND_THRUST]+ transition_status"/>
@@ -183,21 +183,19 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <define name="PSI_DDGAIN"   value=" 0"/>
   </section>
 
-  <section name="INS" prefix="INS_">
-  </section>
 
- <!-- Gains for vertical navigation -->
- <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
-   <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
-   <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
-   <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
-   <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
-   <define name="MAX_SUM_ERR" value="2000000"/>
-   <define name="HOVER_KP"    value="200"/>
-   <define name="HOVER_KD"    value="175"/>
-   <define name="HOVER_KI"    value="72"/>
-   <define name="NOMINAL_HOVER_THROTTLE" value ="0.4"/>
-   <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
+  <!-- Gains for vertical navigation -->
+  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
+    <define name="MIN_ERR_Z"   value="POS_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_Z"   value="POS_BFP_OF_REAL( 10.)"/>
+    <define name="MIN_ERR_ZD"  value="SPEED_BFP_OF_REAL(-10.)"/>
+    <define name="MAX_ERR_ZD"  value="SPEED_BFP_OF_REAL( 10.)"/>
+    <define name="MAX_SUM_ERR" value="2000000"/>
+    <define name="HOVER_KP"    value="200"/>
+    <define name="HOVER_KD"    value="175"/>
+    <define name="HOVER_KI"    value="72"/>
+    <define name="NOMINAL_HOVER_THROTTLE" value ="0.4"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
   </section>
 
   <section name="AHRS" prefix="AHRS_">
@@ -206,68 +204,57 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <define name="H_Z" value=" 0.87161"/>
   </section>
 
- <!-- Gains for horizontal navigation-->
- <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-   <define name="PGAIN" value="100"/>
-   <define name="DGAIN" value="100"/>
-   <define name="IGAIN" value="0"/>
- </section>
-
- <section name="MISC">
-   <define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
-   <!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
-   <define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
-   <!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
-   <define name="TRANSITION_MAX_OFFSET" value="-82.0" unit="deg"/>
-   <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value=" {1,1,-1,-1,1,-1,1,1,1,1,1,1}"/>
-
-
-   <define name="USE_AIRSPEED" value="TRUE"/>
-   <define name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>
- </section>
-
- <section name="SIMULATOR" prefix="NPS_">
-   <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
-   <define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
-   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
- </section>
-
- <modules main_freq="512">
-   <load name="gps_ubx_ucenter.xml"/>
-
-   <!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
-   <load name="led_safety_status.xml">
-         <define name="USE_LED_BODY" value="1"/>
-         <define name="SAFETY_WARNING_LED" value="BODY"/>
-   </load>
-
- <!--Use an airspeed sensor and get the measured airspeed in the messages-->
-   <load name="airspeed_ets.xml">
-     <define name="AIRSPEED_ETS_SYNC_SEND"/>
-   </load>
-
- <!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section -->
-   <!--load name="gain_scheduling.xml"/-->
-</modules>
+  <!-- Gains for horizontal navigation-->
+  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
+    <define name="PGAIN" value="100"/>
+    <define name="DGAIN" value="100"/>
+    <define name="IGAIN" value="0"/>
+  </section>
+
+  <section name="MISC">
+    <define name="REF_QUAT_INFINITESIMAL_STEP" value="TRUE"/>
+    <!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
+    <define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
+    <!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
+    <define name="TRANSITION_MAX_OFFSET" value="-82.0" unit="deg"/>
+    <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value=" {1,1,-1,-1,1,-1,1,1,1,1,1,1}"/>
+
+
+    <define name="USE_AIRSPEED" value="TRUE"/>
+    <define name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>
+  </section>
+
+  <modules main_freq="512">
+    <load name="gps_ubx_ucenter.xml"/>
+
+    <!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
+    <load name="led_safety_status.xml">
+      <define name="USE_LED_BODY" value="1"/>
+      <define name="SAFETY_WARNING_LED" value="BODY"/>
+    </load>
+
+    <!--Use an airspeed sensor and get the measured airspeed in the messages-->
+    <load name="airspeed_ets.xml">
+      <define name="AIRSPEED_ETS_SYNC_SEND"/>
+    </load>
+
+    <!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section -->
+    <!--load name="gain_scheduling.xml"/-->
+  </modules>
 
   <firmware name="rotorcraft">
     <target name="ap" board="lisa_m_2.0">
-    <subsystem name="radio_control" type="spektrum">
-      <!-- Put the mode on channel AUX1-->
-      <define name="RADIO_MODE" value="RADIO_AUX1"/>
-<!--       <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
-    </subsystem>
-
- <!-- To use an airspeed sensor on I2C, enable I2C2-->
-     <define name="USE_I2C2"/>
+      <subsystem name="radio_control" type="spektrum">
+        <!-- Put the mode on channel AUX1-->
+        <define name="RADIO_MODE" value="RADIO_AUX1"/>
+        <!--       <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
+      </subsystem>
 
-    <!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
-<!--       <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/> -->
-    </target>
+      <!-- To use an airspeed sensor on I2C, enable I2C2-->
+      <define name="USE_I2C2"/>
 
-    <target name="nps" board="pc">
-      <subsystem name="fdm"           type="jsbsim"/>
-      <subsystem name="radio_control" type="ppm"/>
+      <!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
+      <!--       <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/> -->
     </target>
 
     <subsystem name="motor_mixing"/>
@@ -282,8 +269,8 @@ More information on the Quadshot can be found at transition-robotics.com -->
     <subsystem name="stabilization" type="int_quat"/>
 
     <subsystem name="ahrs"          type="int_cmpl_quat">
-      <configure name="USE_MAGNETOMETER" value="0"/>
-      <define name="AHRS_USE_GPS_HEADING" value="0"/>
+      <configure name="USE_MAGNETOMETER" value="TRUE"/>
+      <!--define name="AHRS_GRAVITY_UPDATE_COORDINATED_TURN" value="TRUE"/-->
     </subsystem>
 
     <subsystem name="ins"/>
diff --git a/conf/conf_example.xml b/conf/conf_example.xml
index f48a4237b96..532ee1d2052 100644
--- a/conf/conf_example.xml
+++ b/conf/conf_example.xml
@@ -179,6 +179,16 @@
    settings=" settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
    gui_color="blue"
   />
+  <aircraft
+   name="quadshot"
+   ac_id="19"
+   airframe="airframes/examples/quadshot_asp21_spektrum.xml"
+   radio="radios/spektrum.xml"
+   telemetry="telemetry/default_rotorcraft.xml"
+   flight_plan="flight_plans/rotorcraft_basic.xml"
+   settings=" settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml"
+   gui_color="blue"
+  />
   <aircraft
    name="setup_lisam2"
    ac_id="16"
diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml
index fbf8fb8b7b1..e2a91654a8c 100644
--- a/conf/conf_tests.xml
+++ b/conf/conf_tests.xml
@@ -289,6 +289,16 @@
    settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/control/ctl_dash_loiter_trim.xml settings/estimation/ins_neutrals.xml"
    gui_color="blue"
   />
+  <aircraft
+   name="quadshot"
+   ac_id="19"
+   airframe="airframes/examples/quadshot_asp21_spektrum.xml"
+   radio="radios/spektrum.xml"
+   telemetry="telemetry/default_rotorcraft.xml"
+   flight_plan="flight_plans/rotorcraft_basic.xml"
+   settings=" settings/rotorcraft_basic.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int_quat.xml"
+   gui_color="blue"
+  />
   <aircraft
    name="setup_lisam2"
    ac_id="16"