some additional for the energy_ctl

groundspeedprotection
no v_ctl_airspeed_setpoint below STALL_AIRSPEED
use of V_CTL_ALTITUDE_MAX_CLIMB
and
V_CTLAUTO_PITCH_AIRSPEED_gains can be set from airframe.xml

settings file for energyadaptive
energyadaptive.xml

conf/settings/control/ctl_energy.xml

@@ -50,9 +50,13 @@
         <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_total_igain" shortname="I_en_tot" param="V_CTL_ENERGY_TOT_IGAIN"/>
         <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_diff_pgain" shortname="P_en_dis" param="V_CTL_ENERGY_DIFF_PGAIN"/>
         <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_diff_igain" shortname="I_en_dis" param="V_CTL_ENERGY_DIFF_IGAIN"/>
-
       </dl_settings>
 
+      <dl_settings name="groundspeed">
+        <dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_groundspeed_setpoint" shortname="gs_sp"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_pgain" shortname="gs_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_igain" shortname="gs_igain"/>
+      </dl_settings>
 <!--
 
        <dl_settings NAME="agr">

conf/settings/control/ctl_energyadaptive.xml

@@ -0,0 +1,124 @@
+<!DOCTYPE settings SYSTEM "../settings.dtd">
+
+<!-- A conf to use to tune a new A/C -->
+
+<settings>
+  <dl_settings>
+    <dl_settings NAME="control">
+
+      <dl_settings NAME="trim">
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_roll_trim" shortname="roll_trim" module="inter_mcu" param="COMMAND_ROLL_TRIM"/>
+        <dl_setting MAX="960" MIN="-960" STEP="1" VAR="ap_state->command_pitch_trim" shortname="pitch_trim" param="COMMAND_PITCH_TRIM"/>
+      </dl_settings>
+
+
+
+      <dl_settings NAME="attitude">
+        <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_attitude_gain" shortname="roll_pgain" param="H_CTL_ROLL_ATTITUDE_GAIN" module="stabilization/stabilization_attitude"/>
+        <dl_setting MAX="15000" MIN="0" STEP="250" VAR="h_ctl_roll_rate_gain" shortname="roll_dgain" param="H_CTL_ROLL_RATE_GAIN" module="stabilization/stabilization_attitude"/>
+        <dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_roll_igain" shortname="roll_igain" param="H_CTL_ROLL_IGAIN" handler="SetRollIGain" module="stabilization/stabilization_adaptive"/>
+        <dl_setting MAX="25000" MIN="0" STEP="250" VAR="h_ctl_pitch_pgain" shortname="pitch_pgain" param="H_CTL_PITCH_PGAIN" module="stabilization/stabilization_attitude"/>
+        <dl_setting MAX="50000" MIN="0" STEP="250" VAR="h_ctl_pitch_dgain" shortname="pitch_dgain" param="H_CTL_PITCH_DGAIN" module="stabilization/stabilization_attitude"/>
+        <dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_pitch_igain" shortname="pitch_igain" param="H_CTL_PITCH_IGAIN" handler="SetPitchIGain" module="stabilization/stabilization_adaptive"/>
+        <dl_setting MAX=".3" MIN="0." STEP="0.001" VAR="h_ctl_pitch_of_roll" shortname="pitch_of_roll" param="H_CTL_PITCH_OF_ROLL" module="stabilization/stabilization_attitude"/>
+        <dl_setting MAX="5000" MIN="0" STEP="100" VAR="h_ctl_aileron_of_throttle" shortname="aileron_of_throttle" module="stabilization/stabilization_adaptive"/>
+
+        <dl_setting MAX="60" MIN="0" STEP="1" VAR="h_ctl_roll_max_setpoint" shortname="max_roll" param="H_CTL_ROLL_MAX_SETPOINT" unit="rad" alt_unit="deg"/>
+        <dl_setting MAX="1" MIN="0" STEP="1" var="use_airspeed_ratio" values="FALSE|TRUE"/>
+      </dl_settings>
+
+
+      <dl_settings NAME="feedforward">
+        <dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_roll_Kffa" shortname="roll_Kffa" param="H_CTL_ROLL_KFFA"/>
+        <dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_roll_Kffd" shortname="roll_Kffd" param="H_CTL_ROLL_KFFD"/>
+        <dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_pitch_Kffa" shortname="pitch_Kffa" param="H_CTL_PITCH_KFFA"/>
+        <dl_setting MAX="5000" MIN="0" STEP="10" VAR="h_ctl_pitch_Kffd" shortname="pitch_Kffd" param="H_CTL_PITCH_KFFD"/>
+      </dl_settings>
+
+
+      <dl_settings name="energy_loop">
+        <dl_setting MAX="45" MIN="8.0" STEP="0.5" VAR="v_ctl_auto_airspeed_setpoint" shortname="airspeed sp" />
+
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_nominal_cruise_throttle" shortname="cruise throttle" />
+        <dl_setting MAX="1" MIN="-1" STEP="0.05" VAR="v_ctl_auto_throttle_nominal_cruise_pitch" shortname="cruise pitch" />
+        <dl_setting MAX="1.0" MIN="0" STEP="0.01" VAR="v_ctl_desired_acceleration" shortname="acc_cmd" />
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
+
+        <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_of_airspeed_pgain" shortname="P_th_air"/>
+        <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_throttle_of_airspeed_igain" shortname="I_th_air" />
+        <dl_setting MAX="1." MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_of_airspeed_pgain" shortname="P_pitch_air" />
+        <dl_setting MAX="0.1" MIN="0" STEP="0.001" VAR="v_ctl_auto_pitch_of_airspeed_igain" shortname="I_pitch_air" />
+        <dl_setting MAX="5." MIN="0" STEP="0.01" VAR="v_ctl_auto_pitch_of_airspeed_dgain" shortname="D_pitch_air" />
+
+        <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_total_pgain" shortname="P_en_tot" param="V_CTL_ENERGY_TOT_PGAIN"/>
+        <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_total_igain" shortname="I_en_tot" param="V_CTL_ENERGY_TOT_IGAIN"/>
+        <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_diff_pgain" shortname="P_en_dis" param="V_CTL_ENERGY_DIFF_PGAIN"/>
+        <dl_setting MAX="1." MIN="0" STEP="0.01" VAR="v_ctl_energy_diff_igain" shortname="I_en_dis" param="V_CTL_ENERGY_DIFF_IGAIN"/>
+      </dl_settings>
+
+      <dl_settings name="groundspeed">
+        <dl_setting max="40" min="5" step="0.1" var="v_ctl_auto_groundspeed_setpoint" shortname="gs_sp"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_pgain" shortname="gs_pgain"/>
+        <dl_setting max="2" min="0" step="0.01" var="v_ctl_auto_groundspeed_igain" shortname="gs_igain"/>
+      </dl_settings>
+
+      <dl_settings name="climb_accel">
+        <dl_setting MAX="2.0" MIN="0" STEP="0.01" VAR="v_ctl_altitude_pgain" shortname="alt_pgain" module="guidance/energy_ctrl" param="V_CTL_ALTITUDE_PGAIN"/>
+        <dl_setting MAX="1.0" MIN="0" STEP="0.01" VAR="v_ctl_airspeed_pgain" shortname="speed_pgain"  param="V_CTL_AIRSPEED_PGAIN"/>
+        <dl_setting MAX="10.0" MIN="0" STEP="0.5" VAR="v_ctl_max_climb" shortname="max_climb"  param="V_CTL_MAX_CLIMB"/>
+        <dl_setting MAX="1.0" MIN="0" STEP="0.05" VAR="v_ctl_max_acceleration" shortname="max_acc_g"  param="V_CTL_MAX_ACCELERATION"/>
+      </dl_settings>
+
+<!--
+
+       <dl_settings NAME="agr">
+          <dl_setting MAX="1.0" MIN="0." STEP="0.05" VAR="agr_climb_throttle" shortname="climb_throttle" param="AGR_CLIMB_THROTTLE"/>
+          <dl_setting MAX="0.5" MIN="-0.1" STEP="0.05" VAR="agr_climb_pitch" shortname="climb_pitch" param="AGR_CLIMB_PITCH"/>
+          <dl_setting MAX="1.0" MIN="0." STEP="0.1" VAR="agr_climb_nav_ratio" shortname="climb_nav_ratio" param="AGR_CLIMB_NAV_RATIO"/>
+          <dl_setting MAX="1.0" MIN="0." STEP="0.05" VAR="agr_descent_throttle" shortname="descent_throttle" param="AGR_DESCENT_THROTTLE"/>
+          <dl_setting MAX="0.1" MIN="-0.5" STEP="0.05" VAR="agr_descent_pitch" shortname="descent_ptich" param="AGR_DESCENT_PITCH"/>
+          <dl_setting MAX="1.0" MIN="0." STEP="0.1" VAR="agr_descent_nav_ratio" shortname="descent_nav_ratio" param="AGR_DESCENT_NAV_RATIO"/>
+       </dl_settings>
+
+      <dl_settings name="auto_throttle">
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_cruise_throttle" shortname="cruise throttle" handler="SetCruiseThrottle" param="V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE">
+          <strip_button name="Loiter" value="0.1" group="dash_loiter"/>
+          <strip_button name="Cruise" value="0" group="dash_loiter"/>
+          <strip_button name="Dash" value="1" group="dash_loiter"/>
+        </dl_setting>
+        <dl_setting MAX="0.05" MIN="0.00" STEP="0.005" VAR="v_ctl_auto_throttle_pgain" shortname="throttle_pgain" param="V_CTL_AUTO_THROTTLE_PGAIN"/>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="v_ctl_auto_throttle_igain" shortname="throttle_igain" param="V_CTL_AUTO_THROTTLE_IGAIN"/>
+        <dl_setting MAX="2" MIN="0.0" STEP="0.1" VAR="v_ctl_auto_throttle_dgain" shortname="throttle_dgain"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_climb_throttle_increment" shortname="throttle_incr" param="V_CTL_AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_throttle_pitch_of_vz_pgain" shortname="pitch_of_vz" param="V_CTL_AUTO_THROTTLE_PITCH_OF_VZ_PGAIN"/>
+        <dl_setting MAX="10" MIN="-10" STEP="0.1" VAR="v_ctl_auto_throttle_pitch_of_vz_dgain" shortname="pitch_of_vz (d)"/>
+      </dl_settings>
+
+      <dl_settings name="auto_pitch">
+        <dl_setting MAX="0.1" MIN="0.01" STEP="0.01" VAR="v_ctl_auto_pitch_pgain" shortname="pgain" param="V_CTL_AUTO_PITCH_PGAIN"/>
+        <dl_setting MAX="1" MIN="0" STEP="0.01" VAR="v_ctl_auto_pitch_igain" shortname="igain" param="V_CTL_AUTO_PITCH_IGAIN"/>
+      </dl_settings>
+
+-->
+
+      <dl_settings name="nav">
+        <dl_setting MAX="3" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pgain" shortname="course pgain" param="H_CTL_COURSE_PGAIN"/>
+        <dl_setting MAX="2" MIN="0" STEP="0.1" VAR="h_ctl_course_dgain" shortname="course dgain" param="H_CTL_COURSE_DGAIN"/>
+        <dl_setting MAX="2" MIN="0.1" STEP="0.05" VAR="h_ctl_course_pre_bank_correction" shortname="pre bank cor" param="H_CTL_COURSE_PRE_BANK_CORRECTION"/>
+        <dl_setting MAX="1" MIN="0.0" STEP="0.05" VAR="nav_glide_pitch_trim" shortname="glide pitch trim" param="NAV_GLIDE_PITCH_TRIM"/>
+        <dl_setting MAX="1" MIN="0.02" STEP="0.01" VAR="h_ctl_roll_slew" shortname="roll slew"/>
+        <dl_setting MAX="500" MIN="-500" STEP="5" VAR="nav_radius"/>
+        <dl_setting MAX="359" MIN="0" STEP="5" VAR="nav_course"/>
+        <dl_setting MAX="2" MIN="1" STEP="1" VAR="nav_mode"/>
+        <dl_setting MAX="5" MIN="-5" STEP="0.5" VAR="nav_climb"/>
+        <dl_setting MAX="15" MIN="-15" STEP="1" VAR="fp_pitch"/>
+        <dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="subsystems/nav" handler="IncreaseShift" shortname="inc. shift"/>
+        <dl_setting MAX="50" MIN="5" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
+        <dl_setting MAX="0.2" MIN="0" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
+        <dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
+      </dl_settings>
+
+    </dl_settings>
+  </dl_settings>
+</settings>

sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2006  Pascal Brisset, Antoine Drouin, Michel Gorraz
+ * Copyright (C) 2012 TUDelft, Tobias Muench
  *
  * This file is part of paparazzi.
  *
@@ -47,15 +47,15 @@
  *
  *  Pseudo-Control Unit = dimensionless acceleration [g]
  *
- *  	- pitch <-> pseudocontrol:    sin(Theta) steers Vdot in [g]
- *  	- throttle <-> pseudocontrol: motor characteristic as function of V x throttle steeds VDot
+ *      - pitch <-> pseudocontrol:    sin(Theta) steers Vdot in [g]
+ *      - throttle <-> pseudocontrol: motor characteristic as function of V x throttle steeds VDot
  *
  *  @dot
  *  digraph total_energy_control {
- *  	node [shape=record];
- *  	b [label="attitude loop" URL="\ref attitude_loop"];
- *  	c [label="climb loop" URL="\ref v_ctl_climb_loop"];
- *  	b -> c [ arrowhead="open", style="dashed" ];
+ *      node [shape=record];
+ *      b [label="attitude loop" URL="\ref attitude_loop"];
+ *      c [label="climb loop" URL="\ref v_ctl_climb_loop"];
+ *      b -> c [ arrowhead="open", style="dashed" ];
  *  }
  *  @enddot
  *
