diff --git a/conf/settings/fw_h_ctl_a_settings.xml b/conf/settings/fw_h_ctl_a_settings.xml
index 45b7fca900..1058bb50ca 100644
--- a/conf/settings/fw_h_ctl_a_settings.xml
+++ b/conf/settings/fw_h_ctl_a_settings.xml
@@ -7,6 +7,7 @@
     <dl_settings NAME="fw_h_ctl_a">
       <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_Kff" module="fw_h_ctl_a"/>
       <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_roll_igain" module="fw_h_ctl_a" handler="SetRollIGain"/>
+      <dl_setting MAX="0" MIN="-5000" STEP="10" VAR="h_ctl_pitch_igain" module="fw_h_ctl_a" handler="SetPitchIGain"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/sw/airborne/fw_h_ctl_a.c b/sw/airborne/fw_h_ctl_a.c
index 3839b16cd1..232ddff6ad 100644
--- a/sw/airborne/fw_h_ctl_a.c
+++ b/sw/airborne/fw_h_ctl_a.c
@@ -68,7 +68,9 @@ float  h_ctl_roll_setpoint;
 float h_ctl_roll_attitude_gain;
 float h_ctl_roll_rate_gain;
 float  h_ctl_roll_igain;
+float  h_ctl_pitch_igain;
 float  h_ctl_roll_sum_err;
+float  h_ctl_pitch_sum_err;
 float h_ctl_roll_Kff;
 pprz_t h_ctl_aileron_setpoint;
 float  h_ctl_roll_slew;
@@ -210,6 +212,10 @@ void h_ctl_attitude_loop ( void ) {
 #define H_CTL_REF_DT (1./60.)
 
 inline static void h_ctl_roll_loop( void ) {
+
+  if (pprz_mode == PPRZ_MODE_MANUAL)
+    h_ctl_roll_sum_err = 0;
+
   h_ctl_ref_roll_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_roll_setpoint - h_ctl_ref_roll_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_roll_rate;
   h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
   h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
@@ -277,9 +283,16 @@ inline static void h_ctl_pitch_loop( void ) {
 #endif
 
   float err = estimator_theta - h_ctl_ref_pitch_angle;
+
+  if (pprz_mode == PPRZ_MODE_MANUAL)
+    h_ctl_pitch_sum_err = 0;
+  h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
+  BoundAbs(h_ctl_pitch_sum_err, H_CTL_ROLL_SUM_ERR_MAX);
+
+
   float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
   last_err = err;
-  float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err;
+  float cmd = h_ctl_pitch_pgain * err + h_ctl_pitch_dgain * d_err + h_ctl_pitch_igain * h_ctl_pitch_sum_err;
 
   cmd /= airspeed_ratio2;
 
diff --git a/sw/airborne/fw_h_ctl_a.h b/sw/airborne/fw_h_ctl_a.h
index 07d592fc79..55c26b21ca 100644
--- a/sw/airborne/fw_h_ctl_a.h
+++ b/sw/airborne/fw_h_ctl_a.h
@@ -37,7 +37,9 @@
 #include "airframe.h"
 
 extern float h_ctl_roll_sum_err;
+extern float h_ctl_pitch_sum_err;
 extern float h_ctl_roll_igain;
+extern float h_ctl_pitch_igain;
 extern float h_ctl_roll_Kff;
 #define H_CTL_ROLL_SUM_ERR_MAX 100.
 
@@ -46,6 +48,11 @@ extern float h_ctl_roll_Kff;
   h_ctl_roll_igain = _gain; \
   }
 
+#define fw_h_ctl_a_SetPitchIGain(_gain) { \
+  h_ctl_pitch_sum_err = 0; \
+  h_ctl_pitch_igain = _gain; \
+  }
+
 /* inner roll loop parameters */
 extern float h_ctl_ref_roll_angle;
 extern float h_ctl_ref_roll_rate;