Phase 03: CAM decode coverage, real sensor data in TX, V2X monitor for ESP32 path
CAM codec: - Stop rejecting CAMs carrying a specialVehicleContainer. It is declared last in CamParameters, after everything this decoder reads, so buses / emergency vehicles / road-works vehicles now decode for position and kinematics instead of being dropped outright - Drop the lowFrequencyContainer parse - it extracted nothing into Cam, and its reads were only correct when no high-frequency optionals were present - Document why the 7 optional-presence bits are consumed but not acted on: UPER writes a SEQUENCE's presence bitmap up front but each field's value in declaration order, and all seven are declared after yawRate - Field widths and container ordering verified against the ETSI ASN.1 sources in the C-ITS-Parser checkout, not from memory Transmit path: - Own StationID is now a persisted random 32-bit value instead of a hardcoded 0. Receivers key on StationID to track a station across CAMs, so every unit broadcasting 0 made two MicrOBUs indistinguishable - including to this app's own detection engine - Populate longitudinalAcceleration from successive GNSS speed samples. Not from the accelerometer: CAM wants signed along-track acceleration, and the raw sensor is device-frame with gravity in it. Null outside a usable sample gap rather than a fabricated value - CAM pinger builds from live GNSS/IMU via PhoneCamBuilder instead of beaconing a hardcoded bench coordinate with speed and heading pinned to zero, so it now exercises the sensor pipeline and not just the wire. Sends nothing without a fix, and reports that rather than sitting at "Sent: 0" V2X monitor: - Received-CAM pane for the ESP32-C5 path, replacing the MQTT topic list that is permanently empty there. One row per station rather than per message - CAMs arrive at 1-10 Hz per station, so the pane is bounded by road users nearby, not by traffic rate. Nearest first, tinted by active alert level - DENM hazard pins on the live map as a warning triangle, drawn above vehicle markers. CiT One path only: the ESP32 firmware forwards BTP-B port 2001 (CAM) and drops port 2002 before it reaches the phone DenmParser uses tolerant field-name matching - the Use Case API's DENM JSON schema is not yet confirmed against real payloads. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
co-authored by
Claude Opus 5
parent
64fb78590e
commit
f3ae81a8fe
@@ -60,7 +60,15 @@ class CamTransmitLoop @Inject constructor(
|
||||
@Volatile private var latestGyroZ: Float? = null
|
||||
@Volatile private var elevatedUntilMs: Long = 0L
|
||||
|
||||
/** Own station id for the ESP32-C5 path — see [PhoneCamBuilder]'s KDoc on why this is a placeholder. */
|
||||
/** Previous GNSS fix, kept only to derive along-track acceleration — see [longitudinalAccel]. */
|
||||
@Volatile private var previousGnss: GnssReading? = null
|
||||
|
||||
/**
|
||||
* Own station id for the ESP32-C5 path, loaded once per [start] from
|
||||
* [ObuHardwarePreferences.getOrCreateOwnStationId]. 0 means "not loaded yet" — the loop waits
|
||||
* for the real value rather than beaconing as station 0, which would be indistinguishable
|
||||
* from every other MicrOBU to any receiver.
|
||||
*/
|
||||
@Volatile var stationId: Long = 0L
|
||||
|
||||
/**
|
||||
@@ -80,7 +88,9 @@ class CamTransmitLoop @Inject constructor(
|
||||
fun start() {
|
||||
if (job?.isActive == true) return
|
||||
elevatedUntilMs = 0L
|
||||
previousGnss = null
|
||||
job = scope.launch {
|
||||
stationId = obuHardwarePrefs.getOrCreateOwnStationId()
|
||||
obuHardwarePrefs.obuHardwareFlow.collectLatest { hardware ->
|
||||
if (hardware != ObuHardware.ESP32_C5) return@collectLatest
|
||||
runTransmitLoop()
|
||||
@@ -101,7 +111,7 @@ class CamTransmitLoop @Inject constructor(
|
||||
while (true) {
|
||||
val gnss = latestGnss
|
||||
if (gnss != null) {
|
||||
val cam = PhoneCamBuilder.build(gnss, latestGyroZ, stationId)
|
||||
val cam = PhoneCamBuilder.build(gnss, latestGyroZ, stationId, longitudinalAccel(gnss))
|
||||
val bytes = codec.encodeCam(cam)
|
||||
usbSerialTransport.sendCamTx(bytes)
|
||||
}
|
||||
@@ -109,6 +119,32 @@ class CamTransmitLoop @Inject constructor(
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Along-track acceleration in m/s², from the change in GNSS speed since the previous fix.
|
||||
*
|
||||
* Deliberately not from the accelerometer: CAM's `longitudinalAcceleration` is acceleration
|
||||
* along the direction of travel, while the raw accelerometer reads in the device frame with
|
||||
* gravity included — extracting the along-track component from it needs a full orientation
|
||||
* estimate, which this path doesn't have (the detection engine sidesteps the same problem by
|
||||
* working on orientation-independent magnitudes, which is not what CAM wants here).
|
||||
*
|
||||
* Returns null — encoded as ASN.1 `unavailable` — when there's no usable previous fix, when
|
||||
* the gap is too short to divide by safely, or when it's long enough that the two samples
|
||||
* aren't really consecutive. Better an honest "unavailable" than a fabricated number a
|
||||
* receiving vehicle might brake on.
|
||||
*/
|
||||
private fun longitudinalAccel(gnss: GnssReading): Double? {
|
||||
val prev = previousGnss
|
||||
previousGnss = gnss
|
||||
if (prev == null) return null
|
||||
|
||||
val dtSec = (gnss.timestamp - prev.timestamp) / 1000.0
|
||||
if (dtSec < MIN_ACCEL_DT_SEC || dtSec > MAX_ACCEL_DT_SEC) return null
|
||||
|
||||
val dv = gnss.speedMs.toDouble() - prev.speedMs.toDouble()
|
||||
return dv / dtSec
|
||||
}
|
||||
|
||||
private fun currentRateHz(gnss: GnssReading?): Double {
|
||||
val now = System.currentTimeMillis()
|
||||
val inGeofence = gnss != null && config.geofences.any { fence ->
|
||||
@@ -120,5 +156,11 @@ class CamTransmitLoop @Inject constructor(
|
||||
|
||||
companion object {
|
||||
private const val ELEVATED_HOLD_MS = 5_000L
|
||||
|
||||
/** Below this gap, GNSS speed noise divided by a tiny dt produces absurd accelerations. */
|
||||
private const val MIN_ACCEL_DT_SEC = 0.2
|
||||
|
||||
/** Above this gap the two fixes aren't consecutive enough to call the result acceleration. */
|
||||
private const val MAX_ACCEL_DT_SEC = 3.0
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user