@@ -78,8 +78,6 @@ uint8_t v_ctl_mode = V_CTL_MODE_MANUAL;
 uint8_t v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;
 uint8_t v_ctl_auto_throttle_submode = V_CTL_CLIMB_MODE_AUTO_THROTTLE;
 float v_ctl_auto_throttle_sum_err = 0;
-float v_ctl_auto_airspeed_controlled = 0;
-float v_ctl_auto_groundspeed_setpoint = 0;
 
 #ifdef LOITER_TRIM
 #error "Energy Controller can not accep Loiter Trim"
@@ -96,9 +94,7 @@ float v_ctl_altitude_pgain;
 float v_ctl_airspeed_pgain;
 float v_ctl_altitude_error;    ///< in meters, (setpoint - alt) -> positive = too low
 
-float v_ctl_auto_airspeed_setpoint; ///< in meters per second
-
-float v_ctl_max_climb = 2;
+float v_ctl_max_climb;
 float v_ctl_max_acceleration = 0.5;
 
 /* inner loop */
@@ -125,6 +121,19 @@ float v_ctl_energy_total_igain;
 float v_ctl_energy_diff_pgain;
 float v_ctl_energy_diff_igain;
 
+float v_ctl_auto_airspeed_setpoint; ///< in meters per second
+float v_ctl_auto_airspeed_setpoint_slew;
+#ifndef AIRSPEED_SETPOINT_SLEW
+#define AIRSPEED_SETPOINT_SLEW 1
+#endif
+float v_ctl_auto_airspeed_controlled;
+#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
+float v_ctl_auto_groundspeed_setpoint; ///< in meters per second
+float v_ctl_auto_groundspeed_pgain;
+float v_ctl_auto_groundspeed_igain;
+float v_ctl_auto_groundspeed_sum_err;
+#define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100
+#endif
 
 pprz_t v_ctl_throttle_setpoint;
 pprz_t v_ctl_throttle_slewed;
