Drive the bench CAM beacon round a street loop in St. Georg
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.
This commit is contained in:
Ashin Walpola
@@ -1,7 +1,7 @@
|
|||||||
# OBU transmit firmware - Phase 2 (in progress: HLN-SV DENM beacon)
|
# OBU transmit firmware - Phase 2 (in progress: HLN-SV DENM beacon)
|
||||||
|
|
||||||
Started. See `docs/04-transmit-setup.md` in the project root for build/flash
|
Build and flash steps are under "Build and flash" below. (`docs/04-transmit-setup.md`,
|
||||||
steps and how to validate this against your own sniffer.
|
referenced here and in the sources, is not in the repo.)
|
||||||
|
|
||||||
## Toolchain: use a dedicated terminal (ESP-IDF 5.5.4)
|
## Toolchain: use a dedicated terminal (ESP-IDF 5.5.4)
|
||||||
|
|
||||||
@@ -29,6 +29,123 @@ referenced an older source path under `micrOBU_workspace/v2x-obu-esp32c5/`,
|
|||||||
which makes `idf.py fullclean` refuse to run). If that error reappears, delete
|
which makes `idf.py fullclean` refuse to run). If that error reappears, delete
|
||||||
`build/` manually rather than fighting it.
|
`build/` manually rather than fighting it.
|
||||||
|
|
||||||
|
## Build and flash
|
||||||
|
|
||||||
|
The firmware needs no button, phone or serial connection to start. On every power-up or reset,
|
||||||
|
`app_main` sets up the radio and starts `tx_task`, which starts driving the simulated route and
|
||||||
|
sending CAMs on 5900 MHz. A board flashed with this image starts beaconing on its own as soon as it
|
||||||
|
gets power.
|
||||||
|
|
||||||
|
In a fresh PowerShell window:
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
$env:IDF_PYTHON_ENV_PATH = $null; $env:IDF_PATH = $null
|
||||||
|
. C:\Espressif\frameworks\esp-idf-v5.5.4\export.ps1
|
||||||
|
cd C:\Users\Ashin\AndroidStudioProjects\MicrOBU\obu-cam-transmistter
|
||||||
|
```
|
||||||
|
|
||||||
|
1. **Check what the board is running before you flash it.** Both this project and `obu-firmware`
|
||||||
|
name their image `obu_firmware.bin`, so the file name tells you nothing. Read the app
|
||||||
|
descriptor instead (replace `COMx` with the board's port):
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
python -m esptool --chip esp32c5 -p COMx read_flash 0x10000 0x100 $env:TEMP\desc.bin
|
||||||
|
$b = [IO.File]::ReadAllBytes("$env:TEMP\desc.bin")
|
||||||
|
function S($o,$n){ [Text.Encoding]::ASCII.GetString($b,$o,$n).Trim([char]0) }
|
||||||
|
"time=" + (S 0x70 16) + " date=" + (S 0x80 16) + " idf=" + (S 0x90 32)
|
||||||
|
```
|
||||||
|
|
||||||
|
`idf=v6.1...` means the board runs the production OBU (`obu-firmware`). Don't flash this beacon
|
||||||
|
over it. `idf=v5.5.4` means this transmitter, or another bench image.
|
||||||
|
|
||||||
|
2. **Build:**
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
idf.py build
|
||||||
|
```
|
||||||
|
|
||||||
|
A rebuild after small changes takes about 1-2 minutes. The output is
|
||||||
|
`build\obu_firmware.bin`.
|
||||||
|
|
||||||
|
3. **Flash** (the board must be on its UART bridge port or its native USB port):
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
idf.py -p COMx flash
|
||||||
|
```
|
||||||
|
|
||||||
|
The flash is good when esptool prints `Hash of data verified.`, then resets the board.
|
||||||
|
|
||||||
|
4. **Check that it's transmitting:**
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
idf.py -p COMx monitor
|
||||||
|
```
|
||||||
|
|
||||||
|
(Exit with `Ctrl+]`.) About 1.4 s after reset you should see:
|
||||||
|
|
||||||
|
```
|
||||||
|
W obu-tx: OCB @ 5900 MHz - CAM beacon armed, driving a 103-point street loop
|
||||||
|
I obu-tx: CAM sent (119 bytes) @ 5900 MHz genDeltaT=1087 pos=53.5531770,10.0220980 50.0 km/h heading 77.0 pt1
|
||||||
|
```
|
||||||
|
|
||||||
|
After that, a `CAM sent` line appears about 3 times a second, with the position, speed and
|
||||||
|
heading changing. These lines only mean each frame was
|
||||||
|
handed to the radio. To confirm the frames actually went out, capture them with a second
|
||||||
|
ESP32-C5 running the receiver firmware.
|
||||||
|
|
||||||
|
Don't open the console of the production OBU (COM3) while the phone is attached. Opening the port
|
||||||
|
resets that board and drops the phone's USB link. The beacon boards have no phone attached, so
|
||||||
|
this doesn't apply to them.
|
||||||
|
|
||||||
|
## Simulated drive
|
||||||
|
|
||||||
|
The beacon pretends to be a car driving a loop through St. Georg / Berliner Tor in Hamburg, on
|
||||||
|
the real streets. The route comes from six waypoints, which you set in `tools/make_route.py`.
|
||||||
|
|
||||||
|
**Changing the route:** edit `WAYPOINTS` in `tools/make_route.py`, then run
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
py -3.11 tools/make_route.py
|
||||||
|
```
|
||||||
|
|
||||||
|
The script asks the OSRM demo server (router.project-osrm.org) for a legal driving route through the
|
||||||
|
waypoints in order and back to the first, then writes three files:
|
||||||
|
|
||||||
|
- `main/route_points.h`: the street geometry, thinned to points no more than 1.5 m off the line
|
||||||
|
(currently 103 points).
|
||||||
|
- `tools/route_osrm.json`: OSRM's raw answer. `--offline` rebuilds the header from it without the
|
||||||
|
network.
|
||||||
|
- `tools/route_map.html`: the route on an OpenStreetMap map. Open it in a browser and check it
|
||||||
|
before building.
|
||||||
|
|
||||||
|
Then build and flash as above. Route data (c) OpenStreetMap contributors, ODbL; routing by OSRM.
|
||||||
|
|
||||||
|
**Things to know about the routing:**
|
||||||
|
- OSRM follows one-way streets and turn bans, so the loop can be longer than the waypoints suggest.
|
||||||
|
The current one is 5.2 km, with two turn-round detours: a loop via Borgfelder Straße and
|
||||||
|
Anckelmannsplatz between wp2 and wp3, and one round Nagelsweg, Norderstraße and Repsoldstraße
|
||||||
|
between wp5 and wp6. To avoid a detour, move the waypoint on either side of it.
|
||||||
|
- Each waypoint is sent with the direction towards the next one. Without it, points on divided
|
||||||
|
roads (Beim Strohhause, for example) snap to the carriageway going the other way, and the loop
|
||||||
|
grows to 8.4 km of U-turns.
|
||||||
|
|
||||||
|
**How the car drives** (`main/route.c`):
|
||||||
|
- **Speed:** it cruises at 50 km/h (`CRUISE_MPS` in `main/main.c`). Each bend gets a speed limit
|
||||||
|
from its radius, keeping sideways acceleration at 2 m/s², so a 90° junction turn is taken at
|
||||||
|
about 15 km/h and a gentle curve barely slows the car. It never drops below 10 km/h
|
||||||
|
(`MIN_CORNER_MPS`). It brakes at 2 m/s² and accelerates at 1.5 m/s², planning braking across as
|
||||||
|
many points as a bend needs. A lap takes about 7.7 min, averaging 40 km/h.
|
||||||
|
- **Heading:** the compass bearing of the current straight piece. It changes gradually through
|
||||||
|
curves but jumps at sharp junction turns.
|
||||||
|
- **When CAMs are sent:** following ETSI EN 302 637-2, the state is checked every 100 ms. A CAM goes
|
||||||
|
out when the heading changed by more than 4°, the position by more than 4 m, or the speed by more
|
||||||
|
than 0.5 m/s since the last one, and at least once a second. That's about 3 CAMs a second at
|
||||||
|
50 km/h.
|
||||||
|
- **What's filled in:** position, speed and heading go into both the CAM and the GeoNetworking
|
||||||
|
source position vector. `genDeltaT` is milliseconds since boot.
|
||||||
|
- **Testing:** `route.c` only uses standard headers, so you can compile it on the PC with MSYS2 gcc
|
||||||
|
and simulate a lap.
|
||||||
|
|
||||||
## CAM encoding
|
## CAM encoding
|
||||||
|
|
||||||
`main/cam.c` IS compiled here (unlike `obu-firmware`'s copy, which is a
|
`main/cam.c` IS compiled here (unlike `obu-firmware`'s copy, which is a
|
||||||
@@ -44,10 +161,12 @@ hazard-light GPIO is grounded. No location/alacarte containers.
|
|||||||
- `main/main.c` - entry point, the `phy_11p_set`/`phy_change_channel(5900,...)`
|
- `main/main.c` - entry point, the `phy_11p_set`/`phy_change_channel(5900,...)`
|
||||||
register hack, GPIO polling, TX loop
|
register hack, GPIO polling, TX loop
|
||||||
- `main/denm.c` / `.h` - ASN.1 UPER encoding of a minimal DENM
|
- `main/denm.c` / `.h` - ASN.1 UPER encoding of a minimal DENM
|
||||||
|
- `main/route.c` / `.h` - simulated drive round the route loop
|
||||||
|
- `main/route_points.h` - the route, generated by `tools/make_route.py`
|
||||||
- `main/geonet.c` / `.h` - GeoNetworking Basic/Common/SHB headers + BTP-B
|
- `main/geonet.c` / `.h` - GeoNetworking Basic/Common/SHB headers + BTP-B
|
||||||
- `main/dot11p.c` / `.h` - 802.11 OCB (QoS Data, broadcast) frame + LLC/SNAP
|
- `main/dot11p.c` / `.h` - 802.11 OCB (QoS Data, broadcast) frame + LLC/SNAP
|
||||||
|
|
||||||
Known gaps, tracked as TODOs in the source: no real GNSS (lat/long hardcoded
|
Known gaps, tracked as TODOs in the source: no real GNSS (the CAM position comes
|
||||||
0), no real time source (detectionTime/referenceTime hardcoded 0, decodes as
|
from the simulated drive above), no real time source (detectionTime/referenceTime hardcoded 0, decodes as
|
||||||
2004-01-01), fixed (non-rotating) pseudonym MAC, SHB instead of GeoBroadcast
|
2004-01-01), fixed (non-rotating) pseudonym MAC, SHB instead of GeoBroadcast
|
||||||
(no multi-hop forwarding), unsecured (no IEEE 1609.2 signing).
|
(no multi-hop forwarding), unsecured (no IEEE 1609.2 signing).
|
||||||
|
|||||||
@@ -1,8 +1,8 @@
|
|||||||
# wifi_patches.c is intentionally NOT in this list anymore - superseded by
|
# wifi_patches.c is intentionally NOT in this list anymore - superseded by
|
||||||
# tx_custom.c (see that file for why). Left on disk, unused, for history.
|
# tx_custom.c (see that file for why). Left on disk, unused, for history.
|
||||||
idf_component_register(
|
idf_component_register(
|
||||||
SRCS "main.c" "denm.c" "cam.c" "geonet.c" "dot11p.c" "tx_custom.c"
|
SRCS "main.c" "denm.c" "cam.c" "geonet.c" "dot11p.c" "tx_custom.c" "route.c"
|
||||||
INCLUDE_DIRS "."
|
INCLUDE_DIRS "."
|
||||||
REQUIRES esp_event esp_netif nvs_flash driver esp_phy
|
REQUIRES esp_event esp_timer esp_netif nvs_flash driver esp_phy
|
||||||
PRIV_REQUIRES esp_wifi
|
PRIV_REQUIRES esp_wifi
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -4,6 +4,7 @@
|
|||||||
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
||||||
const uint8_t mac[6], uint8_t station_type,
|
const uint8_t mac[6], uint8_t station_type,
|
||||||
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
||||||
|
uint16_t speed_cm_s, uint16_t heading_ddeg,
|
||||||
uint16_t btp_dest_port,
|
uint16_t btp_dest_port,
|
||||||
uint8_t *out, size_t out_len)
|
uint8_t *out, size_t out_len)
|
||||||
{
|
{
|
||||||
@@ -68,12 +69,12 @@ int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
|||||||
uint32_t lon_u = (uint32_t)longitude_tenmicrodeg;
|
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 >> 24); *p++ = (uint8_t)(lon_u >> 16);
|
||||||
*p++ = (uint8_t)(lon_u >> 8); *p++ = (uint8_t)(lon_u);
|
*p++ = (uint8_t)(lon_u >> 8); *p++ = (uint8_t)(lon_u);
|
||||||
// PAI(1 bit) + Speed(15 bits), packed into 2 bytes: 0 = PAI false,
|
// PAI(1 bit) + Speed(15 bits, signed, 0.01 m/s), packed into 2 bytes. PAI stays 0: the
|
||||||
// speed 0 - which is actually correct semantics for a STATIONARY
|
// position has no accuracy estimate behind it.
|
||||||
// vehicle beacon, not just a placeholder.
|
uint16_t spd = speed_cm_s > 0x7FFF ? 0x7FFF : speed_cm_s;
|
||||||
*p++ = 0x00; *p++ = 0x00;
|
*p++ = (uint8_t)(spd >> 8); *p++ = (uint8_t)(spd & 0xFF);
|
||||||
// Heading (16 bits, 0.1 degree units): 0 = due north / unavailable
|
// Heading (16 bits, 0.1 degree units, clockwise from north)
|
||||||
*p++ = 0x00; *p++ = 0x00;
|
*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
|
// 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
|
// 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
|
// four bytes were missing, which is why a standards-compliant receiver read our CAM payload's
|
||||||
|
|||||||
@@ -31,6 +31,10 @@
|
|||||||
// working (same extended header shape as CAM). Fine for a single-vehicle
|
// working (same extended header shape as CAM). Fine for a single-vehicle
|
||||||
// beacon; revisit if you need real multi-hop forwarding later.
|
// beacon; revisit if you need real multi-hop forwarding later.
|
||||||
//
|
//
|
||||||
|
// `speed_cm_s` (0.01 m/s) and `heading_ddeg` (0.1 deg) also go into the Source Long Position
|
||||||
|
// Vector; pass the same values as the CAM's high-frequency container. Speed is a 15-bit field, so
|
||||||
|
// values above 32767 are clamped.
|
||||||
|
//
|
||||||
// `btp_dest_port` is the BTP-B destination port for the service being carried
|
// `btp_dest_port` is the BTP-B destination port for the service being carried
|
||||||
// (ETSI TS 103 248): 2001 = CAM, 2002 = DENM, 2003 = MAPEM, 2004 = SPATEM, ...
|
// (ETSI TS 103 248): 2001 = CAM, 2002 = DENM, 2003 = MAPEM, 2004 = SPATEM, ...
|
||||||
//
|
//
|
||||||
@@ -38,6 +42,7 @@
|
|||||||
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
int geonet_wrap_shb(const uint8_t *its_payload, int its_len,
|
||||||
const uint8_t mac[6], uint8_t station_type,
|
const uint8_t mac[6], uint8_t station_type,
|
||||||
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
int32_t latitude_tenmicrodeg, int32_t longitude_tenmicrodeg,
|
||||||
|
uint16_t speed_cm_s, uint16_t heading_ddeg,
|
||||||
uint16_t btp_dest_port,
|
uint16_t btp_dest_port,
|
||||||
uint8_t *out, size_t out_len);
|
uint8_t *out, size_t out_len);
|
||||||
|
|
||||||
|
|||||||
@@ -1,7 +1,11 @@
|
|||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <stdbool.h>
|
||||||
|
#include <math.h>
|
||||||
#include "freertos/FreeRTOS.h"
|
#include "freertos/FreeRTOS.h"
|
||||||
#include "freertos/task.h"
|
#include "freertos/task.h"
|
||||||
|
#include "esp_timer.h"
|
||||||
#include "driver/gpio.h"
|
#include "driver/gpio.h"
|
||||||
#include "esp_wifi.h"
|
#include "esp_wifi.h"
|
||||||
#include "esp_event.h"
|
#include "esp_event.h"
|
||||||
@@ -14,13 +18,17 @@
|
|||||||
#include "geonet.h"
|
#include "geonet.h"
|
||||||
#include "dot11p.h"
|
#include "dot11p.h"
|
||||||
#include "tx_custom.h"
|
#include "tx_custom.h"
|
||||||
|
#include "route.h"
|
||||||
|
#include "route_points.h"
|
||||||
|
|
||||||
static const char *TAG = "obu-tx";
|
static const char *TAG = "obu-tx";
|
||||||
|
|
||||||
// CAM beacon: transmit a Cooperative Awareness Message every TX_INTERVAL_MS,
|
// CAM beacon for a simulated car driving round a block in Hamburg (see route.c). The CAM
|
||||||
// unconditionally (no hazard-light gating - CAM is a continuous beacon, unlike
|
// generation rules follow ETSI EN 302 637-2 clause 6.1.3: every CHECK_INTERVAL_MS the car's state
|
||||||
// the event-triggered DENM). Matches the working Rust reference
|
// is compared with the last CAM sent, and a new CAM goes out when the heading changed by more than
|
||||||
// (esp32-c_its-companion, feat/tx-cam), which beacons CAM on 5900 MHz.
|
// 4 degrees, the position by more than 4 m, the speed by more than 0.5 m/s, or 1 s has passed.
|
||||||
|
// There is no hazard-light gating - CAM is a continuous beacon, unlike the event-triggered DENM.
|
||||||
|
// Transmits on 5900 MHz like the working Rust reference (esp32-c_its-companion, feat/tx-cam).
|
||||||
|
|
||||||
// ISOLATION TEST for whether tx_custom.c is the blocker.
|
// ISOLATION TEST for whether tx_custom.c is the blocker.
|
||||||
// 1 = transmit via the STANDARD, well-tested esp_wifi_80211_tx() using a
|
// 1 = transmit via the STANDARD, well-tested esp_wifi_80211_tx() using a
|
||||||
@@ -55,15 +63,19 @@ static const char *TAG = "obu-tx";
|
|||||||
#define VEHICLE_LENGTH_DM 40 // VehicleLengthValue, 10cm steps (4.0 m)
|
#define VEHICLE_LENGTH_DM 40 // VehicleLengthValue, 10cm steps (4.0 m)
|
||||||
#define VEHICLE_WIDTH_DM 18 // VehicleWidth, 10cm steps (1.8 m)
|
#define VEHICLE_WIDTH_DM 18 // VehicleWidth, 10cm steps (1.8 m)
|
||||||
#define BTP_PORT_CAM 2001 // BTP-B destination port for CAM (ETSI TS 103 248)
|
#define BTP_PORT_CAM 2001 // BTP-B destination port for CAM (ETSI TS 103 248)
|
||||||
#define TX_INTERVAL_MS 1000 // CAM beacon period (1 Hz; ITS allows 1-10 Hz)
|
#define CHECK_INTERVAL_MS 100 // T_CheckCamGen: how often the generation rules are evaluated
|
||||||
|
#define CAM_MAX_INTERVAL_MS 1000 // T_GenCamMax: a CAM goes out at least this often
|
||||||
|
#define CAM_HEADING_DDEG 40 // > 4 degrees heading change triggers a CAM
|
||||||
|
#define CAM_POSITION_M 4.0 // > 4 m position change triggers a CAM
|
||||||
|
#define CAM_SPEED_CM_S 50 // > 0.5 m/s speed change triggers a CAM
|
||||||
|
|
||||||
// Bench location, hardcoded since there's no GNSS module wired in yet and
|
// ---- Simulated drive ----
|
||||||
// the unit is genuinely stationary here: 53°33'16.8"N 10°01'20.6"E, in
|
// route_points (main/route_points.h) is the street geometry of a driving loop through six waypoints
|
||||||
// 1/10-microdegree units (decimal_degrees * 10,000,000). Replace with real
|
// in St. Georg, generated by tools/make_route.py from OpenStreetMap via OSRM. To change the route,
|
||||||
// GNSS output once you have a fix source; until then this beats 0/0
|
// edit WAYPOINTS in that script and rerun it. No GNSS is wired in; replace with real fixes once
|
||||||
// ("Null Island"), which is an obvious placeholder-tell on any map.
|
// there is one.
|
||||||
#define BENCH_LATITUDE_TENMICRODEG 535546667
|
#define CRUISE_MPS (50.0 / 3.6) // 50 km/h, the urban limit
|
||||||
#define BENCH_LONGITUDE_TENMICRODEG 100223889
|
#define MIN_CORNER_MPS (10.0 / 3.6) // slowest the car goes, for hairpins and U-turns
|
||||||
|
|
||||||
// Single source of truth for the pseudonym/link-layer address: used both as
|
// Single source of truth for the pseudonym/link-layer address: used both as
|
||||||
// the 802.11 source MAC (Addr2) and as GN_ADDR's MID field, since the GN
|
// the 802.11 source MAC (Addr2) and as GN_ADDR's MID field, since the GN
|
||||||
@@ -78,33 +90,28 @@ static const uint8_t pseudonym_mac[6] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x01};
|
|||||||
extern void phy_11p_set(int enable, int unused);
|
extern void phy_11p_set(int enable, int unused);
|
||||||
extern void phy_change_channel(int freq_mhz, int bw_mode, int sec_chan_offset, int unused);
|
extern void phy_change_channel(int freq_mhz, int bw_mode, int sec_chan_offset, int unused);
|
||||||
|
|
||||||
static void send_cam(void)
|
static void send_cam(const route_state_t *car, uint16_t gen_delta)
|
||||||
{
|
{
|
||||||
// GenerationDeltaTime is TimestampIts mod 65536 (ms). No RTC/GNSS time here,
|
|
||||||
// so use a free-running ms counter that advances one beacon-interval per
|
|
||||||
// send. It wraps at 65536, which is exactly the field's defined behaviour.
|
|
||||||
static uint16_t gen_delta = 0;
|
|
||||||
|
|
||||||
uint8_t frame[300];
|
uint8_t frame[300];
|
||||||
cam_fields_t fields = {
|
cam_fields_t fields = {
|
||||||
.station_id = STATION_ID,
|
.station_id = STATION_ID,
|
||||||
.station_type = STATION_TYPE,
|
.station_type = STATION_TYPE,
|
||||||
.generation_delta_time = gen_delta,
|
.generation_delta_time = gen_delta,
|
||||||
.latitude_tenmicrodeg = BENCH_LATITUDE_TENMICRODEG,
|
.latitude_tenmicrodeg = car->latitude_tenmicrodeg,
|
||||||
.longitude_tenmicrodeg = BENCH_LONGITUDE_TENMICRODEG,
|
.longitude_tenmicrodeg = car->longitude_tenmicrodeg,
|
||||||
.speed_cm_s = 0, // stationary
|
.speed_cm_s = car->speed_cm_s,
|
||||||
.heading_ddeg = 3601, // HeadingValue unavailable (no heading source)
|
.heading_ddeg = car->heading_ddeg,
|
||||||
.vehicle_length_dm = VEHICLE_LENGTH_DM,
|
.vehicle_length_dm = VEHICLE_LENGTH_DM,
|
||||||
.vehicle_width_dm = VEHICLE_WIDTH_DM,
|
.vehicle_width_dm = VEHICLE_WIDTH_DM,
|
||||||
};
|
};
|
||||||
gen_delta += TX_INTERVAL_MS;
|
|
||||||
|
|
||||||
uint8_t cam_payload[96];
|
uint8_t cam_payload[96];
|
||||||
int cam_len = cam_encode(&fields, cam_payload, sizeof(cam_payload));
|
int cam_len = cam_encode(&fields, cam_payload, sizeof(cam_payload));
|
||||||
|
|
||||||
uint8_t gn_payload[160];
|
uint8_t gn_payload[160];
|
||||||
int gn_len = geonet_wrap_shb(cam_payload, cam_len, pseudonym_mac, STATION_TYPE,
|
int gn_len = geonet_wrap_shb(cam_payload, cam_len, pseudonym_mac, STATION_TYPE,
|
||||||
BENCH_LATITUDE_TENMICRODEG, BENCH_LONGITUDE_TENMICRODEG,
|
car->latitude_tenmicrodeg, car->longitude_tenmicrodeg,
|
||||||
|
car->speed_cm_s, car->heading_ddeg,
|
||||||
BTP_PORT_CAM, gn_payload, sizeof(gn_payload));
|
BTP_PORT_CAM, gn_payload, sizeof(gn_payload));
|
||||||
|
|
||||||
// qos=false for the standard-TX path (esp_wifi_80211_tx accepts only non-QoS
|
// qos=false for the standard-TX path (esp_wifi_80211_tx accepts only non-QoS
|
||||||
@@ -127,7 +134,10 @@ static void send_cam(void)
|
|||||||
if (err != ESP_OK) {
|
if (err != ESP_OK) {
|
||||||
ESP_LOGW(TAG, "esp_wifi_80211_tx (standard) failed: %d", err);
|
ESP_LOGW(TAG, "esp_wifi_80211_tx (standard) failed: %d", err);
|
||||||
} else {
|
} else {
|
||||||
ESP_LOGI(TAG, "CAM sent via STANDARD tx (%d bytes) @ %d MHz genDeltaT=%u", frame_len, TX_FREQ_MHZ, gen_delta);
|
ESP_LOGI(TAG, "CAM sent (%d bytes) @ %d MHz genDeltaT=%u pos=%.7f,%.7f %.1f km/h heading %.1f pt%d",
|
||||||
|
frame_len, TX_FREQ_MHZ, gen_delta,
|
||||||
|
car->latitude_tenmicrodeg / 1e7, car->longitude_tenmicrodeg / 1e7,
|
||||||
|
car->speed_cm_s * 0.036, car->heading_ddeg / 10.0, car->segment + 1);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
// tx_custom path: submits to the driver's internal HMAC TX path,
|
// tx_custom path: submits to the driver's internal HMAC TX path,
|
||||||
@@ -151,12 +161,49 @@ static void send_cam(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool cam_due(const route_state_t *car, const route_state_t *last, int64_t since_last_ms)
|
||||||
|
{
|
||||||
|
if (since_last_ms >= CAM_MAX_INTERVAL_MS) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
int dh = abs((int)car->heading_ddeg - (int)last->heading_ddeg);
|
||||||
|
if (dh > 1800) {
|
||||||
|
dh = 3600 - dh;
|
||||||
|
}
|
||||||
|
if (dh > CAM_HEADING_DDEG) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
if (abs((int)car->speed_cm_s - (int)last->speed_cm_s) > CAM_SPEED_CM_S) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
// Flat-earth distance is plenty for a 4 m threshold.
|
||||||
|
double north_m = (car->latitude_tenmicrodeg - last->latitude_tenmicrodeg) * 0.0111194930;
|
||||||
|
double east_m = (car->longitude_tenmicrodeg - last->longitude_tenmicrodeg) * 0.0111194930
|
||||||
|
* cos(car->latitude_tenmicrodeg / 1e7 * M_PI / 180.0);
|
||||||
|
return north_m * north_m + east_m * east_m > CAM_POSITION_M * CAM_POSITION_M;
|
||||||
|
}
|
||||||
|
|
||||||
static void tx_task(void *arg)
|
static void tx_task(void *arg)
|
||||||
{
|
{
|
||||||
|
route_state_t car;
|
||||||
|
route_state_t last_sent;
|
||||||
|
int64_t last_sent_ms = 0;
|
||||||
|
bool sent_any = false;
|
||||||
|
TickType_t wake = xTaskGetTickCount();
|
||||||
|
|
||||||
|
route_step(0.0, &car);
|
||||||
while (1) {
|
while (1) {
|
||||||
// CAM is a continuous beacon - send every interval, unconditionally.
|
int64_t now_ms = esp_timer_get_time() / 1000;
|
||||||
send_cam();
|
if (!sent_any || cam_due(&car, &last_sent, now_ms - last_sent_ms)) {
|
||||||
vTaskDelay(pdMS_TO_TICKS(TX_INTERVAL_MS));
|
// GenerationDeltaTime is TimestampIts mod 65536 (ms). No real clock here, so use
|
||||||
|
// milliseconds since boot, which advances at the right rate.
|
||||||
|
send_cam(&car, (uint16_t)now_ms);
|
||||||
|
last_sent = car;
|
||||||
|
last_sent_ms = now_ms;
|
||||||
|
sent_any = true;
|
||||||
|
}
|
||||||
|
vTaskDelayUntil(&wake, pdMS_TO_TICKS(CHECK_INTERVAL_MS));
|
||||||
|
route_step(CHECK_INTERVAL_MS / 1000.0, &car);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -255,8 +302,13 @@ void app_main(void)
|
|||||||
phy_change_channel(TX_FREQ_MHZ, 1, 0, 0);
|
phy_change_channel(TX_FREQ_MHZ, 1, 0, 0);
|
||||||
ESP_LOGI(TAG, "phy_change_channel returned");
|
ESP_LOGI(TAG, "phy_change_channel returned");
|
||||||
|
|
||||||
ESP_LOGW(TAG, "OCB @ %d MHz - CAM beacon armed, transmitting every %d ms",
|
if (route_init(route_points, sizeof(route_points) / sizeof(route_points[0]),
|
||||||
TX_FREQ_MHZ, TX_INTERVAL_MS);
|
CRUISE_MPS, MIN_CORNER_MPS) != 0) {
|
||||||
|
ESP_LOGE(TAG, "route_init failed - check route_points");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
ESP_LOGW(TAG, "OCB @ %d MHz - CAM beacon armed, driving a %d-point street loop",
|
||||||
|
TX_FREQ_MHZ, (int)(sizeof(route_points) / sizeof(route_points[0])));
|
||||||
|
|
||||||
xTaskCreate(tx_task, "tx_task", 4096, NULL, 5, NULL);
|
xTaskCreate(tx_task, "tx_task", 4096, NULL, 5, NULL);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -0,0 +1,125 @@
|
|||||||
|
#include "route.h"
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#define DEG_TO_RAD (M_PI / 180.0)
|
||||||
|
#define METRES_PER_DEG 111194.93 // mean Earth radius 6371 km; a block is small enough for a flat projection
|
||||||
|
#define ACCEL_MPS2 1.5 // pulling away from a corner
|
||||||
|
#define DECEL_MPS2 2.0 // braking ahead of a corner
|
||||||
|
#define LATERAL_MPS2 2.0 // sideways acceleration a normal driver takes a bend at
|
||||||
|
#define STRAIGHT_DEG 3.0 // kinks gentler than this are digitising noise, not bends
|
||||||
|
#define BEND_SPAN_M 15.0 // longest segment counted towards a bend's radius (see route_init)
|
||||||
|
|
||||||
|
static const route_point_t *s_pts;
|
||||||
|
static int s_n;
|
||||||
|
static double s_len[ROUTE_MAX_POINTS]; // segment i runs from point i to point (i+1) % n
|
||||||
|
static double s_bearing_deg[ROUTE_MAX_POINTS];
|
||||||
|
static double s_corner_mps[ROUTE_MAX_POINTS]; // speed limit at point i, where segment i starts
|
||||||
|
static double s_cruise_mps;
|
||||||
|
static int s_seg;
|
||||||
|
static double s_pos_m; // distance along the current segment
|
||||||
|
|
||||||
|
static double segment_speed(int seg, double pos_m)
|
||||||
|
{
|
||||||
|
double v = s_cruise_mps;
|
||||||
|
double pull_away = sqrt(s_corner_mps[seg] * s_corner_mps[seg] + 2.0 * ACCEL_MPS2 * pos_m);
|
||||||
|
int next = (seg + 1) % s_n;
|
||||||
|
double braking = sqrt(s_corner_mps[next] * s_corner_mps[next]
|
||||||
|
+ 2.0 * DECEL_MPS2 * (s_len[seg] - pos_m));
|
||||||
|
if (pull_away < v) v = pull_away;
|
||||||
|
if (braking < v) v = braking;
|
||||||
|
return v;
|
||||||
|
}
|
||||||
|
|
||||||
|
int route_init(const route_point_t *points, int n, double cruise_mps, double min_corner_mps)
|
||||||
|
{
|
||||||
|
if (n < 2 || n > ROUTE_MAX_POINTS) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
s_pts = points;
|
||||||
|
s_n = n;
|
||||||
|
s_cruise_mps = cruise_mps;
|
||||||
|
|
||||||
|
for (int i = 0; i < n; i++) {
|
||||||
|
const route_point_t *a = &points[i];
|
||||||
|
const route_point_t *b = &points[(i + 1) % n];
|
||||||
|
double mid_lat = (a->latitude_tenmicrodeg + (double)b->latitude_tenmicrodeg) / 2e7;
|
||||||
|
double north_m = (b->latitude_tenmicrodeg - a->latitude_tenmicrodeg) / 1e7 * METRES_PER_DEG;
|
||||||
|
double east_m = (b->longitude_tenmicrodeg - a->longitude_tenmicrodeg) / 1e7 * METRES_PER_DEG
|
||||||
|
* cos(mid_lat * DEG_TO_RAD);
|
||||||
|
s_len[i] = sqrt(north_m * north_m + east_m * east_m);
|
||||||
|
if (s_len[i] < 0.01) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
double bearing = atan2(east_m, north_m) / DEG_TO_RAD;
|
||||||
|
s_bearing_deg[i] = bearing < 0 ? bearing + 360.0 : bearing;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Speed limit at each point from how tight the bend there is. A polyline bend of angle theta
|
||||||
|
// between segments of length L approximates an arc of radius L / theta, and a car takes a
|
||||||
|
// radius R at sqrt(a_lat * R). L is capped at BEND_SPAN_M: at a junction the two streets can be
|
||||||
|
// hundreds of metres long, but the car still turns within the width of the crossing.
|
||||||
|
for (int i = 0; i < n; i++) {
|
||||||
|
double turn = fabs(s_bearing_deg[i] - s_bearing_deg[(i + n - 1) % n]);
|
||||||
|
if (turn > 180.0) {
|
||||||
|
turn = 360.0 - turn;
|
||||||
|
}
|
||||||
|
double v = cruise_mps;
|
||||||
|
if (turn > STRAIGHT_DEG) {
|
||||||
|
double span = s_len[(i + n - 1) % n] < s_len[i] ? s_len[(i + n - 1) % n] : s_len[i];
|
||||||
|
if (span > BEND_SPAN_M) {
|
||||||
|
span = BEND_SPAN_M;
|
||||||
|
}
|
||||||
|
v = sqrt(LATERAL_MPS2 * span / (turn * DEG_TO_RAD));
|
||||||
|
}
|
||||||
|
if (v > cruise_mps) v = cruise_mps;
|
||||||
|
if (v < min_corner_mps) v = min_corner_mps;
|
||||||
|
s_corner_mps[i] = v;
|
||||||
|
}
|
||||||
|
|
||||||
|
// A point's limit also has to respect the bends after it (the car must be able to brake for
|
||||||
|
// them within the segments in between) and before it (it can only have sped up so much since).
|
||||||
|
// segment_speed only looks at the two ends of a segment, so settle this here. Limits only ever
|
||||||
|
// go down, so repeating the two passes until nothing changes terminates.
|
||||||
|
for (int changed = 1; changed;) {
|
||||||
|
changed = 0;
|
||||||
|
for (int k = 0; k < 2 * n; k++) {
|
||||||
|
int i = (2 * n - 1 - k) % n; // backwards: braking
|
||||||
|
int next = (i + 1) % n;
|
||||||
|
double v = sqrt(s_corner_mps[next] * s_corner_mps[next] + 2.0 * DECEL_MPS2 * s_len[i]);
|
||||||
|
if (v < s_corner_mps[i] - 1e-9) { s_corner_mps[i] = v; changed = 1; }
|
||||||
|
}
|
||||||
|
for (int k = 0; k < 2 * n; k++) {
|
||||||
|
int i = k % n; // forwards: accelerating
|
||||||
|
int next = (i + 1) % n;
|
||||||
|
double v = sqrt(s_corner_mps[i] * s_corner_mps[i] + 2.0 * ACCEL_MPS2 * s_len[i]);
|
||||||
|
if (v < s_corner_mps[next] - 1e-9) { s_corner_mps[next] = v; changed = 1; }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
s_seg = 0;
|
||||||
|
s_pos_m = 0.0;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void route_step(double dt_s, route_state_t *out)
|
||||||
|
{
|
||||||
|
// Advance with the speed at the start of the step; at 100 ms steps the error is well under a metre.
|
||||||
|
double d = segment_speed(s_seg, s_pos_m) * dt_s;
|
||||||
|
while (s_pos_m + d >= s_len[s_seg]) {
|
||||||
|
d -= s_len[s_seg] - s_pos_m;
|
||||||
|
s_seg = (s_seg + 1) % s_n;
|
||||||
|
s_pos_m = 0.0;
|
||||||
|
}
|
||||||
|
s_pos_m += d;
|
||||||
|
|
||||||
|
const route_point_t *a = &s_pts[s_seg];
|
||||||
|
const route_point_t *b = &s_pts[(s_seg + 1) % s_n];
|
||||||
|
double f = s_pos_m / s_len[s_seg];
|
||||||
|
out->latitude_tenmicrodeg = (int32_t)lround(a->latitude_tenmicrodeg
|
||||||
|
+ f * (b->latitude_tenmicrodeg - a->latitude_tenmicrodeg));
|
||||||
|
out->longitude_tenmicrodeg = (int32_t)lround(a->longitude_tenmicrodeg
|
||||||
|
+ f * (b->longitude_tenmicrodeg - a->longitude_tenmicrodeg));
|
||||||
|
out->speed_cm_s = (uint16_t)lround(segment_speed(s_seg, s_pos_m) * 100.0);
|
||||||
|
out->heading_ddeg = (uint16_t)(lround(s_bearing_deg[s_seg] * 10.0) % 3600);
|
||||||
|
out->segment = s_seg;
|
||||||
|
}
|
||||||
@@ -0,0 +1,40 @@
|
|||||||
|
#ifndef ROUTE_H
|
||||||
|
#define ROUTE_H
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
// Simulated drive around a closed loop of waypoints, so the beacon looks like a
|
||||||
|
// car going round the block instead of a parked one.
|
||||||
|
//
|
||||||
|
// The car follows straight lines between the points and goes from the last one
|
||||||
|
// back to the first, forever. For street-following, the points are the street
|
||||||
|
// geometry from tools/make_route.py (main/route_points.h). Speed is a function
|
||||||
|
// of where the car is on a segment: it cruises, brakes ahead of each bend down to
|
||||||
|
// the speed that bend allows, and accelerates away after it. Heading is the
|
||||||
|
// bearing of the current segment.
|
||||||
|
//
|
||||||
|
// Uses only standard headers, so it also compiles on the host for testing.
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int32_t latitude_tenmicrodeg; // 1/10 microdegree, same units as the CAM
|
||||||
|
int32_t longitude_tenmicrodeg;
|
||||||
|
} route_point_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
int32_t latitude_tenmicrodeg;
|
||||||
|
int32_t longitude_tenmicrodeg;
|
||||||
|
uint16_t speed_cm_s; // SpeedValue units (0.01 m/s)
|
||||||
|
uint16_t heading_ddeg; // HeadingValue units (0.1 deg, 0..3599, 0 = north, clockwise)
|
||||||
|
int segment; // index of the route point the car last passed
|
||||||
|
} route_state_t;
|
||||||
|
|
||||||
|
#define ROUTE_MAX_POINTS 512 // keep in step with MAX_POINTS in tools/make_route.py
|
||||||
|
|
||||||
|
// `points` must stay valid for as long as the route is used. Returns 0, or -1
|
||||||
|
// if n is out of range (2..ROUTE_MAX_POINTS) or a segment has zero length.
|
||||||
|
// min_corner_mps is the slowest the car ever goes (hairpins, U-turns).
|
||||||
|
int route_init(const route_point_t *points, int n, double cruise_mps, double min_corner_mps);
|
||||||
|
|
||||||
|
// Moves the car on by dt_s seconds and writes its new position into `out`.
|
||||||
|
void route_step(double dt_s, route_state_t *out);
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,116 @@
|
|||||||
|
// GENERATED by tools/make_route.py - do not edit by hand; change WAYPOINTS there and rerun.
|
||||||
|
// Route data (c) OpenStreetMap contributors, ODbL. Routing by OSRM.
|
||||||
|
//
|
||||||
|
// Driving loop through 6 waypoints: 5179 m (legs 147 m, 1837 m, 381 m, 819 m, 1234 m, 761 m), 103 points after
|
||||||
|
// simplifying to 1.5 m. Streets: Beim Strohhause, Berlinertordamm, Berliner Tor, Bei der Hauptfeuerwache, Westphalensweg, Berliner Tor, Berlinertordamm, Borgfelder Straße, Anckelmannstraße, Anckelmannsplatz, Bürgerweide, Wallstraße, Lübeckertordamm, Steindamm, Kreuzweg, Adenauerallee, Nagelsweg, Norderstraße, Repsoldstraße, Kurt-Schumacher-Allee, Kreuzweg, Adenauerallee, Kurt-Schumacher-Allee, Beim Strohhause.
|
||||||
|
#ifndef ROUTE_POINTS_H
|
||||||
|
#define ROUTE_POINTS_H
|
||||||
|
#include "route.h"
|
||||||
|
|
||||||
|
static const route_point_t route_points[] = {
|
||||||
|
{ 535531770, 100220980 },
|
||||||
|
{ 535534470, 100240600 },
|
||||||
|
{ 535535790, 100244160 },
|
||||||
|
{ 535536400, 100244520 },
|
||||||
|
{ 535537130, 100244160 },
|
||||||
|
{ 535538480, 100242840 },
|
||||||
|
{ 535538720, 100242140 },
|
||||||
|
{ 535540390, 100240200 },
|
||||||
|
{ 535548880, 100233930 },
|
||||||
|
{ 535549470, 100235130 },
|
||||||
|
{ 535554140, 100253280 },
|
||||||
|
{ 535553570, 100254580 },
|
||||||
|
{ 535546070, 100251700 },
|
||||||
|
{ 535543130, 100250020 },
|
||||||
|
{ 535542570, 100249130 },
|
||||||
|
{ 535542760, 100247800 },
|
||||||
|
{ 535539980, 100244600 },
|
||||||
|
{ 535538720, 100242140 },
|
||||||
|
{ 535538140, 100241960 },
|
||||||
|
{ 535535720, 100243380 },
|
||||||
|
{ 535535300, 100244470 },
|
||||||
|
{ 535535090, 100246580 },
|
||||||
|
{ 535536830, 100264460 },
|
||||||
|
{ 535537600, 100270920 },
|
||||||
|
{ 535538520, 100275220 },
|
||||||
|
{ 535541110, 100290830 },
|
||||||
|
{ 535540170, 100291550 },
|
||||||
|
{ 535539890, 100294430 },
|
||||||
|
{ 535539580, 100295330 },
|
||||||
|
{ 535532290, 100299300 },
|
||||||
|
{ 535527980, 100302450 },
|
||||||
|
{ 535525130, 100291880 },
|
||||||
|
{ 535524220, 100289760 },
|
||||||
|
{ 535522900, 100287820 },
|
||||||
|
{ 535522890, 100285300 },
|
||||||
|
{ 535522610, 100282810 },
|
||||||
|
{ 535520620, 100276050 },
|
||||||
|
{ 535519820, 100271130 },
|
||||||
|
{ 535519670, 100269030 },
|
||||||
|
{ 535520220, 100267120 },
|
||||||
|
{ 535520930, 100262840 },
|
||||||
|
{ 535521850, 100260020 },
|
||||||
|
{ 535522840, 100258210 },
|
||||||
|
{ 535527790, 100257880 },
|
||||||
|
{ 535538650, 100261370 },
|
||||||
|
{ 535551660, 100266470 },
|
||||||
|
{ 535555790, 100268750 },
|
||||||
|
{ 535559620, 100271540 },
|
||||||
|
{ 535561120, 100272050 },
|
||||||
|
{ 535562430, 100271250 },
|
||||||
|
{ 535563830, 100269390 },
|
||||||
|
{ 535569610, 100260190 },
|
||||||
|
{ 535579530, 100242810 },
|
||||||
|
{ 535584090, 100237660 },
|
||||||
|
{ 535585850, 100234670 },
|
||||||
|
{ 535584970, 100230060 },
|
||||||
|
{ 535584000, 100227260 },
|
||||||
|
{ 535580730, 100222030 },
|
||||||
|
{ 535579100, 100218590 },
|
||||||
|
{ 535574500, 100208430 },
|
||||||
|
{ 535570560, 100199010 },
|
||||||
|
{ 535568130, 100194820 },
|
||||||
|
{ 535564910, 100187620 },
|
||||||
|
{ 535563990, 100184630 },
|
||||||
|
{ 535562540, 100181160 },
|
||||||
|
{ 535559780, 100176310 },
|
||||||
|
{ 535542180, 100136840 },
|
||||||
|
{ 535539720, 100133430 },
|
||||||
|
{ 535537350, 100132010 },
|
||||||
|
{ 535534760, 100131390 },
|
||||||
|
{ 535523840, 100132690 },
|
||||||
|
{ 535524980, 100155090 },
|
||||||
|
{ 535524890, 100156360 },
|
||||||
|
{ 535524440, 100157650 },
|
||||||
|
{ 535523030, 100158530 },
|
||||||
|
{ 535517610, 100158580 },
|
||||||
|
{ 535504440, 100168810 },
|
||||||
|
{ 535501360, 100156400 },
|
||||||
|
{ 535499250, 100144880 },
|
||||||
|
{ 535498470, 100133880 },
|
||||||
|
{ 535498630, 100126150 },
|
||||||
|
{ 535499110, 100121980 },
|
||||||
|
{ 535504260, 100117230 },
|
||||||
|
{ 535505720, 100115500 },
|
||||||
|
{ 535507630, 100112310 },
|
||||||
|
{ 535508400, 100111560 },
|
||||||
|
{ 535510000, 100110940 },
|
||||||
|
{ 535511390, 100119560 },
|
||||||
|
{ 535512440, 100122280 },
|
||||||
|
{ 535514510, 100132770 },
|
||||||
|
{ 535515020, 100134120 },
|
||||||
|
{ 535516630, 100135630 },
|
||||||
|
{ 535518260, 100136100 },
|
||||||
|
{ 535523950, 100134870 },
|
||||||
|
{ 535524980, 100155090 },
|
||||||
|
{ 535524890, 100156360 },
|
||||||
|
{ 535524440, 100157650 },
|
||||||
|
{ 535523470, 100158460 },
|
||||||
|
{ 535518560, 100158480 },
|
||||||
|
{ 535519070, 100162430 },
|
||||||
|
{ 535522260, 100178080 },
|
||||||
|
{ 535527880, 100197650 },
|
||||||
|
{ 535530060, 100209020 },
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
@@ -0,0 +1,170 @@
|
|||||||
|
"""Turn the beacon's waypoints into a street-following route for main/route_points.h.
|
||||||
|
|
||||||
|
Asks the OSRM demo server (router.project-osrm.org, OpenStreetMap data) for a driving route that
|
||||||
|
visits WAYPOINTS in order and returns to the first, thins the street geometry out, and writes:
|
||||||
|
|
||||||
|
main/route_points.h the C array the firmware drives (commit this)
|
||||||
|
tools/route_osrm.json the raw OSRM answer, so the header can be regenerated offline (--offline)
|
||||||
|
tools/route_map.html the route over an OpenStreetMap map, to check it before flashing
|
||||||
|
|
||||||
|
Each waypoint also gets a bearing (the direction towards the next waypoint), so OSRM snaps it onto
|
||||||
|
the carriageway going that way. Without it, points on divided roads land on the wrong side and
|
||||||
|
every leg grows a U-turn detour.
|
||||||
|
|
||||||
|
Usage (Python 3.8+, standard library only):
|
||||||
|
py tools/make_route.py fetch from OSRM, then write all three files
|
||||||
|
py -3 tools/make_route.py --offline rebuild from the saved route_osrm.json
|
||||||
|
|
||||||
|
Route data (c) OpenStreetMap contributors, ODbL. Routing by OSRM.
|
||||||
|
"""
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import pathlib
|
||||||
|
import urllib.request
|
||||||
|
|
||||||
|
# (latitude, longitude) in decimal degrees, in driving order. The route closes back to the first.
|
||||||
|
WAYPOINTS = [
|
||||||
|
(53.553309, 10.022043),
|
||||||
|
(53.553611, 10.024146),
|
||||||
|
(53.556164, 10.027121),
|
||||||
|
(53.558310, 10.023362),
|
||||||
|
(53.554062, 10.013515),
|
||||||
|
(53.551540, 10.013398),
|
||||||
|
]
|
||||||
|
BEARING_TOLERANCE_DEG = 60 # how far the road's direction may differ from the waypoint bearing
|
||||||
|
SIMPLIFY_M = 1.5 # drop points that move the line by less than this
|
||||||
|
MAX_POINTS = 512 # must match ROUTE_MAX_POINTS in main/route.h
|
||||||
|
|
||||||
|
HERE = pathlib.Path(__file__).resolve().parent
|
||||||
|
PROJECT = HERE.parent
|
||||||
|
OSRM_JSON = HERE / "route_osrm.json"
|
||||||
|
HEADER = PROJECT / "main" / "route_points.h"
|
||||||
|
MAP_HTML = HERE / "route_map.html"
|
||||||
|
METRES_PER_DEG = 111194.93
|
||||||
|
|
||||||
|
|
||||||
|
def to_xy(lat, lon, lat0):
|
||||||
|
return (lon * METRES_PER_DEG * math.cos(math.radians(lat0)), lat * METRES_PER_DEG)
|
||||||
|
|
||||||
|
|
||||||
|
def bearing(a, b):
|
||||||
|
north = b[0] - a[0]
|
||||||
|
east = (b[1] - a[1]) * math.cos(math.radians(a[0]))
|
||||||
|
return math.degrees(math.atan2(east, north)) % 360
|
||||||
|
|
||||||
|
|
||||||
|
def fetch():
|
||||||
|
n = len(WAYPOINTS)
|
||||||
|
pts = WAYPOINTS + [WAYPOINTS[0]]
|
||||||
|
bearings = [round(bearing(WAYPOINTS[i], WAYPOINTS[(i + 1) % n])) for i in range(n)]
|
||||||
|
bearings.append(bearings[0])
|
||||||
|
url = ("https://router.project-osrm.org/route/v1/driving/"
|
||||||
|
+ ";".join(f"{lon},{lat}" for lat, lon in pts)
|
||||||
|
+ "?overview=full&geometries=geojson&steps=true&bearings="
|
||||||
|
+ ";".join(f"{b},{BEARING_TOLERANCE_DEG}" for b in bearings))
|
||||||
|
req = urllib.request.Request(url, headers={"User-Agent": "MicrOBU-route-tool"})
|
||||||
|
with urllib.request.urlopen(req, timeout=30) as resp:
|
||||||
|
data = json.load(resp)
|
||||||
|
if data.get("code") != "Ok":
|
||||||
|
raise SystemExit(f"OSRM error: {data.get('code')} {data.get('message')}")
|
||||||
|
OSRM_JSON.write_text(json.dumps(data, indent=1), encoding="utf-8")
|
||||||
|
return data
|
||||||
|
|
||||||
|
|
||||||
|
def simplify(xy, tol):
|
||||||
|
"""Douglas-Peucker, iterative. Keeps the first and last point."""
|
||||||
|
keep = [False] * len(xy)
|
||||||
|
keep[0] = keep[-1] = True
|
||||||
|
stack = [(0, len(xy) - 1)]
|
||||||
|
while stack:
|
||||||
|
a, b = stack.pop()
|
||||||
|
(ax, ay), (bx, by) = xy[a], xy[b]
|
||||||
|
dx, dy = bx - ax, by - ay
|
||||||
|
seg2 = dx * dx + dy * dy
|
||||||
|
worst, worst_d = -1, tol
|
||||||
|
for i in range(a + 1, b):
|
||||||
|
px, py = xy[i]
|
||||||
|
if seg2 == 0:
|
||||||
|
d = math.hypot(px - ax, py - ay)
|
||||||
|
else:
|
||||||
|
t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / seg2))
|
||||||
|
d = math.hypot(px - ax - t * dx, py - ay - t * dy)
|
||||||
|
if d > worst_d:
|
||||||
|
worst, worst_d = i, d
|
||||||
|
if worst >= 0:
|
||||||
|
keep[worst] = True
|
||||||
|
stack += [(a, worst), (worst, b)]
|
||||||
|
return [i for i, k in enumerate(keep) if k]
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
ap = argparse.ArgumentParser()
|
||||||
|
ap.add_argument("--offline", action="store_true", help="use the saved route_osrm.json")
|
||||||
|
args = ap.parse_args()
|
||||||
|
data = json.loads(OSRM_JSON.read_text(encoding="utf-8")) if args.offline else fetch()
|
||||||
|
route = data["routes"][0]
|
||||||
|
|
||||||
|
# GeoJSON is [lon, lat]; round to the CAM's 1/10-microdegree grid and drop repeats.
|
||||||
|
raw = []
|
||||||
|
for lon, lat in route["geometry"]["coordinates"]:
|
||||||
|
p = (round(lat * 1e7), round(lon * 1e7))
|
||||||
|
if not raw or p != raw[-1]:
|
||||||
|
raw.append(p)
|
||||||
|
if raw[0] == raw[-1]:
|
||||||
|
raw.pop()
|
||||||
|
closed = raw + [raw[0]]
|
||||||
|
lat0 = closed[0][0] / 1e7
|
||||||
|
xy = [to_xy(p[0] / 1e7, p[1] / 1e7, lat0) for p in closed]
|
||||||
|
pts = [closed[i] for i in simplify(xy, SIMPLIFY_M)][:-1]
|
||||||
|
if len(pts) > MAX_POINTS:
|
||||||
|
raise SystemExit(f"{len(pts)} points is more than MAX_POINTS={MAX_POINTS}; raise SIMPLIFY_M")
|
||||||
|
|
||||||
|
streets = []
|
||||||
|
for leg in route["legs"]:
|
||||||
|
for step in leg["steps"]:
|
||||||
|
if step["name"] and (not streets or streets[-1] != step["name"]):
|
||||||
|
streets.append(step["name"])
|
||||||
|
legs = ", ".join(f"{round(l['distance'])} m" for l in route["legs"])
|
||||||
|
|
||||||
|
lines = [
|
||||||
|
"// GENERATED by tools/make_route.py - do not edit by hand; change WAYPOINTS there and rerun.",
|
||||||
|
"// Route data (c) OpenStreetMap contributors, ODbL. Routing by OSRM.",
|
||||||
|
"//",
|
||||||
|
f"// Driving loop through {len(WAYPOINTS)} waypoints: {round(route['distance'])} m "
|
||||||
|
f"(legs {legs}), {len(pts)} points after",
|
||||||
|
f"// simplifying to {SIMPLIFY_M} m. Streets: {', '.join(streets)}.",
|
||||||
|
"#ifndef ROUTE_POINTS_H",
|
||||||
|
"#define ROUTE_POINTS_H",
|
||||||
|
'#include "route.h"',
|
||||||
|
"",
|
||||||
|
"static const route_point_t route_points[] = {",
|
||||||
|
]
|
||||||
|
lines += [f" {{ {lat}, {lon} }}," for lat, lon in pts]
|
||||||
|
lines += ["};", "", "#endif", ""]
|
||||||
|
HEADER.write_text("\n".join(lines), encoding="utf-8", newline="\n")
|
||||||
|
|
||||||
|
MAP_HTML.write_text(f"""<!doctype html><meta charset="utf-8"><title>Beacon route</title>
|
||||||
|
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css">
|
||||||
|
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
|
||||||
|
<style>html,body,#m{{height:100%;margin:0}}</style><div id="m"></div><script>
|
||||||
|
const pts={json.dumps([[p[0] / 1e7, p[1] / 1e7] for p in pts])};
|
||||||
|
const wps={json.dumps(WAYPOINTS)};
|
||||||
|
const m=L.map('m');
|
||||||
|
L.tileLayer('https://tile.openstreetmap.org/{{z}}/{{x}}/{{y}}.png',{{maxZoom:19,
|
||||||
|
attribution:'© OpenStreetMap contributors'}}).addTo(m);
|
||||||
|
const line=L.polyline(pts.concat([pts[0]]),{{color:'#d33',weight:4}}).addTo(m);
|
||||||
|
wps.forEach((w,i)=>L.marker(w,{{title:'wp'+(i+1)}}).bindTooltip('wp'+(i+1),{{permanent:true}}).addTo(m));
|
||||||
|
L.circleMarker(pts[0],{{radius:7,color:'#060'}}).bindTooltip('start').addTo(m);
|
||||||
|
m.fitBounds(line.getBounds(),{{padding:[20,20]}});
|
||||||
|
</script>
|
||||||
|
""", encoding="utf-8")
|
||||||
|
|
||||||
|
print(f"{round(route['distance'])} m, legs {legs}")
|
||||||
|
print(f"{len(route['geometry']['coordinates'])} OSRM points -> {len(pts)} after simplifying")
|
||||||
|
print(f"wrote {HEADER.relative_to(PROJECT)}, {OSRM_JSON.relative_to(PROJECT)}, "
|
||||||
|
f"{MAP_HTML.relative_to(PROJECT)}")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
<!doctype html><meta charset="utf-8"><title>Beacon route</title>
|
||||||
|
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css">
|
||||||
|
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
|
||||||
|
<style>html,body,#m{height:100%;margin:0}</style><div id="m"></div><script>
|
||||||
|
const pts=[[53.553177, 10.022098], [53.553447, 10.02406], [53.553579, 10.024416], [53.55364, 10.024452], [53.553713, 10.024416], [53.553848, 10.024284], [53.553872, 10.024214], [53.554039, 10.02402], [53.554888, 10.023393], [53.554947, 10.023513], [53.555414, 10.025328], [53.555357, 10.025458], [53.554607, 10.02517], [53.554313, 10.025002], [53.554257, 10.024913], [53.554276, 10.02478], [53.553998, 10.02446], [53.553872, 10.024214], [53.553814, 10.024196], [53.553572, 10.024338], [53.55353, 10.024447], [53.553509, 10.024658], [53.553683, 10.026446], [53.55376, 10.027092], [53.553852, 10.027522], [53.554111, 10.029083], [53.554017, 10.029155], [53.553989, 10.029443], [53.553958, 10.029533], [53.553229, 10.02993], [53.552798, 10.030245], [53.552513, 10.029188], [53.552422, 10.028976], [53.55229, 10.028782], [53.552289, 10.02853], [53.552261, 10.028281], [53.552062, 10.027605], [53.551982, 10.027113], [53.551967, 10.026903], [53.552022, 10.026712], [53.552093, 10.026284], [53.552185, 10.026002], [53.552284, 10.025821], [53.552779, 10.025788], [53.553865, 10.026137], [53.555166, 10.026647], [53.555579, 10.026875], [53.555962, 10.027154], [53.556112, 10.027205], [53.556243, 10.027125], [53.556383, 10.026939], [53.556961, 10.026019], [53.557953, 10.024281], [53.558409, 10.023766], [53.558585, 10.023467], [53.558497, 10.023006], [53.5584, 10.022726], [53.558073, 10.022203], [53.55791, 10.021859], [53.55745, 10.020843], [53.557056, 10.019901], [53.556813, 10.019482], [53.556491, 10.018762], [53.556399, 10.018463], [53.556254, 10.018116], [53.555978, 10.017631], [53.554218, 10.013684], [53.553972, 10.013343], [53.553735, 10.013201], [53.553476, 10.013139], [53.552384, 10.013269], [53.552498, 10.015509], [53.552489, 10.015636], [53.552444, 10.015765], [53.552303, 10.015853], [53.551761, 10.015858], [53.550444, 10.016881], [53.550136, 10.01564], [53.549925, 10.014488], [53.549847, 10.013388], [53.549863, 10.012615], [53.549911, 10.012198], [53.550426, 10.011723], [53.550572, 10.01155], [53.550763, 10.011231], [53.55084, 10.011156], [53.551, 10.011094], [53.551139, 10.011956], [53.551244, 10.012228], [53.551451, 10.013277], [53.551502, 10.013412], [53.551663, 10.013563], [53.551826, 10.01361], [53.552395, 10.013487], [53.552498, 10.015509], [53.552489, 10.015636], [53.552444, 10.015765], [53.552347, 10.015846], [53.551856, 10.015848], [53.551907, 10.016243], [53.552226, 10.017808], [53.552788, 10.019765], [53.553006, 10.020902]];
|
||||||
|
const wps=[[53.553309, 10.022043], [53.553611, 10.024146], [53.556164, 10.027121], [53.55831, 10.023362], [53.554062, 10.013515], [53.55154, 10.013398]];
|
||||||
|
const m=L.map('m');
|
||||||
|
L.tileLayer('https://tile.openstreetmap.org/{z}/{x}/{y}.png',{maxZoom:19,
|
||||||
|
attribution:'© OpenStreetMap contributors'}).addTo(m);
|
||||||
|
const line=L.polyline(pts.concat([pts[0]]),{color:'#d33',weight:4}).addTo(m);
|
||||||
|
wps.forEach((w,i)=>L.marker(w,{title:'wp'+(i+1)}).bindTooltip('wp'+(i+1),{permanent:true}).addTo(m));
|
||||||
|
L.circleMarker(pts[0],{radius:7,color:'#060'}).bindTooltip('start').addTo(m);
|
||||||
|
m.fitBounds(line.getBounds(),{padding:[20,20]});
|
||||||
|
</script>
|
||||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user