TECS: log more TECS states to enable better analysis

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

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
 	 *