SulTee/SulTee/PilotView.swift

@@ -53,6 +53,133 @@ struct MJPEGStreamView: UIViewRepresentable {
     }
 }
 
+// MARK: - Fighter-jet compass tape HUD
+
+/// Horizontal scrolling compass tape — fighter-jet / HUD style.
+/// Current heading is always centred; cardinal labels glow yellow.
+struct CompassTapeHUD: View {
+    let heading: Double   // degrees, 0–360
+
+    // Layout constants
+    private let tapeHeight: CGFloat  = 52
+    private let pixPerDeg:  CGFloat  = 3.6   // horizontal pixels per degree
+
+    var body: some View {
+        ZStack {
+            // Frosted dark background
+            RoundedRectangle(cornerRadius: 8)
+                .fill(.black.opacity(0.50))
+                .overlay(
+                    RoundedRectangle(cornerRadius: 8)
+                        .stroke(.white.opacity(0.15), lineWidth: 0.5)
+                )
+
+            Canvas { ctx, size in
+                drawTape(ctx: ctx, size: size)
+            }
+            .clipped()
+
+            // Fixed centre triangle — points downward from the top edge
+            VStack(spacing: 0) {
+                Image(systemName: "arrowtriangle.down.fill")
+                    .font(.system(size: 9, weight: .bold))
+                    .foregroundStyle(.yellow)
+                    .shadow(color: .yellow.opacity(0.6), radius: 4)
+                Spacer()
+            }
+            .padding(.top, 1)
+
+            // Heading readout centred at bottom of tape
+            VStack {
+                Spacer()
+                Text(String(format: "%03.0f°", heading))
+                    .font(.system(size: 12, weight: .bold, design: .monospaced))
+                    .foregroundStyle(.yellow)
+                    .shadow(color: .black, radius: 2)
+            }
+            .padding(.bottom, 3)
+        }
+        .frame(height: tapeHeight)
+    }
+
+    // MARK: Canvas drawing
+
+    private func drawTape(ctx: GraphicsContext, size: CGSize) {
+        let midX  = size.width / 2
+        let tickY: CGFloat = 10   // top of tick marks
+
+        // Snap to nearest whole degree for stable per-step grid
+        let hdgInt = Int(heading.rounded())
+        // Sub-pixel phase correction so the tape glides smoothly
+        let phase  = heading - Double(hdgInt)   // fractional remainder
+
+        // Draw ±80° around current heading in 5° steps
+        for offset in stride(from: -80, through: 80, by: 5) {
+            let actualDeg = ((hdgInt + offset) % 360 + 360) % 360
+            let x = midX + (CGFloat(offset) - CGFloat(phase)) * pixPerDeg
+
+            // Skip ticks outside the visible area
+            guard x >= 0 && x <= size.width else { continue }
+
+            let isCardinal      = actualDeg % 90 == 0
+            let isIntercardinal = actualDeg % 45 == 0
+            let is30            = actualDeg % 30 == 0
+
+            let tickH: CGFloat
+            let lineW: CGFloat
+            let alpha:  Double
+            if isCardinal {
+                tickH = 18; lineW = 2.0; alpha = 1.0
+            } else if isIntercardinal {
+                tickH = 13; lineW = 1.5; alpha = 0.85
+            } else {
+                tickH = 8;  lineW = 1.0; alpha = 0.55
+            }
+
+            // Tick mark
+            var tick = Path()
+            tick.move(to:    CGPoint(x: x, y: tickY))
+            tick.addLine(to: CGPoint(x: x, y: tickY + tickH))
+            ctx.stroke(tick, with: .color(.white.opacity(alpha)), lineWidth: lineW)
+
+            // Label for cardinals, intercardinals, and every 30°
+            if isCardinal || isIntercardinal || is30 {
+                let label = compassLabel(actualDeg)
+                let color: Color = isCardinal ? .yellow : .white.opacity(0.85)
+                let size12: CGFloat = isCardinal ? 12 : 9.5
+                let weight: Font.Weight = isCardinal ? .bold : .semibold
+                ctx.draw(
+                    Text(label)
+                        .font(.system(size: size12, weight: weight, design: .monospaced))
+                        .foregroundStyle(color),
+                    at: CGPoint(x: x, y: tickY + tickH + 9),
+                    anchor: .top
+                )
+            }
+        }
+
+        // Subtle centre guide line (vertical)
+        var guide = Path()
+        guide.move(to:    CGPoint(x: midX, y: tickY))
+        guide.addLine(to: CGPoint(x: midX, y: tickY + 18))
+        ctx.stroke(guide, with: .color(.yellow.opacity(0.25)), lineWidth: 1)
+    }
+
+    private func compassLabel(_ deg: Int) -> String {
+        switch deg {
+        case 0:   return "N"
+        case 45:  return "NE"
+        case 90:  return "E"
+        case 135: return "SE"
+        case 180: return "S"
+        case 225: return "SW"
+        case 270: return "W"
+        case 315: return "NW"
+        default:  return "\(deg)"
+        }
+    }
+}
+
 // MARK: - Virtual joystick
 
 /// Single-touch virtual joystick.  Knob snaps back to center on release (dead-man).
