fix tecs status logging

msg/tecs_status.msg

@@ -18,9 +18,12 @@ float32 airspeed_filtered
 float32 airspeedDerivativeSp
 float32 airspeedDerivative
 
-float32 totalEnergyRateSp
-float32 totalEnergyRate
-float32 energyDistributionRateSp
-float32 energyDistributionRate
+float32 totalEnergyError
+float32 energyDistributionError
+float32 totalEnergyRateError
+float32 energyDistributionRateError
+
+float32 throttle_sp
+float32 pitch_sp
 
 uint8 mode

src/lib/external_lgpl/tecs/tecs.cpp

@@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 	// Calculate total energy values
 	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
 	float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
-	float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
+	_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
 
 	// Apply 0.5 second first order filter to STEdot_error
 	// This is required to remove accelerometer noise from the  measurement
-	STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
-	_STEdotErrLast = STEdot_error;
+	_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
+	_STEdotErrLast = _STEdot_error;
 
 	// Calculate throttle demand
 	// If underspeed condition is set, then demand full throttle
@@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		}
 
 		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
-		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
 		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// Rate limit PD + FF throttle
@@ -438,11 +438,11 @@ void TECS::_update_pitch(void)
 	// Calculate Specific Energy Balance demand, and error
 	float SEB_dem      = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
 	float SEBdot_dem   = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
-	float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
-	float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
+	_SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
+	_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
 
 	// Calculate integrator state, constraining input if pitch limits are exceeded
-	float integ7_input = SEB_error * _integGain;
+	float integ7_input = _SEB_error * _integGain;
 
 	if (_pitch_dem_unc > _PITCHmaxf) {
 		integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
@@ -460,7 +460,7 @@ void TECS::_update_pitch(void)
 	// 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 temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
 	if (_climbOutDem)
 	{
 		temp += _PITCHminf * gainInv;
@@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 		_tecs_state.mode = ECL_TECS_MODE_NORMAL;
 	}
 
-	_tecs_state.hgt_dem  = _hgt_dem_adj;
-	_tecs_state.hgt      = _integ3_state;
-	_tecs_state.dhgt_dem = _hgt_rate_dem;
-	_tecs_state.dhgt     = _integ2_state;
-	_tecs_state.spd_dem  = _TAS_dem_adj;
-	_tecs_state.spd      = _integ5_state;
-	_tecs_state.dspd     = _vel_dot;
-	_tecs_state.ithr     = _integ6_state;
-	_tecs_state.iptch    = _integ7_state;
-	_tecs_state.thr      = _throttle_dem;
-	_tecs_state.ptch     = _pitch_dem;
-	_tecs_state.dspd_dem = _TAS_rate_dem;
+	_tecs_state.altitude_sp = _hgt_dem_adj;
+	_tecs_state.altitude_filtered = _integ3_state;
+	_tecs_state.altitude_rate_sp = _hgt_rate_dem;
+	_tecs_state.altitude_rate = _integ2_state;
+
+	_tecs_state.airspeed_sp = _TAS_dem_adj;
+	_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
+	_tecs_state.airspeed_filtered = _integ5_state;
+	_tecs_state.airspeed_rate = _vel_dot;
+
+	_tecs_state.total_energy_error = _STE_error;
+	_tecs_state.energy_distribution_error = _SEB_error;
+	_tecs_state.total_energy_rate_error = _STEdot_error;
+	_tecs_state.energy_distribution_error = _SEBdot_error;
+
+	_tecs_state.energy_error_integ = _integ6_state;
+	_tecs_state.energy_distribution_error_integ = _integ7_state;
+
+
+	_tecs_state.throttle_sp = _throttle_dem;
+	_tecs_state.pitch_sp = _pitch_dem;
 
 	_update_pitch_throttle_last_usec = now;
 

src/lib/external_lgpl/tecs/tecs.h

@@ -79,6 +79,10 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
+		_STE_error(0.0f),
+		_STEdot_error(0.0f),
+		_SEB_error(0.0f),
+		_SEBdot_error(0.0f),
 		_airspeed_enabled(false),
 		_states_initalized(false),
 		_in_air(false),
@@ -137,18 +141,22 @@ public:
 
 	struct tecs_state {
 		uint64_t timestamp;
-		float hgt;
-		float dhgt;
-		float hgt_dem;
-		float dhgt_dem;
-		float spd_dem;
-		float spd;
-		float dspd;
-		float ithr;
-		float iptch;
-		float thr;
-		float ptch;
-		float dspd_dem;
+		float altitude_filtered;
+		float altitude_sp;
+		float altitude_rate;
+		float altitude_rate_sp;
+		float airspeed_filtered;
+		float airspeed_sp;
+		float airspeed_rate;
+		float airspeed_rate_sp;
+		float energy_error_integ;
+		float energy_distribution_error_integ;
+		float total_energy_error;
+		float total_energy_rate_error;
+		float energy_distribution_error;
+		float energy_distribution_rate_error;
+		float throttle_sp;
+		float pitch_sp;
 		enum ECL_TECS_MODE mode;
 	};
 
@@ -376,6 +384,15 @@ private:
 	// Specific energy error quantities
 	float _STE_error;
 
+	// Energy error rate
+	float _STEdot_error;
+
+	// Specific energy balance error
+	float _SEB_error;
+
+	// Specific energy balance error rate
+	float _SEBdot_error;
+
 	// Time since last update of main TECS loop (seconds)
 	float _DT;
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 				break;
 		}
 
