Merge pull request #79 from dagar/integrator_fix

fw multiply integrator gain in loop

attitude_fw/ecl_pitch_controller.cpp

@@ -181,12 +181,12 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 			id = math::min(id, 0.0f);
 		}
 
-		_integrator += id;
+		_integrator += id * _k_i;
 	}
 
 	/* integrator limit */
 	//xxx: until start detection is available: integral part in control signal is limited here
-	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
 	_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +

attitude_fw/ecl_roll_controller.cpp

@@ -128,12 +128,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 			id = math::min(id, 0.0f);
 		}
 
-		_integrator += id;
+		_integrator += id * _k_i;
 	}
 
 	/* integrator limit */
 	//xxx: until start detection is available: integral part in control signal is limited here
-	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */

attitude_fw/ecl_wheel_controller.cpp

@@ -100,12 +100,12 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
             id = math::min(id, 0.0f);
         }
 
-        _integrator += id;
+        _integrator += id * _k_i;
     }
 
     /* integrator limit */
     //xxx: until start detection is available: integral part in control signal is limited here
-    float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+    float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
 
     /* Apply PI rate controller and store non-limited output */
     _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +

attitude_fw/ecl_yaw_controller.cpp

@@ -217,12 +217,12 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
 			id = math::min(id, 0.0f);
 		}
 
-		_integrator += id;
+		_integrator += id * _k_i;
 	}
 
 	/* integrator limit */
 	//xxx: until start detection is available: integral part in control signal is limited here
-	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	float integrator_constrained = math::constrain(_integrator, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
 	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *