diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg
index bf9ab8242c..110361bee1 100644
--- a/msg/TecsStatus.msg
+++ b/msg/TecsStatus.msg
@@ -1,7 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 
 float32 altitude_sp			# Altitude setpoint AMSL [m]
-float32 altitude_filtered		# Altitude filtered AMSL [m]
+float32 altitude_sp_filtered		# Altitude setpoint filtered AMSL [m]
 float32 height_rate_setpoint		# Height rate setpoint [m/s]
 float32 height_rate			# Height rate [m/s]
 float32 equivalent_airspeed_sp		# Equivalent airpseed setpoint [m/s]
diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp
index 702bf3206e..de81e47b43 100644
--- a/src/lib/tecs/TECS.cpp
+++ b/src/lib/tecs/TECS.cpp
@@ -706,7 +706,7 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 	_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
 	_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
 	const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
-	_debug_status.altitude_sp = ref_alt.alt;
+	_debug_status.altitude_sp_ref = ref_alt.alt;
 	_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
 	_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
 
@@ -798,7 +798,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_debug_status.control = _control.getDebugOutput();
 		_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
 		_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
-		_debug_status.altitude_sp = control_setpoint.altitude_reference.alt;
+		_debug_status.altitude_sp_ref = control_setpoint.altitude_reference.alt;
 		_debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate;
 		_debug_status.altitude_rate_feedforward = control_setpoint.altitude_rate_setpoint;
 	}
diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
index 4f98ae3d31..e42242386a 100644
--- a/src/lib/tecs/TECS.hpp
+++ b/src/lib/tecs/TECS.hpp
@@ -551,7 +551,7 @@ public:
 		TECSControl::DebugOutput control;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
-		float altitude_sp;
+		float altitude_sp_ref;
 		float altitude_rate_alt_ref;
 		float altitude_rate_feedforward;
 		enum ECL_TECS_MODE tecs_mode;
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index d43a266135..ac2b97ffef 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -484,7 +484,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
 	}
 
 	t.altitude_sp = alt_sp;
-	t.altitude_filtered = debug_output.altitude_sp;
+	t.altitude_sp_filtered = debug_output.altitude_sp_ref;
 	t.height_rate_setpoint = debug_output.control.altitude_rate_control;
 	t.height_rate = -_local_pos.vz;
 	t.equivalent_airspeed_sp = equivalent_airspeed_sp;
diff --git a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp
index 6553127dc3..5f0f50e9e0 100644
--- a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp
+++ b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +36,7 @@
 
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_global_position.h>
 
 class MavlinkStreamNavControllerOutput : public MavlinkStream
 {
@@ -59,6 +60,7 @@ private:
 
 	uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)};
 	uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
 	bool send() override
 	{
@@ -69,6 +71,9 @@ private:
 			tecs_status_s tecs_status{};
 			_tecs_status_sub.copy(&tecs_status);
 
+			vehicle_global_position_s vehicle_global_position{};
+			_vehicle_global_position_sub.copy(&vehicle_global_position);
+
 			mavlink_nav_controller_output_t msg{};
 
 			msg.nav_roll = math::degrees(pos_ctrl_status.nav_roll);
@@ -77,7 +82,7 @@ private:
 			msg.target_bearing = roundf(math::degrees(pos_ctrl_status.target_bearing));
 			msg.wp_dist = math::constrain(roundf(pos_ctrl_status.wp_dist), 0.f, (float)UINT16_MAX);
 			msg.xtrack_error = pos_ctrl_status.xtrack_error;
-			msg.alt_error = tecs_status.altitude_filtered - tecs_status.altitude_sp;
+			msg.alt_error = tecs_status.altitude_sp - vehicle_global_position.alt;
 			msg.aspd_error = tecs_status.true_airspeed_filtered - tecs_status.true_airspeed_sp;
 
 			mavlink_msg_nav_controller_output_send_struct(_mavlink->get_channel(), &msg);