@@ -115,6 +242,7 @@ struct PilotView: View {
 
     @State private var selectedCamIndex = 0
     @State private var joystick: CGPoint = .zero
+    @State private var showHUD = true
 
     /// 10 Hz publish timer — rate-limits cmd_vel to Orin.
     private let cmdTimer = Timer.publish(every: 0.1, on: .main, in: .common).autoconnect()
@@ -127,18 +255,49 @@ struct PilotView: View {
             MJPEGStreamView(urlString: Self.cameras[selectedCamIndex].url)
                 .ignoresSafeArea()
 
-            // Camera switcher pill — top-right (hidden when only one source)
+            // ── Top overlay bar ─────────────────────────────────────────────
+            VStack(spacing: 0) {
+                HStack(alignment: .top, spacing: 8) {
+
+                    // HUD toggle — top-left
+                    Button {
+                        withAnimation(.easeInOut(duration: 0.2)) { showHUD.toggle() }
+                    } label: {
+                        Image(systemName: showHUD ? "waveform.path.ecg.rectangle.fill"
+                                                  : "waveform.path.ecg.rectangle")
+                            .font(.system(size: 15, weight: .semibold))
+                            .foregroundStyle(showHUD ? .yellow : .white.opacity(0.7))
+                            .padding(8)
+                            .background(.ultraThinMaterial, in: RoundedRectangle(cornerRadius: 8))
+                    }
+
+                    // Compass tape — fills remaining width
+                    if showHUD, let hdg = sensor.botHeadingDeg {
+                        CompassTapeHUD(heading: hdg)
+                            .transition(.opacity.combined(with: .move(edge: .top)))
+                    } else if showHUD {
+                        // Placeholder when no heading data yet
+                        RoundedRectangle(cornerRadius: 8)
+                            .fill(.black.opacity(0.45))
+                            .frame(height: 52)
+                            .overlay(
+                                Text("NO IMU")
+                                    .font(.system(size: 11, weight: .semibold, design: .monospaced))
+                                    .foregroundStyle(.white.opacity(0.4))
+                            )
+                            .transition(.opacity.combined(with: .move(edge: .top)))
+                    }
+
+                    // Camera switcher — top-right (hidden when only one source)
                     if Self.cameras.count > 1 {
-                VStack {
-                    HStack {
-                        Spacer()
                         cameraSwitcher
+                    }
+                }
                 .padding(.top, 56)
-                            .padding(.trailing, 16)
-                    }
+                .padding(.horizontal, 16)
+
                 Spacer()
             }
-            }
 
             // Joystick overlay — bottom-right, semi-transparent
             JoystickControl(value: $joystick)

SulTee/SulTee/SensorManager.swift

@@ -58,6 +58,12 @@ final class SensorManager: NSObject, ObservableObject {
     }
     @Published var distanceSource: DistanceSource = .gps
 
+    // MARK: - Bot IMU heading (saltybot/imu)
+
+    /// Robot's magnetic heading in degrees [0, 360), received via MQTT.
+    /// Nil until the first message arrives.
+    @Published var botHeadingDeg: Double? = nil
+
     // MARK: - UWB local data (saltybot/uwb/range + saltybot/uwb/position)
 
     @Published var uwbPosition: UWBPosition? = nil
@@ -100,6 +106,7 @@ final class SensorManager: NSObject, ObservableObject {
     private static let uwbRangeTopic      = "saltybot/uwb/range"
     private static let uwbPositionTopic   = "saltybot/uwb/position"
     private static let uwbTagPosTopic     = "saltybot/uwb/tag/position"   // Orin-fused phone position
+    private static let botIMUTopic         = "saltybot/imu"
     private static let followModeTopic    = "saltybot/follow/mode"
     private static let followRangeTopic   = "saltybot/follow/range"
     private static let maxBreadcrumbs     = 60
@@ -167,6 +174,7 @@ final class SensorManager: NSObject, ObservableObject {
             case Self.uwbRangeTopic:    self.handleUWBRange(payload)
             case Self.uwbPositionTopic: self.handleUWBPosition(payload)
             case Self.uwbTagPosTopic:   self.handleUWBTagPosition(payload)
+            case Self.botIMUTopic:      self.handleBotIMU(payload)
             default: break
             }
         }
@@ -206,6 +214,7 @@ final class SensorManager: NSObject, ObservableObject {
         mqtt.subscribe(topic: Self.uwbRangeTopic)
         mqtt.subscribe(topic: Self.uwbPositionTopic)
         mqtt.subscribe(topic: Self.uwbTagPosTopic)
+        mqtt.subscribe(topic: Self.botIMUTopic)
         ensureSensorsRunning()
         startRateTimer()
         startMQTTGPSTimer()
@@ -244,9 +253,11 @@ final class SensorManager: NSObject, ObservableObject {
 
     // MARK: - MQTT: Pilot tab lifecycle + cmd_vel
 
-    /// Ensure MQTT is open. Safe to call even when Follow-Me is already streaming.
+    /// Ensure MQTT is open and bot IMU topic subscribed.
+    /// Safe to call even when Follow-Me is already streaming.
     func ensureMQTTConnected() {
         mqtt.connect()   // no-op if state != .disconnected (guarded inside MQTTClient)
+        mqtt.subscribe(topic: Self.botIMUTopic)
     }
 
     /// Tear down MQTT only if Follow-Me is not running.
@@ -376,6 +387,33 @@ final class SensorManager: NSObject, ObservableObject {
         updateBestPhonePosition()
     }
 
+    // MARK: - Bot IMU handler
+
+    /// Parse heading from the robot's IMU message (saltybot/imu).
+    /// Accepts {"heading": <degrees>}, {"true_heading": <degrees>}, or {"yaw": <value>}.
+    /// Yaw is treated as degrees if ≤ 360, radians otherwise (converted × 180/π).
+    private func handleBotIMU(_ payload: String) {
+        guard let data = payload.data(using: .utf8),
+              let json = try? JSONSerialization.jsonObject(with: data) as? [String: Any]
+        else { return }
+
+        let deg: Double?
+        if let h = json["heading"] as? Double {
+            deg = h
+        } else if let h = json["true_heading"] as? Double {
+            deg = h
+        } else if let y = json["yaw"] as? Double {
+            deg = abs(y) <= 2 * .pi ? y * 180 / .pi : y   // radians → degrees if needed
+        } else {
+            deg = nil
+        }
+
+        if let d = deg {
+            botHeadingDeg = (d.truncatingRemainder(dividingBy: 360) + 360)
+                            .truncatingRemainder(dividingBy: 360)
+        }
+    }
+
     // MARK: - Phone position source selection
 
     /// Selects the best available phone position: