stream directions to car directly

gearslip/src/main/java/app/seb3thehacker/gearslip/GearslipRunner.kt

@@ -4,6 +4,7 @@ import android.os.Build
 import app.seb3thehacker.gearslip.audio.AudioLink
 import app.seb3thehacker.gearslip.audio.CarMic
 import app.seb3thehacker.gearslip.media.CarMediaStatus
+import app.seb3thehacker.gearslip.car.ClusterNav
 import app.seb3thehacker.gearslip.car.CarAssistant
 import app.seb3thehacker.gearslip.car.CarEnvironment
 import app.seb3thehacker.gearslip.car.CarKeys
@@ -70,6 +71,7 @@ class GearslipRunner(
     @Volatile private var inputKeycodes: List<Int> = emptyList()
     @Volatile private var sensorChannelId: Int = -1
     @Volatile private var mediaStatus: CarMediaStatus? = null
+    @Volatile private var clusterNav: ClusterNav? = null
     private var sensorTypes: List<Int> = emptyList()
     @Volatile private var audioLink: AudioLink? = null
     private var audioMessagesSeen = 0
@@ -145,6 +147,8 @@ class GearslipRunner(
         CarMic.current = null
         mediaStatus?.stop()
         mediaStatus = null
+        clusterNav?.stop()
+        clusterNav = null
         lastSensorNight = null
         CarEnvironment.setSensorNight(null) // back to the clock until a car says otherwise
         projection?.onProjectionStopped()
@@ -180,6 +184,8 @@ class GearslipRunner(
                 onInputMessage(messageId, body)
             } else if (frame.channel == sensorChannelId) {
                 onSensorMessage(messageId, body)
+            } else if (frame.channel == clusterNav?.channelId) {
+                clusterNav?.onMessage(messageId, body)
             } else if (frame.channel == mediaStatus?.channelId) {
                 mediaStatus?.onMessage(messageId, body)
             } else if (frame.channel == CarMic.current?.channelId) {
@@ -424,6 +430,7 @@ class GearslipRunner(
         startSensorChannel(serviceDiscoveryResponse)
         startMicChannel(serviceDiscoveryResponse)
         startMediaStatusChannel(serviceDiscoveryResponse)
+        startClusterNavChannel(serviceDiscoveryResponse)
 
         val input = ServiceDiscovery.findInputService(serviceDiscoveryResponse, video.displayId)
         if (input == null) {
@@ -496,6 +503,17 @@ class GearslipRunner(
         openChannel(channel)
     }
 
+    /** Turn-by-turn for the instrument cluster: the next turn, its road, and how far. */
+    private fun startClusterNavChannel(serviceDiscoveryResponse: ByteArray) {
+        val channel = ServiceDiscovery.findNavStatusChannel(serviceDiscoveryResponse) ?: run {
+            log.i("no navigation status channel - the cluster won't show turns")
+            return
+        }
+        log.i("navigation status channel: $channel")
+        clusterNav = ClusterNav(channel) { id, body -> send(id, body, encrypted = true, channel = channel) }
+        openChannel(channel)
+    }
+
     /**
      * Every sensor type the head unit advertised, subscribed to at once - there is no screen
      * to pick and choose yet, this is meant to show everything reachable. [CarSensors] answers

gearslip/src/main/java/app/seb3thehacker/gearslip/ServiceDiscovery.kt

@@ -114,10 +114,15 @@ object ServiceDiscovery {
      * The channel the car takes "now playing" on (MediaPlaybackStatusService, Service field 9),
      * which feeds its cluster and media screens. It carries no settings. Null when the car has none.
      */
-    fun findMediaStatusChannel(response: ByteArray): Int? =
+    fun findMediaStatusChannel(response: ByteArray): Int? = findChannelWith(response, 9)
+
+    /** The instrument cluster's turn-by-turn channel (NavigationStatusService, Service field 8). */
+    fun findNavStatusChannel(response: ByteArray): Int? = findChannelWith(response, 8)
+
+    private fun findChannelWith(response: ByteArray, serviceField: Int): Int? =
         Wire.allBytes(Wire.fields(response), 1).firstNotNullOfOrNull { channel ->
             val service = Wire.fields(channel)
-            if (service.any { it.number == 9 }) Wire.varint(service, 1)?.toInt() else null
+            if (service.any { it.number == serviceField }) Wire.varint(service, 1)?.toInt() else null
         }
 
     /**

gearslip/src/main/java/app/seb3thehacker/gearslip/car/CarServices.kt

@@ -33,6 +33,9 @@ object CarServices {
     val nav: CarAppConnection by lazy { CarAppConnection(requireNotNull(context)) }
     val media: CarMedia by lazy { CarMedia(requireNotNull(context)) }
 
+    /** [nav], or null before the car screen has set things up. */
+    fun navOrNull(): CarAppConnection? = if (context != null) nav else null
+
     /** [media], or null before the car screen has set things up: for code outside the car UI. */
     fun mediaOrNull(): CarMedia? = if (context != null) media else null
 

gearslip/src/main/java/app/seb3thehacker/gearslip/car/ClusterNav.kt

@@ -0,0 +1,196 @@
+package app.seb3thehacker.gearslip.car
+
+import androidx.car.app.model.CarText
+import androidx.car.app.model.Distance
+import androidx.car.app.navigation.model.Maneuver
+import androidx.car.app.navigation.model.NavigationTemplate
+import androidx.car.app.navigation.model.RoutingInfo
+import androidx.car.app.navigation.model.Step
+import app.seb3thehacker.gearslip.GearslipLog
+import app.seb3thehacker.gearslip.Protobuf
+
+/**
+ * The next turn on the car's instrument cluster, over its NavigationStatusService (Service
+ * field 8), the way Android Auto feeds it:
+ *
+ *   -> NavigationStatus         { status = 1 (1 active, 2 inactive, 3 rerouting) }
+ *   -> NavigationNextTurnEvent  { road = 1; turn_side = 2; event = 3; turn_number = 5; turn_angle = 6 }
+ *   -> NavigationNextTurnDistanceEvent { distance_meters = 1; time_to_turn_seconds = 2;
+ *                                        display_distance_e3 = 3; display_distance_unit = 4 }
+ *
+ * Newer cars also take a NavigationState message; the deprecated pair above is what a 2018
+ * Uconnect understands. The turn comes from the map app's trip (what the Car App Library hands
+ * to clusters), or from its routing card when it sends no trip.
+ */
+class ClusterNav(
+    val channelId: Int,
+    private val sendOnChannel: (messageId: Int, body: ByteArray) -> Unit,
+) {
+    @Volatile private var running = false
+    private var sentStatus = -1
+    private var sentTurn: String? = null
+    private var sentDistance: String? = null
+
+    fun onMessage(messageId: Int, body: ByteArray) {
+        when (messageId) {
+            MSG_CHANNEL_OPEN_RESPONSE -> {
+                val status = Protobuf.readInt32Field(body, 1) ?: 0
+                GearslipLog.i("cluster: <- ChannelOpenResponse: status=$status")
+                if (status == 0) start()
+            }
+            // The car saying when it wants turns. Sending all along is harmless, so these are only logged.
+            MSG_START -> GearslipLog.i("cluster: <- start")
+            MSG_STOP -> GearslipLog.i("cluster: <- stop")
+            else -> GearslipLog.i("cluster: <- unhandled message id=$messageId (${body.size} bytes)")
+        }
+    }
+
+    private fun start() {
+        if (running) return
+        running = true
+        Thread({
+            while (running) {
+                runCatching { tick() }.onFailure { GearslipLog.w("cluster: ${it.message}") }
+                Thread.sleep(TICK_MS)
+            }
+        }, "gearslip-cluster").start()
+    }
+
+    fun stop() {
+        running = false
+    }
+
+    /** One turn as the car wants it, from whichever source the map app fills. */
+    private class Turn(val step: Step, val distance: Distance?, val seconds: Long?, val rerouting: Boolean)
+
+    private fun current(): Turn? {
+        val nav = CarServices.navOrNull() ?: return null
+        nav.trip.value?.let { trip ->
+            val step = trip.steps.firstOrNull() ?: return@let
+            val estimate = trip.stepTravelEstimates.firstOrNull()
+            return Turn(step, estimate?.remainingDistance, estimate?.remainingTimeSeconds?.takeIf { it >= 0 }, trip.isLoading)
+        }
+        val info = (nav.template.value as? NavigationTemplate)?.navigationInfo as? RoutingInfo ?: return null
+        val step = info.currentStep ?: return null
+        return Turn(step, info.currentDistance, null, info.isLoading)
+    }
+
+    private fun tick() {
+        val turn = current()
+        val status = when {
+            turn == null -> STATUS_INACTIVE
+            turn.rerouting -> STATUS_REROUTING
+            else -> STATUS_ACTIVE
+        }
+        if (status != sentStatus) {
+            sendOnChannel(MSG_STATUS, Protobuf.varintField(1, status.toLong()))
+            GearslipLog.i("cluster: -> ${STATUS_NAMES[status]}")
+            sentStatus = status
+            if (status == STATUS_INACTIVE) { sentTurn = null; sentDistance = null }
+        }
+        if (turn == null || status == STATUS_REROUTING) return
+        sendTurn(turn.step)
+        turn.distance?.let { sendDistance(it, turn.seconds) }
+    }
+
+    private fun sendTurn(step: Step) {
+        val maneuver = step.maneuver
+        val (event, side) = maneuver?.let { eventFor(it.type) } ?: (EVENT_UNKNOWN to SIDE_UNSPECIFIED)
+        val road = step.road.text().ifBlank { step.cue.text() }
+        val key = "$road|$event|$side|${maneuver?.roundaboutExitNumber}"
+        if (key == sentTurn) return
+        sentTurn = key
+        var body = Protobuf.stringField(1, road) +
+            Protobuf.varintField(2, side.toLong()) +
+            Protobuf.varintField(3, event.toLong())
+        maneuver?.roundaboutExitNumber?.takeIf { it > 0 }?.let { body += Protobuf.varintField(5, it.toLong()) }
+        maneuver?.roundaboutExitAngle?.takeIf { it > 0 }?.let { body += Protobuf.varintField(6, it.toLong()) }
+        sendOnChannel(MSG_TURN, body)
+        GearslipLog.i("cluster: -> turn event=$event side=$side road=\"$road\"")
+    }
+
+    /**
+     * The Car App Library's distance units use the same numbers as the protocol's (1 metres ...
+     * 7 yards), so the app's own display choice passes straight through.
+     */
+    private fun sendDistance(distance: Distance, seconds: Long?) {
+        val meters = toMeters(distance)
+        val key = "${distance.displayDistance}|${distance.displayUnit}|${seconds ?: -1}"
+        if (key == sentDistance) return
+        sentDistance = key
+        sendOnChannel(
+            MSG_DISTANCE,
+            Protobuf.varintField(1, meters.toLong()) +
+                Protobuf.varintField(2, (seconds ?: 0L).coerceAtLeast(0)) +
+                Protobuf.varintField(3, (distance.displayDistance * 1000).toLong()) +
+                Protobuf.varintField(4, distance.displayUnit.toLong()),
+        )
+    }
+
+    private fun toMeters(d: Distance): Int = (d.displayDistance * when (d.displayUnit) {
+        Distance.UNIT_KILOMETERS, Distance.UNIT_KILOMETERS_P1 -> 1000.0
+        Distance.UNIT_MILES, Distance.UNIT_MILES_P1 -> 1609.344
+        Distance.UNIT_FEET -> 0.3048
+        Distance.UNIT_YARDS -> 0.9144
+        else -> 1.0
+    }).toInt()
+
+    private fun CarText?.text() = this?.toString().orEmpty()
+
+    companion object {
+        private const val TICK_MS = 500L
+        private const val MSG_CHANNEL_OPEN_RESPONSE = 8
+        private const val MSG_START = 32769
+        private const val MSG_STOP = 32770
+        private const val MSG_STATUS = 32771
+        private const val MSG_TURN = 32772
+        private const val MSG_DISTANCE = 32773
+
+        private const val STATUS_ACTIVE = 1
+        private const val STATUS_INACTIVE = 2
+        private const val STATUS_REROUTING = 3
+        private val STATUS_NAMES = mapOf(1 to "navigating", 2 to "not navigating", 3 to "rerouting")
+
+        private const val SIDE_LEFT = 1
+        private const val SIDE_RIGHT = 2
+        private const val SIDE_UNSPECIFIED = 3
+
+        private const val EVENT_UNKNOWN = 0
+
+        /** The car's turn icon and side for a Car App Library manoeuvre. */
+        fun eventFor(type: Int): Pair<Int, Int> = when (type) {
+            Maneuver.TYPE_DEPART -> 1 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_NAME_CHANGE -> 2 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_KEEP_LEFT, Maneuver.TYPE_TURN_SLIGHT_LEFT -> 3 to SIDE_LEFT
+            Maneuver.TYPE_KEEP_RIGHT, Maneuver.TYPE_TURN_SLIGHT_RIGHT -> 3 to SIDE_RIGHT
+            Maneuver.TYPE_TURN_NORMAL_LEFT -> 4 to SIDE_LEFT
+            Maneuver.TYPE_TURN_NORMAL_RIGHT -> 4 to SIDE_RIGHT
+            Maneuver.TYPE_TURN_SHARP_LEFT -> 5 to SIDE_LEFT
+            Maneuver.TYPE_TURN_SHARP_RIGHT -> 5 to SIDE_RIGHT
+            Maneuver.TYPE_U_TURN_LEFT -> 6 to SIDE_LEFT
+            Maneuver.TYPE_U_TURN_RIGHT -> 6 to SIDE_RIGHT
+            Maneuver.TYPE_ON_RAMP_SLIGHT_LEFT, Maneuver.TYPE_ON_RAMP_NORMAL_LEFT, Maneuver.TYPE_ON_RAMP_SHARP_LEFT,
+            Maneuver.TYPE_ON_RAMP_U_TURN_LEFT -> 7 to SIDE_LEFT
+            Maneuver.TYPE_ON_RAMP_SLIGHT_RIGHT, Maneuver.TYPE_ON_RAMP_NORMAL_RIGHT, Maneuver.TYPE_ON_RAMP_SHARP_RIGHT,
+            Maneuver.TYPE_ON_RAMP_U_TURN_RIGHT -> 7 to SIDE_RIGHT
+            Maneuver.TYPE_OFF_RAMP_SLIGHT_LEFT, Maneuver.TYPE_OFF_RAMP_NORMAL_LEFT -> 8 to SIDE_LEFT
+            Maneuver.TYPE_OFF_RAMP_SLIGHT_RIGHT, Maneuver.TYPE_OFF_RAMP_NORMAL_RIGHT -> 8 to SIDE_RIGHT
+            Maneuver.TYPE_FORK_LEFT -> 9 to SIDE_LEFT
+            Maneuver.TYPE_FORK_RIGHT -> 9 to SIDE_RIGHT
+            Maneuver.TYPE_MERGE_LEFT -> 10 to SIDE_LEFT
+            Maneuver.TYPE_MERGE_RIGHT -> 10 to SIDE_RIGHT
+            Maneuver.TYPE_MERGE_SIDE_UNSPECIFIED -> 10 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_ROUNDABOUT_ENTER_CW, Maneuver.TYPE_ROUNDABOUT_ENTER_CCW -> 11 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_ROUNDABOUT_EXIT_CW, Maneuver.TYPE_ROUNDABOUT_EXIT_CCW -> 12 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_ROUNDABOUT_ENTER_AND_EXIT_CW, Maneuver.TYPE_ROUNDABOUT_ENTER_AND_EXIT_CW_WITH_ANGLE,
+            Maneuver.TYPE_ROUNDABOUT_ENTER_AND_EXIT_CCW, Maneuver.TYPE_ROUNDABOUT_ENTER_AND_EXIT_CCW_WITH_ANGLE -> 13 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_STRAIGHT -> 14 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_FERRY_BOAT, Maneuver.TYPE_FERRY_BOAT_LEFT, Maneuver.TYPE_FERRY_BOAT_RIGHT -> 16 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_FERRY_TRAIN, Maneuver.TYPE_FERRY_TRAIN_LEFT, Maneuver.TYPE_FERRY_TRAIN_RIGHT -> 17 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_DESTINATION, Maneuver.TYPE_DESTINATION_STRAIGHT -> 19 to SIDE_UNSPECIFIED
+            Maneuver.TYPE_DESTINATION_LEFT -> 19 to SIDE_LEFT
+            Maneuver.TYPE_DESTINATION_RIGHT -> 19 to SIDE_RIGHT
+            else -> EVENT_UNKNOWN to SIDE_UNSPECIFIED
+        }
+    }
+}

gearslip/src/main/java/app/seb3thehacker/gearslip/host/CarAppConnection.kt

@@ -587,10 +587,19 @@ class CarAppConnection(private val context: Context) {
         override fun openMicrophone(request: Bundleable?): Bundleable? = null
     }
 
+    private val _trip = MutableStateFlow<androidx.car.app.navigation.model.Trip?>(null)
+    /** The route as the app tells clusters about it: steps and how far to each. Null when not navigating. */
+    val trip: StateFlow<androidx.car.app.navigation.model.Trip?> = _trip.asStateFlow()
+
     private val navigationHost = object : INavigationHost.Stub() {
         override fun navigationStarted() = GearslipLog.i("host: navigation started")
-        override fun navigationEnded() = GearslipLog.i("host: navigation ended")
-        override fun updateTrip(trip: Bundleable?) = Unit
+        override fun navigationEnded() {
+            GearslipLog.i("host: navigation ended")
+            _trip.value = null
+        }
+        override fun updateTrip(trip: Bundleable?) {
+            _trip.value = runCatching { trip?.get() as? androidx.car.app.navigation.model.Trip }.getOrNull()
+        }
     }
 
     /**