@@ -190,6 +199,8 @@ void v_ctl_init( void ) {
 #endif
 
   v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
+  v_ctl_auto_airspeed_setpoint_slew = v_ctl_auto_airspeed_setpoint;
+
 
   /* inner loops */
   v_ctl_climb_setpoint = 0.;
@@ -207,13 +218,38 @@ void v_ctl_init( void ) {
   v_ctl_auto_throttle_of_airspeed_igain = V_CTL_AUTO_THROTTLE_OF_AIRSPEED_IGAIN;
 #endif
 
+#ifdef V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN
+  v_ctl_auto_pitch_of_airspeed_pgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_PGAIN;
+  v_ctl_auto_pitch_of_airspeed_igain = V_CTL_AUTO_PITCH_OF_AIRSPEED_IGAIN;
+  v_ctl_auto_pitch_of_airspeed_dgain = V_CTL_AUTO_PITCH_OF_AIRSPEED_DGAIN;
+#endif
+
+
 #ifdef V_CTL_ENERGY_TOT_PGAIN
   v_ctl_energy_total_pgain = V_CTL_ENERGY_TOT_PGAIN;
   v_ctl_energy_total_igain = V_CTL_ENERGY_TOT_IGAIN;
   v_ctl_energy_diff_pgain = V_CTL_ENERGY_DIFF_PGAIN;
   v_ctl_energy_diff_igain = V_CTL_ENERGY_DIFF_IGAIN;
 #endif
 
+#ifdef V_CTL_ALTITUDE_MAX_CLIMB
+  v_ctl_max_climb = V_CTL_ALTITUDE_MAX_CLIMB;
+#else
+  v_ctl_max_climb = 2;
+#warning "V_CTL_ALTITUDE_MAX_CLIMB not defined - default is 2m/s"
+#endif
+
+#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
+  v_ctl_auto_groundspeed_setpoint = V_CTL_AUTO_GROUNDSPEED_SETPOINT;
+  v_ctl_auto_groundspeed_pgain = V_CTL_AUTO_GROUNDSPEED_PGAIN;
+  v_ctl_auto_groundspeed_igain = V_CTL_AUTO_GROUNDSPEED_IGAIN;
+  v_ctl_auto_groundspeed_sum_err = 0.;
+#endif
+
+#ifndef STALL_AIRSPEED
+#define STALL_AIRSPEED NOMINAL_AIRSPEED
+#endif
+
   v_ctl_throttle_setpoint = 0;
 }
 
@@ -225,7 +261,7 @@ void v_ctl_init( void ) {
 void v_ctl_altitude_loop( void )
 {
   // Imput Checks
-  if (v_ctl_auto_airspeed_setpoint <= 0.0f) v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED;
+  if (v_ctl_auto_airspeed_setpoint <= STALL_AIRSPEED * 1.23) v_ctl_auto_airspeed_setpoint = STALL_AIRSPEED * 1.23;
 
   // Altitude Controller
   v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
@@ -261,8 +297,32 @@ static float low_pass_vdot(float v)
 
 void v_ctl_climb_loop( void )
 {
+#ifdef AIRSPEED_SETPOINT_SLEW
+  // airspeed_setpoint ratelimiter:
+  float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew; // FIXME
+  BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * NOMINAL_AIRSPEED);
+  v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
+#endif
+
+#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
+ // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
+  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - (*stateGetHorizontalSpeedNorm_f()));
+  v_ctl_auto_groundspeed_sum_err += err_groundspeed;
+  BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
+  v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;
+
+  // Do not allow controlled airspeed below the setpoint
+  if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) {
+    v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint;
+    // reset integrator of ground speed loop
+    v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain);
+  }
+#else
+    v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
+#endif
+
   // Airspeed outerloop: positive means we need to accelerate
