reintroduce feedforward

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -53,6 +53,7 @@ ECL_PitchController::ECL_PitchController() :
 	_k_p(0.0f),
 	_k_i(0.0f),
 	_k_d(0.0f),
+	_k_ff(0.0f),
 	_integrator_max(0.0f),
 	_max_rate_pos(0.0f),
 	_max_rate_neg(0.0f),
@@ -173,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 //	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 //	warnx("roll: _last_output %.4f", (double)_last_output);
 	return math::constrain(_last_output, -1.0f, 1.0f);

src/lib/ecl/attitude_fw/ecl_pitch_controller.h

@@ -73,21 +73,30 @@ public:
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
 	void set_k_d(float k_d) {
 		_k_d = k_d;
 	}
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
+	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate_pos(float max_rate_pos) {
 		_max_rate_pos = max_rate_pos;
 	}
+
 	void set_max_rate_neg(float max_rate_neg) {
 		_max_rate_neg = max_rate_neg;
 	}
+
 	void set_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
@@ -111,6 +120,7 @@ private:
 	float _k_p;
 	float _k_i;
 	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate_pos;
 	float _max_rate_neg;

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -53,6 +53,7 @@ ECL_RollController::ECL_RollController() :
 	_k_p(0.0f),
 	_k_i(0.0f),
 	_k_d(0.0f),
+	_k_ff(0.0f),
 	_integrator_max(0.0f),
 	_max_rate(0.0f),
 	_last_output(0.0f),
@@ -141,7 +142,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }

src/lib/ecl/attitude_fw/ecl_roll_controller.h

@@ -71,18 +71,27 @@ public:
 			_tc = time_constant;
 		}
 	}
+
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
+
 	void set_k_d(float k_d) {
 		_k_d = k_d;
 	}
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
+	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate(float max_rate) {
 		_max_rate = max_rate;
 	}
@@ -105,6 +114,7 @@ private:
 	float _k_p;
 	float _k_i;
 	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
 	float _last_output;

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -51,6 +51,7 @@ ECL_YawController::ECL_YawController() :
 	_k_p(0.0f),
 	_k_i(0.0f),
 	_k_d(0.0f),
+	_k_ff(0.0f),
 	_integrator_max(0.0f),
 	_max_rate(0.0f),
 	_roll_ff(0.0f),
@@ -159,7 +160,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler;  //scaler is proportional to 1/airspeed
 	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
 

src/lib/ecl/attitude_fw/ecl_yaw_controller.h

@@ -69,22 +69,32 @@ public:
 
 	void set_k_p(float k_p) {
 			_k_p = k_p;
-		}
+	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
+
 	void set_k_d(float k_d) {
 		_k_d = k_d;
 	}
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
+	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate(float max_rate) {
 		_max_rate = max_rate;
 	}
+
 	void set_k_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
+
 	void set_coordinated_min_speed(float coordinated_min_speed) {
 		_coordinated_min_speed = coordinated_min_speed;
 	}
@@ -107,6 +117,7 @@ private:
 	float _k_p;
 	float _k_i;
 	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
 	float  _roll_ff;