From 837c6ce2495e573d088999d6e738a50eefec356f Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Wed, 25 May 2011 10:34:47 +0200
Subject: [PATCH] add an option to reverse airspeed measure when you stupidely
 mix static and dynamic pressure

---
 conf/airframes/ENAC/fixed-wing/spocIII-2.xml | 30 +++++++++++---------
 sw/airborne/modules/sensors/airspeed_ets.c   |  5 ++++
 2 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
index ebe2cde45e3..2983b7f7c3c 100644
--- a/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
+++ b/conf/airframes/ENAC/fixed-wing/spocIII-2.xml
@@ -13,8 +13,10 @@
   <modules>
     <load name="ins_arduimu_basic.xml"/>
     <load name="airspeed_ets.xml">
-      <!--define name="SENSOR_SYNC_SEND"/-->
+      <define name="SENSOR_SYNC_SEND"/>
       <define name="USE_AIRSPEED"/>
+      <define name="AIRSPEED_ETS_SCALE" value="1.53"/>
+      <define name="AIRSPEED_ETS_REVERSE"/>
     </load>
   </modules>
 
@@ -35,7 +37,7 @@
     <subsystem name="navigation"/>
 
     <!-- Communication -->
-    <subsystem name="telemetry" type="transparent"/>
+    <subsystem name="telemetry" type="xbee_api"/>
     <subsystem name="radio_control" type="ppm"/>
 
     <!-- Sensors -->
@@ -54,10 +56,10 @@
 
 <!-- commands section -->
   <servos>
-    <servo name="MOTOR" no="7" min="1000" neutral="1000" max="2000"/>
-    <servo name="AILERON_LEFT" no="2" min="1900" neutral="1526" max="1100"/>
-    <servo name="AILERON_RIGHT" no="6" min="1900" neutral="1537" max="1100"/>
-    <servo name="ELEVATOR" no="0" min="1900" neutral="1500" max="1100"/>
+    <servo name="MOTOR" no="0" min="1000" neutral="1000" max="2000"/>
+    <servo name="AILERON_LEFT" no="6" min="1900" neutral="1486" max="1100"/>
+    <servo name="AILERON_RIGHT" no="2" min="1900" neutral="1400" max="1100"/>
+    <servo name="ELEVATOR" no="7" min="1900" neutral="1590" max="1100"/>
   </servos>
 
   <commands>
@@ -101,7 +103,7 @@
   </section>
  
   <section name="MISC">
-    <define name="MINIMUM_AIRSPEED" value="12." unit="m/s"/>
+    <define name="MINIMUM_AIRSPEED" value="14." unit="m/s"/>
     <define name="NOMINAL_AIRSPEED" value="17." unit="m/s"/>
     <define name="MAXIMUM_AIRSPEED" value="25." unit="m/s"/>
     <define name="CARROT" value="5." unit="s"/>
@@ -109,7 +111,7 @@
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
     <!-- define name="XBEE_INIT" value="\"ATPL2\rATRN1\rATTT80\r\""/ -->
     <!-- define name="NO_XBEE_API_INIT" value="TRUE"/ -->
-    <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
+    <define name="ALT_KALMAN_ENABLED" value="FALSE"/>
 
     <define name="TRIGGER_DELAY" value="1."/>
     <define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
@@ -130,6 +132,8 @@
     <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.85"/>
     <define name="AUTO_PITCH_MAX_PITCH" value="RadOfDeg(20.)"/>
     <define name="AUTO_PITCH_MIN_PITCH" value="-RadOfDeg(20.)"/>
+    <define name="AUTO_THROTTLE_LOITER_TRIM" value="0."/>
+    <define name="AUTO_THROTTLE_DASH_TRIM" value="0."/>
 
     <!-- Climb loop (throttle) -->
     <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
@@ -175,18 +179,18 @@
     <define name="PITCH_MIN_SETPOINT" value="-0.5" unit="radians"/>
 
     <define name="ROLL_ATTITUDE_GAIN" value="-8500."/>
-    <define name="ROLL_RATE_GAIN" value="-2700."/>
+    <define name="ROLL_RATE_GAIN" value="-2400."/>
     <define name="ROLL_IGAIN" value="-0."/>
     <define name="ROLL_KFFA" value="0"/>
     <define name="ROLL_KFFD" value="0"/>
 
-    <define name="PITCH_PGAIN" value="-8200."/>
-    <define name="PITCH_DGAIN" value="-12000."/>
-    <define name="PITCH_IGAIN" value="-0."/>
+    <define name="PITCH_PGAIN" value="-4000."/>
+    <define name="PITCH_DGAIN" value="-2400."/>
+    <define name="PITCH_IGAIN" value="-300."/>
     <define name="PITCH_KFFA" value="0."/>
     <define name="PITCH_KFFD" value="0."/>
 
-    <define name="PITCH_OF_ROLL" value="RadOfDeg(0.0)"/>
+    <define name="PITCH_OF_ROLL" value="0.043"/>
     <define name="AILERON_OF_THROTTLE" value="0.0"/>
 
   </section>
diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c
index 4921e9c6a23..1859cf5ca20 100644
--- a/sw/airborne/modules/sensors/airspeed_ets.c
+++ b/sw/airborne/modules/sensors/airspeed_ets.c
@@ -146,8 +146,13 @@ void airspeed_ets_read_event( void ) {
         airspeed_ets_offset_tmp += airspeed_ets_raw;
     }
     // Convert raw to m/s
+#ifdef AIRSPEED_ETS_REVERSE
+    if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset)
+      airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET;
+#else
     if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset)
       airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET;
+#endif
     else
       airspeed_tmp = 0.0;
     // Airspeed should always be positive