From 447e14906c4c63243d844929f7fea8588a474deb Mon Sep 17 00:00:00 2001
From: RomanBapst <bapstroman@gmail.com>
Date: Tue, 22 Sep 2020 11:50:51 +0300
Subject: [PATCH] TECS: log more TECS states to enable better analysis

Signed-off-by: RomanBapst <bapstroman@gmail.com>
---
 msg/tecs_status.msg                           | 12 ++++++
 src/lib/tecs/TECS.cpp                         | 40 +++++++++++++++++++
 src/lib/tecs/TECS.hpp                         | 16 ++++++++
 .../FixedwingPositionControl.cpp              | 12 ++++++
 4 files changed, 80 insertions(+)

diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg
index a13f34cdce..5a87d26cae 100644
--- a/msg/tecs_status.msg
+++ b/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
diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp
index 404e93cf5b..fa5bc8f570 100644
--- a/src/lib/tecs/TECS.cpp
+++ b/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;
+}
diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
index 0110428129..9e5943c8aa 100644
--- a/src/lib/tecs/TECS.hpp
+++ b/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
 	 *
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 699dbd1413..9d7d91d550 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/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);