fw_tecs: Support tighter altitude tracking during low-height flight (P…

…X4#23519) * fw_tecs: Support tighter altitude tracking during low-height flight. Added FW_T_THR_LOW_HGT defining low-height flight threshold * tecs: Applied smoothed-out altitude TC transition to landings * fw_tecs: modified tighter altitude control for low-height implementation * addressed PR comments * addressed PR comments --------- Co-authored-by: Silvan Fuhrer <[email protected]>
iq-motion-control · Sep 19, 2024 · 2ecffff · 2ecffff
1 parent 4d83bad
commit 2ecffff
Show file tree

Hide file tree

Showing 5 changed files with 143 additions and 21 deletions.
diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg
@@ -2,6 +2,7 @@ uint64 timestamp		# time since system start (microseconds)
 
 float32 altitude_sp			# Altitude setpoint AMSL [m]
 float32 altitude_reference		# Altitude setpoint reference AMSL [m]
+float32 altitude_time_constant		# Time constant of the altitude tracker [s]
 float32 height_rate_reference		# Height rate setpoint reference [m/s]
 float32 height_rate_direct		# Direct height rate setpoint from velocity reference generator [m/s]
 float32 height_rate_setpoint		# Height rate setpoint [m/s]

diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
@@ -651,6 +651,14 @@ class TECS
 	float get_pitch_setpoint() {return _control.getPitchSetpoint();}
 	float get_throttle_setpoint() {return _control.getThrottleSetpoint();}
 
+	/**
+	 * Returns the altitude tracking time constant
+	 */
+	float get_altitude_error_time_constant() const
+	{
+		return 1.0f / math::max(_control_param.altitude_error_gain, 0.01f);
+	}
+
 	uint64_t timestamp() { return _update_timestamp; }
 	float get_underspeed_ratio() { return _control.getRatioUndersped(); }