add an option to reverse airspeed measure when you stupidely mix static

and dynamic pressure
paparazzi · May 25, 2011 · 837c6ce · 837c6ce
1 parent 0f73cd4
commit 837c6ce
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 13 deletions.
diff --git 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,15 +103,15 @@
   </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"/>
     <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
     <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
@@ -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