FW init TECS with PSP_OFF and constrain TAS error

src/lib/external_lgpl/tecs/tecs.cpp

@@ -165,6 +165,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 	_integ4_state = _integ4_state + integ4_input * DT;
 	float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
 	_integ5_state = _integ5_state + integ5_input * DT;
+
 	// limit the airspeed to a minimum of 3 m/s
 	_integ5_state = max(_integ5_state, 3.0f);
 	_update_speed_last_usec = now;
@@ -464,14 +465,14 @@ void TECS::_update_pitch(void)
 	// During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle
 	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
 	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
-	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
+	float gainInv = _integ5_state * _timeConst * CONSTANTS_ONE_G;
 	float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
-	if (_climbOutDem)
-	{
+	if (_climbOutDem) {
 		temp += _PITCHminf * gainInv;
 	}
-	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
-
+	float integ7_err_min = (gainInv * (_PITCHminf - math::radians(5.0f))) - temp;
+	float integ7_err_max = (gainInv * (_PITCHmaxf + math::radians(5.0f))) - temp;
+	_integ7_state = constrain(_integ7_state, integ7_err_min, integ7_err_max);
 
 	// Calculate pitch demand from specific energy balance signals
 	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
@@ -493,7 +494,7 @@ void TECS::_update_pitch(void)
 	_last_pitch_dem = _pitch_dem;
 }
 
-void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
+void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS)
 {
 	// Initialise states and variables if DT > 1 second or in climbout
 	if (_update_pitch_throttle_last_usec == 0 || _DT > DT_MAX || !_in_air || !_states_initalized) {
@@ -501,18 +502,22 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
 		_integ2_state = 0.0f;
 		_integ3_state = baro_altitude;
 		_integ4_state = 0.0f;
-		_integ5_state = _EAS;
+		_integ5_state = _EAS * EAS2TAS;
 		_integ6_state = 0.0f;
 		_integ7_state = 0.0f;
+
 		_last_throttle_dem = throttle_cruise;
-		_last_pitch_dem = pitch;
+		_last_pitch_dem = constrain(pitch, _PITCHminf, _PITCHmaxf);
+		_pitch_dem_unc = _last_pitch_dem;
+
 		_hgt_dem_adj_last = baro_altitude;
 		_hgt_dem_adj = _hgt_dem_adj_last;
 		_hgt_dem_prev = _hgt_dem_adj_last;
 		_hgt_dem_in_old = _hgt_dem_adj_last;
-		_TAS_dem_last = _TAS_dem;
-		_TAS_dem_adj = _TAS_dem;
-		_pitch_dem_unc = pitch;
+
+		_TAS_dem_last = _EAS * EAS2TAS;
+		_TAS_dem_adj = _TAS_dem_last;
+
 		_underspeed = false;
 		_badDescent = false;
 
@@ -523,11 +528,14 @@ void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_alt
 	} else if (_climbOutDem) {
 		_PITCHminf          = ptchMinCO_rad;
 		_THRminf            = _THRmaxf - 0.01f;
+
 		_hgt_dem_adj_last  = baro_altitude;
 		_hgt_dem_adj       = _hgt_dem_adj_last;
 		_hgt_dem_prev      = _hgt_dem_adj_last;
-		_TAS_dem_last      = _TAS_dem;
-		_TAS_dem_adj       = _TAS_dem;
+
+		_TAS_dem_last      = _EAS * EAS2TAS;
+		_TAS_dem_adj       = _EAS * EAS2TAS;
+
 		_underspeed        = false;
 		_badDescent 	   = false;
 	}
@@ -543,9 +551,9 @@ void TECS::_update_STE_rate_lim(void)
 	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
 }
 
-void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
-				 float throttle_min, float throttle_max, float throttle_cruise,
-				 float pitch_limit_min, float pitch_limit_max)
+void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem,
+				float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
 {
 
 	// Calculate time in seconds since last update
@@ -563,7 +571,7 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	_climbOutDem = climbOutDem;
 
 	// initialise selected states and variables if DT > 1 second or in climbout
-	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
+	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO, EAS2TAS);
 
 	if (!_in_air) {
 		return;

src/lib/external_lgpl/tecs/tecs.h

@@ -456,7 +456,7 @@ private:
 	void _update_pitch(void);
 
 	// Initialise states and variables
-	void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
+	void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad, float EAS2TAS);
 
 	// Calculate specific total energy rate limits
 	void _update_STE_rate_lim(void);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -68,6 +68,7 @@
 #include <px4_posix.h>
 
 #include "landingslope.h"
+
 #include <arch/board/board.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_hrt.h>
@@ -84,7 +85,6 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/pid/pid.h>
 #include <systemlib/systemlib.h>
-#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/fw_virtual_attitude_setpoint.h>
@@ -98,7 +98,6 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
@@ -2405,7 +2404,6 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		const math::Vector<3> &ground_speed,
 		unsigned mode)
 {
-	bool run_tecs = true;
 	float dt = 0.01f; // prevent division with 0
 
 	if (_last_tecs_update > 0) {
@@ -2415,7 +2413,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	_last_tecs_update = hrt_absolute_time();
 
 	// do not run TECS if we are not in air
-	run_tecs &= !_vehicle_land_detected.landed;
+	bool run_tecs = !_vehicle_land_detected.landed;
 
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
 	// (it should also not run during VTOL blending because airspeed is too low still)
@@ -2442,14 +2440,10 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		// after transition we ramp up desired airspeed from the speed we had coming out of the transition
 		_asp_after_transition += dt * 2; // increase 2m/s
 
-
-		//throttle_cruise =
-
 		if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) {
 			v_sp = fmaxf(_asp_after_transition, _ctrl_state.airspeed);
-		}
 
-		else {
+		} else {
 			_was_in_transition = false;
 			_asp_after_transition = 0;
 		}
@@ -2479,7 +2473,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 					      || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));
 
 	/* Using tecs library */
-	float pitch_for_tecs = _pitch;
+	float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad;
 
 	// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
 	// between multirotor and fixed wing flight