From e0c88983f131120dc7aff56047295374ba3f8bcd Mon Sep 17 00:00:00 2001 From: sl-ios Date: Tue, 7 Apr 2026 08:32:10 -0400 Subject: [PATCH] =?UTF-8?q?feat:=20Pilot=20tab=20=E2=80=94=20HUD=20compass?= =?UTF-8?q?=20tape=20from=20saltybot/imu=20heading?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - CompassTapeHUD: fighter-jet style horizontal scrolling tape, smooth sub-pixel phase correction, cardinal labels (N/E/S/W) in yellow, intercardinals (NE/SE/SW/NW), numeric ticks every 30°; fixed yellow ▼ centre indicator; degree readout (e.g. 327°) - Toggle button (top-left, waveform icon) shows/hides HUD with fade+slide animation; defaults to on; yellow when active - "NO IMU" placeholder when saltybot/imu data not yet received - SensorManager: subscribe saltybot/imu in startStreaming() and ensureMQTTConnected(); handleBotIMU() parses heading / true_heading (degrees) or yaw (auto-detects radians vs degrees); normalises to [0, 360); exposes as @Published var botHeadingDeg: Double? Co-Authored-By: Claude Sonnet 4.6 --- SulTee/SulTee/PilotView.swift | 179 ++++++++++++++++++++++++++++-- SulTee/SulTee/SensorManager.swift | 40 ++++++- 2 files changed, 208 insertions(+), 11 deletions(-) diff --git a/SulTee/SulTee/PilotView.swift b/SulTee/SulTee/PilotView.swift index b2746c6..71cd97b 100644 --- a/SulTee/SulTee/PilotView.swift +++ b/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,17 +255,48 @@ struct PilotView: View { MJPEGStreamView(urlString: Self.cameras[selectedCamIndex].url) .ignoresSafeArea() - // Camera switcher pill — top-right (hidden when only one source) - if Self.cameras.count > 1 { - VStack { - HStack { - Spacer() - cameraSwitcher - .padding(.top, 56) - .padding(.trailing, 16) + // ── 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 { + cameraSwitcher } - Spacer() } + .padding(.top, 56) + .padding(.horizontal, 16) + + Spacer() } // Joystick overlay — bottom-right, semi-transparent @@ -152,7 +311,7 @@ struct PilotView: View { angularZ: Float(-joystick.x) // drag left → positive angular_z (counter-cw) ) } - .onAppear { sensor.ensureMQTTConnected() } + .onAppear { sensor.ensureMQTTConnected() } .onDisappear { // Safety: publish a stop command and release MQTT if Follow-Me isn't using it. joystick = .zero diff --git a/SulTee/SulTee/SensorManager.swift b/SulTee/SulTee/SensorManager.swift index 96959cf..c659adf 100644 --- a/SulTee/SulTee/SensorManager.swift +++ b/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": }, {"true_heading": }, or {"yaw": }. + /// 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: