Wrap horizontal aircraft position to map extents

project/instruments/pfd.gd

@@ -25,10 +25,6 @@ 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:
@@ -48,3 +44,15 @@ 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

src/marshconnector.cpp

@@ -243,7 +243,12 @@ Transform3D MarshConnector::get_aircraft() {
   return offset * marsh;
 }
 
-Vector3 MarshConnector::get_velocity() { return last_velocity; }
+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);
+}
 
 Vector2 MarshConnector::get_cyclic() {
   return Vector2{last_controls.x, last_controls.y};
@@ -277,6 +282,10 @@ 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) {