-		t.altitudeSp			= s.hgt_dem;
-		t.altitude_filtered		= s.hgt;
-		t.airspeedSp			= s.spd_dem;
-		t.airspeed_filtered		= s.spd;
-
-		t.flightPathAngleSp		= s.dhgt_dem;
-		t.flightPathAngle		= s.dhgt;
-		t.flightPathAngleFiltered	= s.dhgt;
-
-		t.airspeedDerivativeSp		= s.dspd_dem;
-		t.airspeedDerivative		= s.dspd;
-
-		t.totalEnergyRateSp		= s.thr;
-		t.totalEnergyRate		= s.ithr;
-		t.energyDistributionRateSp	= s.ptch;
-		t.energyDistributionRate	= s.iptch;
+		t.altitudeSp 		= s.altitude_sp;
+		t.altitude_filtered = s.altitude_filtered;
+		t.airspeedSp 		= s.airspeed_sp;
+		t.airspeed_filtered = s.airspeed_filtered;
+
+		t.flightPathAngleSp 		= s.altitude_rate_sp;
+		t.flightPathAngle 			= s.altitude_rate;
+		t.flightPathAngleFiltered 	= s.altitude_rate;
+
+		t.airspeedDerivativeSp 	= s.airspeed_rate_sp;
+		t.airspeedDerivative 	= s.airspeed_rate;
+
+		t.totalEnergyError 				= s.total_energy_error;
+		t.totalEnergyRateError 			= s.total_energy_rate_error;
+		t.energyDistributionError 		= s.energy_distribution_error;
+		t.energyDistributionRateError 	= s.energy_distribution_rate_error;
+		
+		t.throttle_sp 	= s.throttle_sp;
+		t.pitch_sp 		= s.pitch_sp;
 
 		if (_tecs_status_pub > 0) {
 			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);

src/modules/sdlog2/sdlog2.c

@@ -1863,15 +1863,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
-			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
-			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
-			log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
-			log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
-			log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
+			log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
+			log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
+			log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
+			log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
+			log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp;
+			log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp;
 			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
 			LOGBUFFER_WRITE_AND_COUNT(TECS);
 		}

src/modules/sdlog2/sdlog2_messages.h

@@ -360,16 +360,16 @@ struct log_TECS_s {
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
-	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
-
-	float totalEnergyRateSp;
-	float totalEnergyRate;
-	float energyDistributionRateSp;
-	float energyDistributionRate;
+	float totalEnergyError;
+	float totalEnergyRateError;
+	float energyDistributionError;
+	float energyDistributionRateError;
+	float throttle_sp;
+	float pitch_sp;
 
 	uint8_t mode;
 };
@@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 	LOG_FORMAT(ENCD, "qfqf",	"cnt0,vel0,cnt1,vel1"),
 	LOG_FORMAT(TSYN, "Q", 		"TimeOffset"),