TECS: add feedforward gain for total energy balance rate

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/lib/tecs/TECS.cpp

@@ -436,7 +436,8 @@ void TECS::_update_pitch_setpoint()
 	}
 
 	// Calculate a specific energy correction that doesn't include the integrator contribution
-	float SEB_rate_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + SEB_rate_setpoint;
+	float SEB_rate_correction = _SEB_rate_error * _pitch_damping_gain + _pitch_integ_state + _SEB_rate_ff *
+				    SEB_rate_setpoint;
 
 	// During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle
 	// demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator

src/lib/tecs/TECS.hpp

@@ -131,6 +131,8 @@ public:
 	void set_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; }
 	void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; }
 
+	void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
+
 
 	// TECS status
 	uint64_t timestamp() { return _pitch_update_timestamp; }
@@ -224,6 +226,7 @@ private:
 	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
 	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
 	float _speed_derivative_time_const{0.01f};			///< speed derivative filter time constant (s)
+	float _SEB_rate_ff{1.0f};
 
 	// complimentary filter states
 	float _vert_vel_state{0.0f};					///< complimentary filter state - height rate (m/sec)