[paparazzi-commits] [4304] Added aggressive climb mode and climb limiter when USE_AIRSPEED is enabled .

[Vassilis V.]

Revision: 4304
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4304
Author:   vassilis
Date:     2009-10-31 04:13:12 +0000 (Sat, 31 Oct 2009)
Log Message:
-----------
Added aggressive climb mode and climb limiter when USE_AIRSPEED is enabled.
Kalman filter parameters have been adjusted for use with the ETS barometric 
sensor.

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/easystar2.xml
    paparazzi3/trunk/sw/airborne/airspeed_ets.c
    paparazzi3/trunk/sw/airborne/baro_ets.c
    paparazzi3/trunk/sw/airborne/fw_h_ctl.c
    paparazzi3/trunk/sw/airborne/fw_v_ctl.c

Modified: paparazzi3/trunk/conf/airframes/easystar2.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/easystar2.xml       2009-10-30 23:15:38 UTC 
(rev 4303)
+++ paparazzi3/trunk/conf/airframes/easystar2.xml       2009-10-31 04:13:12 UTC 
(rev 4304)
@@ -83,7 +83,7 @@
 
   <section name="BAT">
     <define name="MILLIAMP_AT_FULL_THROTTLE" value="20000" unit="mA"/>
-    <define name="CATASTROPHIC_BAT_LEVEL" value="7.5" unit="V"/>
+    <define name="CATASTROPHIC_BAT_LEVEL" value="8.0" unit="V"/>
     <define name="CRITIC_BAT_LEVEL" value="8.5" unit="V"/>
     <define name="LOW_BAT_LEVEL" value="9.5" unit="V"/>
     <define name="MAX_BAT_LEVEL" value="12.5" unit="V"/>
@@ -91,8 +91,8 @@
 
   <section name="MISC">
     <define name="NOMINAL_AIRSPEED" value="15." unit="m/s"/>
-    <define name="CARROT" value="3." unit="s"/>
-    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
+    <define name="CARROT" value="4." unit="s"/>
+    <define name="KILL_MODE_DISTANCE" value="(2.0*MAX_DIST_FROM_HOME)"/>
     <define name="CONTROL_RATE" value="60" unit="Hz"/>
     <define name="NO_XBEE_API_INIT" value="TRUE"/> 
     <define name="ALT_KALMAN_ENABLED" value="TRUE"/>
@@ -117,14 +117,14 @@
     <define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
     <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05" 
unit="rad/(m/s)"/>
     <!-- auto airspeed and altitude inner loop -->
-    <define name="AUTO_AIRSPEED_SETPOINT" value="17.5" unit="m/s"/>
+    <define name="AUTO_AIRSPEED_SETPOINT" value="14.5" unit="m/s"/>
     <define name="AUTO_AIRSPEED_PGAIN" value="0.060"/>
     <define name="AUTO_AIRSPEED_IGAIN" value="0.050"/>    
-    <define name="AUTO_GROUNDSPEED_SETPOINT" value="5" unit="m/s"/>
+    <define name="AUTO_GROUNDSPEED_SETPOINT" value="6.0" unit="m/s"/>
     <define name="AUTO_GROUNDSPEED_PGAIN" value="0.75"/>
     <define name="AUTO_GROUNDSPEED_IGAIN" value="0.25"/>     
-    <define name="AUTO_PITCH_PGAIN" value="-0.182"/>
-    <define name="AUTO_PITCH_IGAIN" value="0.010"/>
+    <define name="AUTO_PITCH_PGAIN" value="-0.125"/>
+    <define name="AUTO_PITCH_IGAIN" value="0.025"/>
     <define name="AUTO_PITCH_MAX_PITCH" value="RadOfDeg(25)"/>
     <define name="AUTO_PITCH_MIN_PITCH" value="RadOfDeg(-25)"/>
     <define name="THROTTLE_SLEW" value="1.0"/>
@@ -149,7 +149,7 @@
 
   <section name="AGGRESSIVE" prefix="AGR_">
     <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate 
Aggressive Climb CANNOT BE ZERO!!-->
-    <define name="BLEND_END" value="8"/><!-- Altitude Error to Blend 
Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
+    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend 
Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
     <define name="CLIMB_THROTTLE" value="0.70"/><!-- Gaz for Aggressive Climb 
-->
     <define name="CLIMB_PITCH" value="RadOfDeg(18)"/><!-- Pitch for Aggressive 
Climb -->
     <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive 
Decent -->
@@ -161,7 +161,7 @@
   <section name="FAILSAFE" prefix="FAILSAFE_">
     <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
     <define name="DEFAULT_THROTTLE" value="0.4" unit="%"/>
