Ashin Walpola
f507a8a9fd
CurvatureCalculationMode is the one extensible ENUMERATED in CAM:
ENUMERATED {yawRateUsed(0), yawRateNotUsed(1), unavailable(2), ...}
UPER encodes an extensible ENUMERATED as an extension bit followed by the root
index - 1 + 2 = 3 bits. All three of our encoders wrote only the 2-bit index,
shifting yawRate and the entire low-frequency container one bit early for any
standards-compliant receiver.
It went unnoticed because every end of this project shared the mistake: the
Kotlin codec was ported bit-for-bit from cam.c, so phone and ESP32 agreed
perfectly with each other and with nothing else. Confirmed against the ETSI
ASN.1 in the C-ITS-Parser checkout, where rasn marks this type - and only this
type - #[non_exhaustive].
Fixed in all three copies of the encoder (app CamUperCodec.kt,
obu-firmware/main/cam.c, obu-cam-transmistter/main/cam.c) plus the decoder,
which now rejects rather than misreads a set extension bit. Frame size is
unchanged at 43 bytes. Transmitter reflashed and the phone decodes its CAMs.
Also in this change:
- serial_link: skip send_frame entirely when no USB host is attached, and raise
the tx mutex timeout above the worst-case hold. With the phone unplugged every
write blocked its full timeout while holding the lock, so forwarded CAM_RX
traffic starved the 1 Hz heartbeat - observed as "tx mutex timeout, dropping
frame" on the console, and it would have tripped the phone's link watchdog.
Verified gone on hardware.
- Log decoded and failed CAMs in CamUseCaseRepository. "The app shows nothing"
had two indistinguishable causes; a silent `?: return` made this bug much
harder to find than it needed to be.
- Remove the ESP32 send-only/send-and-receive toggle. Reception can't be
disabled in firmware (raw TX only works while promiscuous), so it was an
app-side filter pretending to be a radio control.
- V2X monitor follows the serial link state on the ESP32 path instead of MQTT,
which is permanently disconnected there; CAM intake is gated on the link being
up, and engine state is cleared when it drops.
- About screen: 0.5.0, Phase 03.
- Track obu-cam-transmistter, the bench CAM transmitter. Its cam.c is compiled
(unlike obu-firmware's reference copy) and must stay bit-identical to the other
two - this commit is what that coupling costs when it's broken.
- Document the two-toolchain split: this project builds on IDF 5.5.4, obu-firmware
on the pinned 6.1. Exporting both in one shell fails confusingly.
86 lines
4.3 KiB
C
86 lines
4.3 KiB
C
#include "geonet.h"
|
|
#include <string.h>
|
|
|
|
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
|
const uint8_t mac[6], uint8_t station_type,
|
|
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
|
uint16_t btp_dest_port,
|
|
uint8_t *out, size_t out_len)
|
|
{
|
|
// GN Basic Header (4) + GN Common Header (8) + SHB source LPV (24)
|
|
// + BTP-B header (4) + ITS payload
|
|
int total = 4 + 8 + 24 + 4 + its_len;
|
|
if ((size_t)total > out_len) {
|
|
return -1;
|
|
}
|
|
|
|
uint8_t *p = out;
|
|
|
|
// ---- GN Basic Header (4 bytes) ---- (EN 302 636-4-1 clause 9.6)
|
|
*p++ = (uint8_t)((1 << 4) | 1); // version=1, NextHeader=1 (Common Header, unsecured)
|
|
*p++ = 0x00; // reserved
|
|
*p++ = 0x83; // lifetime (~60s in the base/multiplier encoding) - tune if needed
|
|
*p++ = 1; // remaining hop limit = 1 (SHB single-hop; matches CAM in the Rust reference)
|
|
|
|
// ---- GN Common Header (8 bytes) ---- (clause 9.7)
|
|
*p++ = (uint8_t)((2 << 4) | 0); // NextHeader=2 (BTP-B), reserved nibble
|
|
// HeaderType=5 (TSB), HeaderSubtype=0 (SINGLE_HOP) per table 9 - this is
|
|
// the actual encoding for single-hop broadcast. An earlier version of
|
|
// this code used (2,0), which is GEOUNICAST - wrong header type entirely
|
|
// for a broadcast frame; real receivers would try to match the
|
|
// destination-address extended header GeoUnicast expects and mishandle
|
|
// or reject the packet.
|
|
*p++ = (uint8_t)((5 << 4) | 0);
|
|
*p++ = 0x02; // traffic class: SCF=0, ChannelOffload=0, TC-ID=2 (clause 9.7.5)
|
|
*p++ = 0x80; // flags: bit0 = "is mobile" station (clause 9.7.2)
|
|
// Payload length = what follows the WHOLE GeoNetworking header
|
|
// (Basic+Common+Extended), i.e. BTP-B header + ITS payload only - does
|
|
// NOT include the 24-byte extended header itself. An earlier version of
|
|
// this code wrongly added the 24 bytes in here too.
|
|
uint16_t payload_len = (uint16_t)(4 + its_len);
|
|
*p++ = (uint8_t)(payload_len >> 8);
|
|
*p++ = (uint8_t)(payload_len & 0xFF);
|
|
*p++ = 1; // max hop limit = 1, matches basic header RHL (SHB single-hop)
|
|
*p++ = 0x00; // reserved
|
|
|
|
// ---- SHB extended header: Source Long Position Vector (24 bytes) ----
|
|
// (clause 9.5.2). GN_ADDR (8 bytes) is itself structured, not a raw
|
|
// pseudonym (clause 9.5.1): bit0 M-flag(0=auto-derived), bits1-5 ITS-S
|
|
// type (5-bit), bits6-15 reserved(=0), then octets2-7 = MID, which is
|
|
// defined to BE the link-layer (802.11) address - so this must match
|
|
// the source address dot11p_build_frame uses, not just "look similar."
|
|
uint8_t gn_addr[8];
|
|
gn_addr[0] = (uint8_t)((0 << 7) | ((station_type & 0x1F) << 2)); // M=0, ST=station_type, top 2 reserved bits=0
|
|
gn_addr[1] = 0x00; // remaining 8 reserved bits
|
|
memcpy(&gn_addr[2], mac, 6); // MID = link-layer address
|
|
memcpy(p, gn_addr, 8); p += 8;
|
|
// Timestamp (4 bytes, ms since 2004-01-01 mod 2^32) - placeholder 0,
|
|
// same caveat as detectionTime in denm.c.
|
|
memset(p, 0, 4); p += 4;
|
|
// Latitude/Longitude (4+4 bytes, signed, big-endian, 1/10 microdegree) -
|
|
// fixed-width binary fields, not UPER bit-packed.
|
|
uint32_t lat_u = (uint32_t)latitude_tenmicrodeg;
|
|
*p++ = (uint8_t)(lat_u >> 24); *p++ = (uint8_t)(lat_u >> 16);
|
|
*p++ = (uint8_t)(lat_u >> 8); *p++ = (uint8_t)(lat_u);
|
|
uint32_t lon_u = (uint32_t)longitude_tenmicrodeg;
|
|
*p++ = (uint8_t)(lon_u >> 24); *p++ = (uint8_t)(lon_u >> 16);
|
|
*p++ = (uint8_t)(lon_u >> 8); *p++ = (uint8_t)(lon_u);
|
|
// PAI(1 bit) + Speed(15 bits), packed into 2 bytes: 0 = PAI false,
|
|
// speed 0 - which is actually correct semantics for a STATIONARY
|
|
// vehicle beacon, not just a placeholder.
|
|
*p++ = 0x00; *p++ = 0x00;
|
|
// Heading (16 bits, 0.1 degree units): 0 = due north / unavailable
|
|
*p++ = 0x00; *p++ = 0x00;
|
|
|
|
// ---- BTP-B header (4 bytes) ----
|
|
*p++ = (uint8_t)(btp_dest_port >> 8);
|
|
*p++ = (uint8_t)(btp_dest_port & 0xFF);
|
|
*p++ = 0x00; *p++ = 0x00; // destination port info, unused for BTP-B
|
|
|
|
// ---- ITS payload (DENM UPER bytes) ----
|
|
memcpy(p, its_payload, its_len);
|
|
p += its_len;
|
|
|
|
return (int)(p - out);
|
|
}
|