diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp
index 627a84dc..e7a1ef26 100644
--- a/clover/src/simple_offboard.cpp
+++ b/clover/src/simple_offboard.cpp
@@ -620,7 +620,7 @@ void publishState()
 	} else {
 		msg.x = setpoint_position.point.x;
 		msg.y = setpoint_position.point.y;
-		msg.z = setpoint_position.point.z;
+		msg.z = setpoint_altitude.point.z;
 	}
 
 	msg.speed = nav_speed;