-    <define name="DEFAULT_ROLL" value="RadOfDeg(20)" unit="rad"/>
+    <define name="DEFAULT_ROLL" value="RadOfDeg(15)" unit="rad"/>
     <define name="DEFAULT_PITCH" value="RadOfDeg(0)" unit="rad"/>
     <define name="HOME_RADIUS" value="90" unit="m"/>
   </section>
@@ -171,10 +171,6 @@
     <define name="DEVICE_ADDRESS" value="...."/>
   </section>  
 
-  <section name="SIMU">
-    <define name="YAW_RESPONSE_FACTOR" value="0.5"/>
-  </section>
-
   <makefile>
 CONFIG = \"tiny_2_1_1.h\"
 
@@ -215,7 +211,7 @@
 ap.srcs += infrared.c estimator.c
 
 # Control loops
-ap.CFLAGS += -DNAV -DLOITER_TRIM
+ap.CFLAGS += -DNAV -DLOITER_TRIM 
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c 
 
 ap.srcs += nav_line.c
@@ -227,14 +223,14 @@
 #ap.srcs += cam.c point.c
 
 # EagleTree sensors (altimeter and airspeed)
-ap.CFLAGS += -DUSE_AIRSPEED_ETS -DUSE_AIRSPEED -DUSE_BARO_ETS -DUSE_I2C0
+ap.CFLAGS += -DUSE_AIRSPEED_ETS -DUSE_AIRSPEED -DUSE_BARO_ETS -DUSE_I2C0 
-DAGR_CLIMB 
 ap.srcs += airspeed.c airspeed_ets.c baro_ets.c i2c.c $(SRC_ARCH)/i2c_hw.c
 
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
-sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DLOITER_TRIM -DALT_KALMAN 
-DUSE_MODULES
+sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DLOITER_TRIM -DALT_KALMAN 
-DUSE_MODULES 
 sim.srcs += nav_line.c nav_survey_rectangle.c 
-sim.CFLAGS += -DUSE_AIRSPEED_ETS -DUSE_AIRSPEED -DUSE_BARO_ETS -DUSE_I2C0
+sim.CFLAGS += -DUSE_AIRSPEED_ETS -DUSE_AIRSPEED -DUSE_BARO_ETS -DUSE_I2C0 
-DAGR_CLIMB 
 sim.srcs += airspeed.c airspeed_ets.c baro_ets.c i2c.c $(SRC_ARCH)/i2c_hw.c
 
   </makefile>

Modified: paparazzi3/trunk/sw/airborne/airspeed_ets.c
===================================================================
--- paparazzi3/trunk/sw/airborne/airspeed_ets.c 2009-10-30 23:15:38 UTC (rev 
4303)
+++ paparazzi3/trunk/sw/airborne/airspeed_ets.c 2009-10-31 04:13:12 UTC (rev 
4304)
@@ -5,7 +5,6 @@
  * Notes:
  * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained 
together.
  * Sensor should be in the proprietary mode (default) and not in 3rd party 
mode.
- * Aggressive climb mode (AGR_CLIMB) has not been tested.
  * See conf/airframes/easystar2.xml for a configuration example.
  *
  * Sensor module wire assignments:
@@ -98,7 +97,7 @@
 
 void airspeed_ets_periodic( void ) {
   int n;
-  float airspeed_tmp;
+  float airspeed_tmp = 0.0;
 
   // Read raw value
   if (i2c0_status == I2C_IDLE) {
@@ -110,6 +109,8 @@
     else
       airspeed_ets_valid = TRUE;
   }
+  else
+    airspeed_ets_valid = FALSE;
   // Continue only if a new airspeed value was received  
   if (airspeed_ets_valid) {  
     // Calculate offset average if not done already

Modified: paparazzi3/trunk/sw/airborne/baro_ets.c
===================================================================
--- paparazzi3/trunk/sw/airborne/baro_ets.c     2009-10-30 23:15:38 UTC (rev 
4303)
+++ paparazzi3/trunk/sw/airborne/baro_ets.c     2009-10-31 04:13:12 UTC (rev 
4304)
@@ -5,7 +5,6 @@
  * Notes:
  * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained 
together.
  * Sensor should be in the proprietary mode (default) and not in 3rd party 
mode.
- * Aggressive climb mode (AGR_CLIMB) has not been tested with the barometric 
altitude.
  * Pitch gains may need to be updated.
  * See conf/airframes/easystar2.xml for a configuration example.
  *
@@ -51,6 +50,8 @@
 #define BARO_ETS_OFFSET_MIN 10
 #define BARO_ETS_OFFSET_NBSAMPLES_INIT 20
 #define BARO_ETS_OFFSET_NBSAMPLES_AVRG 40
+#define BARO_ETS_R 0.5
+#define BARO_ETS_SIGMA2 0.1
 
 // Global variables
 uint16_t baro_ets_adc;
@@ -79,8 +80,8 @@
   baro_ets_enabled = TRUE;
   baro_ets_updated = FALSE;
   baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + 
BARO_ETS_OFFSET_NBSAMPLES_AVRG;
-  baro_ets_r = 20.0;
-  baro_ets_sigma2 = 1.0;
+  baro_ets_r = BARO_ETS_R;
+  baro_ets_sigma2 = BARO_ETS_SIGMA2;
   i2c0_buf[0] = 0;
   i2c0_buf[1] = 0;  
 }
@@ -132,13 +133,12 @@
   } else {
     baro_ets_altitude = 0.0;
   }
