diff --git a/project/instruments/pfd.gd b/project/instruments/pfd.gd
index 43790a0..bd92309 100644
--- a/project/instruments/pfd.gd
+++ b/project/instruments/pfd.gd
@@ -25,6 +25,10 @@ extends Node2D
 	set(value):
 		altitude = value
 		var step = 1
+		if value > 49.5:
+			step = 5
+		if value > 99.5:
+			step = 10
 		altitude_label.text = str(int(step * round(value / step)))
 
 @export var climbrate: float = 1000:
@@ -44,15 +48,3 @@ extends Node2D
 func update(aircraft: Transform3D, velocity: Vector3):
 	var godot_euler = aircraft.basis.get_euler()
 	euler_attitude_deg = Vector3(godot_euler.z, -godot_euler.x, -godot_euler.y) * rad_to_deg(1)
-
-	const mps_to_kt = 1.94384
-	var v_local = aircraft.basis.inverse() * velocity
-	airspeed = max(0, v_local.z) * mps_to_kt
-	var v_horizontal = Vector3(velocity.x, 0, velocity.z)
-	groundspeed = v_horizontal.length() * mps_to_kt
-
-	const mps_to_fpm = 196.848
-	climbrate = velocity.y * mps_to_fpm
-
-	const m_to_ft = 3.2808
-	altitude = aircraft.origin.y * m_to_ft
diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp
index 591cf11..5d4266e 100644
--- a/src/marshconnector.cpp
+++ b/src/marshconnector.cpp
@@ -243,12 +243,7 @@ Transform3D MarshConnector::get_aircraft() {
   return offset * marsh;
 }
 
-Vector3 MarshConnector::get_velocity() {
-  float heading_rad = parameters[NAV_OFS_HDG] * Math_PI / 180.0;
-  // To rotate clockwise looking from above, that's negative Y in Godot
-  Quaternion heading_offset = Quaternion(Vector3(0, 1, 0), -heading_rad);
-  return heading_offset.xform(last_velocity);
-}
+Vector3 MarshConnector::get_velocity() { return last_velocity; }
 
 Vector2 MarshConnector::get_cyclic() {
   return Vector2{last_controls.x, last_controls.y};
@@ -282,10 +277,6 @@ Vector2 MarshConnector::local_meters_from_global_degrees(double latitude,
   // Y East is just length along the circle of latitude
   double y = (lon - lon0) * (EARTH_RADIUS * cos(lat0));
 
-  const double map_size = 3000.0;
-  x = (Math::fract(x / map_size + 0.5) - 0.5) * map_size;
-  y = (Math::fract(y / map_size + 0.5) - 0.5) * map_size;
-
   return Vector2(x, y);
 }
 void MarshConnector::handle_sim_state(mavlink_message_t message) {