-  float speed_error = v_ctl_auto_airspeed_setpoint - (*stateGetAirspeed_f());
+  float speed_error = v_ctl_auto_airspeed_controlled - (*stateGetAirspeed_f());
 
   // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
   v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
@@ -293,10 +353,9 @@ void v_ctl_climb_loop( void )
   if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB))
   {
     v_ctl_auto_throttle_nominal_cruise_throttle +=
-        	  v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt
-		+ en_tot_err * v_ctl_energy_total_igain * dt;
-    if (v_ctl_auto_throttle_nominal_cruise_throttle < 0.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 0.0f;
-    else if (v_ctl_auto_throttle_nominal_cruise_throttle > 1.0f) v_ctl_auto_throttle_nominal_cruise_throttle = 1.0f;
+        v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt
+      + en_tot_err * v_ctl_energy_total_igain * dt;
+    Bound(v_ctl_auto_throttle_nominal_cruise_throttle,0.0f,1.0f);
   }
 
   // Total Controller
@@ -320,13 +379,14 @@ void v_ctl_climb_loop( void )
     Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
   }
   float v_ctl_pitch_of_vz =
-		+ (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
-		- v_ctl_auto_pitch_of_airspeed_pgain * speed_error
+        + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
+        - v_ctl_auto_pitch_of_airspeed_pgain * speed_error
                 + v_ctl_auto_pitch_of_airspeed_dgain * vdot
                 + v_ctl_energy_diff_pgain * en_dis_err
                 + v_ctl_auto_throttle_nominal_cruise_pitch;
 
-  v_ctl_pitch_setpoint = v_ctl_pitch_of_vz;
+  nav_pitch = v_ctl_pitch_of_vz;
+  Bound(nav_pitch,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT)
 
   ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
 

sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012 TUDelft
+ * Copyright (C) 2012 TUDelft, Tobias Muench
  *
  * This file is part of paparazzi.
  *
@@ -62,6 +62,10 @@ extern float v_ctl_energy_total_igain;
 extern float v_ctl_energy_diff_pgain;
 extern float v_ctl_energy_diff_igain;
 
+extern float v_ctl_auto_groundspeed_pgain;
+extern float v_ctl_auto_groundspeed_igain;
+extern float v_ctl_auto_groundspeed_sum_err;
+
 /////////////////////////////////////////////////
 // Automatically found airplane characteristics