-  // New value available
-  baro_ets_updated = TRUE;
 #else // SITL
   baro_ets_adc = 0;
   baro_ets_altitude = gps_alt / 100.0;
   baro_ets_valid = TRUE;
+#endif
+  // New value available
   baro_ets_updated = TRUE;
-#endif
 }
 

Modified: paparazzi3/trunk/sw/airborne/fw_h_ctl.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_h_ctl.c     2009-10-30 23:15:38 UTC (rev 
4303)
+++ paparazzi3/trunk/sw/airborne/fw_h_ctl.c     2009-10-31 04:13:12 UTC (rev 
4304)
@@ -266,7 +266,7 @@
 
 
 
-#if defined AGR_CLIMB
+#if defined(AGR_CLIMB) && !defined(USE_AIRSPEED)
   /** limit navigation during extreme altitude changes */
   if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent 
divide by zero, reversed or negative values */
     if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE || 
V_CTL_AUTO_THROTTLE_BLENDED) {

Modified: paparazzi3/trunk/sw/airborne/fw_v_ctl.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_v_ctl.c     2009-10-30 23:15:38 UTC (rev 
4303)
+++ paparazzi3/trunk/sw/airborne/fw_v_ctl.c     2009-10-31 04:13:12 UTC (rev 
4304)
@@ -90,6 +90,8 @@
 float v_ctl_auto_groundspeed_sum_err;
 #define V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR 200
 #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
+#define V_CTL_AUTO_CLIMB_LIMIT 0.5/4.0 // m/s/s
+#define V_CTL_AUTO_AGR_CLIMB_GAIN 2.0 // altitude gain multiplier while in 
aggressive climb mode
 #endif
 
 
@@ -106,9 +108,7 @@
   /* inner loops */
   v_ctl_climb_setpoint = 0.;
   v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;
-#ifdef AGR_CLIMB
   v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD;
-#endif
 
   /* "auto throttle" inner loop parameters */
   v_ctl_auto_throttle_nominal_cruise_throttle = 
V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
@@ -150,8 +150,19 @@
  * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode 
  */
 void v_ctl_altitude_loop( void ) {
+  float altitude_pgain_boost = 1.0;
+
+#if defined(USE_AIRSPEED) && defined(AGR_CLIMB)
+  // Aggressive climb mode (boost gain of altitude loop)
+  if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) {
+    float dist = fabs(v_ctl_altitude_error);
+    altitude_pgain_boost = 1.0 + 
(V_CTL_AUTO_AGR_CLIMB_GAIN-1.0)*(dist-AGR_BLEND_END)/(AGR_BLEND_START-AGR_BLEND_END);
+    Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN);
+  }
+#endif
+
   v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
-  v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error
+  v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * 
v_ctl_altitude_error
     + v_ctl_altitude_pre_climb;
   BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
 
@@ -258,6 +269,13 @@
   float controlled_throttle;
   float v_ctl_pitch_of_vz;
 
+  // Limit rate of change of climb setpoint (to ensure that airspeed loop can 
catch-up)
+  static float v_ctl_climb_setpoint_last = 0;
+  float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last;
+  Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT);
+  v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb;
+  v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
+
   // Pitch control (input: rate of climb error, output: pitch setpoint)
   float err  = estimator_z_dot - v_ctl_climb_setpoint;
   v_ctl_auto_pitch_sum_err += err;
@@ -283,6 +301,7 @@
   BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
   controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * 
v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
 
+  // Done, set outputs
   f_throttle = controlled_throttle;
   nav_pitch = v_ctl_pitch_of_vz;
   v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);