Ashin Walpola
21e01499d8
The bench transmitter sent a parked car: one fixed position, speed 0, no heading, a CAM every second. It now simulates a car driving a loop through six waypoints around Berliner Tor on the real streets, which makes it a moving target for the app's map and the use case detection without taking a car out. The route is generated, not hand-traced. tools/make_route.py asks the OSRM demo server for a driving route through the waypoints and back to the first, thins the 371 street points to 103 (none more than 1.5 m off the line), and writes main/route_points.h. It also saves OSRM's answer (--offline rebuilds from it) and a map page to check the route before flashing. Each waypoint is sent with the direction towards the next one: without it, points on divided roads such as Beim Strohhause snapped to the opposite carriageway and the loop came out at 8.4 km of U-turns. With it the loop is 5.2 km, still including two turn-round detours that OSRM needs to reach the waypoints legally (Borgfelder Strasse / Anckelmannsplatz, and Nagelsweg / Norderstrasse / Repsoldstrasse). Route data (c) OpenStreetMap contributors, ODbL. main/route.c moves the car along the points. It cruises at 50 km/h and limits each bend to the speed that keeps sideways acceleration at 2 m/s^2, so a junction turn is taken at about 15 km/h and a gentle curve barely slows it; braking (2 m/s^2) and acceleration (1.5 m/s^2) are planned across as many points as a bend needs. A simulated lap on the host is 5.16 km in 7.7 min, averaging 40 km/h. CAMs now follow the EN 302 637-2 generation rules instead of a fixed 1 Hz: checked every 100 ms, sent on a heading change over 4 degrees, a move over 4 m, a speed change over 0.5 m/s, or after 1 s - about 3 Hz at 50 km/h. generationDeltaTime is milliseconds since boot. The GeoNetworking source position vector now carries the same speed and heading as the CAM instead of zeros. NOTES.md gains build and flash steps (including reading a board's app descriptor first, since both firmwares name their image obu_firmware.bin) and a section on the simulated drive. The pointer to docs/04-transmit-setup.md is corrected: that file is not in the repo. Flashed to the COM8 board and checked on its console: it starts driving on power-up and sends CAMs with changing position, speed and heading. Not yet received over the air.
97 lines
5.3 KiB
C
97 lines
5.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 speed_cm_s, uint16_t heading_ddeg,
|
|
uint16_t btp_dest_port,
|
|
uint8_t *out, size_t out_len)
|
|
{
|
|
// GN Basic Header (4) + GN Common Header (8) + SHB extended header (28)
|
|
// + BTP-B header (4) + ITS payload
|
|
int total = 4 + 8 + 28 + 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
|
|
// Lifetime: multiplier in the upper 6 bits, base in the lower 2 (0 = 50 ms, 1 = 1 s, 2 = 10 s,
|
|
// 3 = 100 s). 0x05 = 1 x 1 s, what real stations send their CAMs with. Was 0x83, commented as
|
|
// ~60 s but decoding to 32 x 100 s = 3200 s. See obu-firmware's geonet.c.
|
|
*p++ = 0x05;
|
|
*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 Position Vector (24) + Reserved (4) = 28 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, signed, 0.01 m/s), packed into 2 bytes. PAI stays 0: the
|
|
// position has no accuracy estimate behind it.
|
|
uint16_t spd = speed_cm_s > 0x7FFF ? 0x7FFF : speed_cm_s;
|
|
*p++ = (uint8_t)(spd >> 8); *p++ = (uint8_t)(spd & 0xFF);
|
|
// Heading (16 bits, 0.1 degree units, clockwise from north)
|
|
*p++ = (uint8_t)(heading_ddeg >> 8); *p++ = (uint8_t)(heading_ddeg & 0xFF);
|
|
// Reserved (4 bytes) - clause 9.8.4: the SHB extended header is the 24-byte Source Position
|
|
// Vector FOLLOWED BY a 4-byte reserved field (media-dependent data), 28 bytes in total. These
|
|
// four bytes were missing, which is why a standards-compliant receiver read our CAM payload's
|
|
// first two bytes (0x02 0x02 = protocolVersion/messageID) as the BTP destination port and saw
|
|
// 514 instead of 2001 - confirmed against live air capture, 2026-08-13. Our own gn_unwrap.c
|
|
// had the identical off-by-four, so ESP32<->ESP32 worked and nothing else did.
|
|
*p++ = 0x00; *p++ = 0x00; *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);
|
|
}
|