TECS: log more TECS states to enable better analysis

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

msg/tecs_status.msg

@@ -22,7 +22,19 @@ float32 energy_distribution_error
 float32 total_energy_rate_error
 float32 energy_distribution_rate_error
 
+float32 total_energy
+float32 total_energy_rate
+float32 total_energy_balance
+float32 total_energy_balance_rate
+
+float32 total_energy_sp
+float32 total_energy_rate_sp
+float32 total_energy_balance_sp
+float32 total_energy_balance_rate_sp
+
 float32 throttle_integ
 float32 pitch_integ
 
+float32 throttle_sp
+
 uint8 mode

src/lib/tecs/TECS.cpp

@@ -624,3 +624,43 @@ float TECS::get_SKE_weighting()
 
 	return SKE_weighting;
 }
+
+float TECS::SEB()
+{
+	const float SKE_weighting = get_SKE_weighting();
+
+	// Calculate the weighting applied to control of specific potential energy error
+	const float SPE_weighting = 2.0f - SKE_weighting;
+
+	return _SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting;
+}
+
+float TECS::SEB_setpoint()
+{
+	const float SKE_weighting = get_SKE_weighting();
+
+	// Calculate the weighting applied to control of specific potential energy error
+	const float SPE_weighting = 2.0f - SKE_weighting;
+
+	return _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting;
+}
+
+float TECS::SEB_rate()
+{
+	const float SKE_weighting = get_SKE_weighting();
+
+	// Calculate the weighting applied to control of specific potential energy error
+	const float SPE_weighting = 2.0f - SKE_weighting;
+
+	return _SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting;
+}
+
+float TECS::SEB_rate_setpoint()
+{
+	const float SKE_weighting = get_SKE_weighting();
+
+	// Calculate the weighting applied to control of specific potential energy error
+	const float SPE_weighting = 2.0f - SKE_weighting;
+
+	return _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting;
+}

src/lib/tecs/TECS.hpp

@@ -157,6 +157,22 @@ public:
 	float throttle_integ_state() { return _throttle_integ_state; }
 	float pitch_integ_state() { return _pitch_integ_state; }
 
+	float STE() { return _SPE_estimate + _SKE_estimate; }
+
+	float STE_setpoint() { return _SPE_setpoint + _SKE_setpoint; }
+
+	float STE_rate() { return _SPE_rate + _SKE_rate; }
+
+	float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
+
+	float SEB();
+
+	float SEB_setpoint();
+
+	float SEB_rate();
+
+	float SEB_rate_setpoint();
+
 	/**
 	 * Handle the altitude reset
 	 *

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -357,9 +357,21 @@ FixedwingPositionControl::tecs_status_publish()
 	t.energy_distribution_error = _tecs.SEB_error();
 	t.energy_distribution_rate_error = _tecs.SEB_rate_error();
 
+	t.total_energy = _tecs.STE();
+	t.total_energy_rate = _tecs.STE_rate();
+	t.total_energy_balance = _tecs.SEB();
+	t.total_energy_balance_rate = _tecs.SEB_rate();
+
+	t.total_energy_sp = _tecs.STE_setpoint();
+	t.total_energy_rate_sp = _tecs.STE_rate_setpoint();
+	t.total_energy_balance_sp = _tecs.SEB_setpoint();
+	t.total_energy_balance_rate_sp = _tecs.SEB_rate_setpoint();
+
 	t.throttle_integ = _tecs.throttle_integ_state();
 	t.pitch_integ = _tecs.pitch_integ_state();
 
+	t.throttle_sp = _tecs.get_throttle_setpoint();
+
 	t.timestamp = hrt_absolute_time();
 
 	_tecs_status_pub.publish(t);