Ashin Walpola
0e9525162d
obu-firmware builds against vanetza-idf from microbu-esp32c5/external, but that tree was gitignored, so a clone of this repository could not build the firmware it ships. It is now committed here as ordinary files in its own folder, microbu-esp32c5/: the colleague's commit cf4b99f plus the V2X2MAP bridge's signature verification (--trust) used on the bench. Nothing is fetched from or pushed to the colleague's repository; this repository and its remotes carry everything. The folder's own .gitignore keeps build output, downloaded components and private key material out, as it did there; the committed file set is identical to that repository's tracked files. The ESP32-C5 is still flashed from obu-firmware/, which only takes vanetza-idf from microbu-esp32c5/, so the two stay separate folders. FLASHING.md says how to take a newer version of the colleague's tree (copy it over the folder, rebuild, test, commit).
240 lines
11 KiB
Python
240 lines
11 KiB
Python
"""A small VRU basic service for the phone emulator: VAM assembly with asn1tools from the
|
|
ETSI ASN.1 modules, the individual-VAM generation rules of ETSI TS 103 300-3 V2.3.1
|
|
clause 6.4 (items 1 to 4, Tables 16 and 17), the VBS PCI of Table 4, and a PoTi stand-in.
|
|
|
|
It is a test stand-in for the phone app's VBS, not a conformant VBS: no clustering,
|
|
no redundancy mitigation, no reception management, no motion prediction.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import math
|
|
import time
|
|
from dataclasses import dataclass, field
|
|
from pathlib import Path
|
|
from typing import Optional
|
|
|
|
from . import messages as m
|
|
|
|
ITS_EPOCH_UNIX = 1072915200
|
|
LEAP_SECONDS_SINCE_2004 = 5 # TAI-UTC 37 s now, 32 s at the epoch (2017-01-01 was the last insertion)
|
|
|
|
# ETSI TS 103 300-3 V2.3.1 Table 16 / Table 17 recommended values
|
|
T_GEN_VAM_MIN_MS = 100
|
|
T_GEN_VAM_MAX_MS = 5000
|
|
T_GEN_VAM_LF_MIN_MS = 2000
|
|
MIN_POSITION_CHANGE_M = 4.0
|
|
MIN_SPEED_CHANGE_MPS = 0.5
|
|
MIN_ORIENTATION_CHANGE_DEG = 4.0
|
|
|
|
# TS 102 965 ITS-AID of the VRU service, TS 103 248 BTP port of VAM
|
|
ITS_AID_VRU = 638
|
|
BTP_PORT_VAM = 2018
|
|
|
|
|
|
def its_timestamp_ms(unix_seconds: Optional[float] = None) -> int:
|
|
"""TS 102 894-2 TimestampIts: TAI milliseconds since 2004-01-01T00:00:00Z."""
|
|
if unix_seconds is None:
|
|
unix_seconds = time.time()
|
|
return int((unix_seconds - ITS_EPOCH_UNIX + LEAP_SECONDS_SINCE_2004) * 1000)
|
|
|
|
|
|
def default_asn1_directory() -> Path:
|
|
here = Path(__file__).resolve()
|
|
return here.parents[3] / 'external' / 'vanetza-idf' / 'asn1' / 'release2'
|
|
|
|
|
|
class Codec:
|
|
"""UPER codec for VAM (and CAM/DENM for the displays) from the submodule's ASN.1 modules."""
|
|
|
|
def __init__(self, directory: Optional[Path] = None):
|
|
import asn1tools
|
|
directory = directory or default_asn1_directory()
|
|
cdd = directory / 'TS102894-2v241-CDD.asn'
|
|
self.vam = asn1tools.compile_files([cdd, directory / 'TS103300-3v231' / 'VAM-PDU-Descriptions.asn',
|
|
directory / 'TS103300-3v231' / 'motorcyclist-special-container.asn'], 'uper')
|
|
self.vam_type = self.vam.modules['VAM-PDU-Descriptions']['VAM']
|
|
self.cam_type = None
|
|
self.denm_type = None
|
|
try:
|
|
cam = asn1tools.compile_files([cdd, directory / 'TS103900v231-CAM.asn'], 'uper')
|
|
self.cam_type = cam.modules['CAM-PDU-Descriptions']['CAM']
|
|
except Exception:
|
|
pass
|
|
try:
|
|
denm = asn1tools.compile_files([cdd, directory / 'TS103831v231-DENM.asn'], 'uper')
|
|
self.denm_type = denm.modules['DENM-PDU-Description']['DENM']
|
|
except Exception:
|
|
pass
|
|
|
|
def encode_vam(self, vam: dict) -> bytes:
|
|
return self.vam_type.encode(vam)
|
|
|
|
def decode_vam(self, octets: bytes) -> dict:
|
|
return self.vam_type.decode(octets)
|
|
|
|
def decode_by_port(self, port: int, octets: bytes):
|
|
if port == BTP_PORT_VAM:
|
|
return 'VAM', self.decode_vam(octets)
|
|
if port == 2001 and self.cam_type:
|
|
return 'CAM', self.cam_type.decode(octets)
|
|
if port == 2002 and self.denm_type:
|
|
return 'DENM', self.denm_type.decode(octets)
|
|
return None, None
|
|
|
|
|
|
@dataclass
|
|
class PotiState:
|
|
"""What PoTi reports: WGS84 position with a 95 % confidence ellipse, kinematics, time."""
|
|
timestamp_ms: int
|
|
latitude: float
|
|
longitude: float
|
|
speed_mps: float = 0.0
|
|
heading_deg: float = 0.0
|
|
altitude_m: Optional[float] = None
|
|
semi_major_m: float = 1.5
|
|
semi_minor_m: float = 1.0
|
|
orientation_deg: float = 0.0
|
|
|
|
def poti_update(self) -> m.PotiUpdate:
|
|
return m.PotiUpdate(
|
|
timestamp_ms=self.timestamp_ms,
|
|
latitude=int(round(self.latitude * 1e7)), longitude=int(round(self.longitude * 1e7)),
|
|
semi_major_cm=min(65535, int(round(self.semi_major_m * 100))),
|
|
semi_minor_cm=min(65535, int(round(self.semi_minor_m * 100))),
|
|
orientation_deci_degree=int(round(self.orientation_deg * 10)) % 3600,
|
|
altitude_cm=None if self.altitude_m is None else int(round(self.altitude_m * 100)),
|
|
speed_cm_s=min(65535, int(round(self.speed_mps * 100))),
|
|
heading_deci_degree=int(round(self.heading_deg * 10)) % 3600,
|
|
pai=self.semi_major_m * 2 <= 40.0) # itsGnPaiInterval / 2, EN 302 636-4-1 / TS 103 836-4-1 clause 8.4
|
|
|
|
|
|
class PotiSimulator:
|
|
"""Static position, or a bicycle riding a circle (radius r at speed v) for the triggers."""
|
|
|
|
def __init__(self, latitude: float, longitude: float, speed_mps: float = 0.0, radius_m: float = 30.0,
|
|
altitude_m: Optional[float] = 12.0):
|
|
self.center = (latitude, longitude)
|
|
self.speed = speed_mps
|
|
self.radius = radius_m
|
|
self.altitude = altitude_m
|
|
self.start = time.time()
|
|
|
|
def state(self, now: Optional[float] = None) -> PotiState:
|
|
now = time.time() if now is None else now
|
|
if self.speed <= 0:
|
|
return PotiState(its_timestamp_ms(now), self.center[0], self.center[1], 0.0, 0.0, self.altitude)
|
|
angle = (now - self.start) * self.speed / self.radius # rad, counter-clockwise
|
|
lat = self.center[0] + math.degrees(self.radius * math.sin(angle) / 6371000.0)
|
|
lon = self.center[1] + math.degrees(self.radius * math.cos(angle) / (6371000.0 * math.cos(math.radians(self.center[0]))))
|
|
heading = (math.degrees(angle) * -1 + 0.0) % 360 # tangent of a counter-clockwise circle, clockwise from north
|
|
return PotiState(its_timestamp_ms(now), lat, lon, self.speed, heading, self.altitude)
|
|
|
|
|
|
def haversine_m(a: PotiState, b: PotiState) -> float:
|
|
r = 6371000.0
|
|
p1, p2 = math.radians(a.latitude), math.radians(b.latitude)
|
|
dp = p2 - p1
|
|
dl = math.radians(b.longitude - a.longitude)
|
|
h = math.sin(dp / 2) ** 2 + math.cos(p1) * math.cos(p2) * math.sin(dl / 2) ** 2
|
|
return 2 * r * math.asin(math.sqrt(h))
|
|
|
|
|
|
@dataclass
|
|
class VbsLite:
|
|
"""VRU-ACTIVE-STANDALONE VBS state for one bicyclist (VRU profile 2, sub-profile bicyclist)."""
|
|
codec: Codec
|
|
station_id: int
|
|
t_gen_vam_ms: int = T_GEN_VAM_MIN_MS # T_GenVam, from the management entity (DCC) in a real VBS
|
|
fixed_period_ms: Optional[int] = None # test override: one VAM every period regardless of the triggers
|
|
active: bool = True # false between PREPARE and COMMIT of an identifier change
|
|
last_vam: Optional[PotiState] = None
|
|
last_vam_at_ms: int = 0
|
|
last_lf_at_ms: int = -10 ** 9
|
|
generated: int = 0
|
|
security_profile: int = 2 # Table 4: SECURED or UNSECURED
|
|
ssp: bytes = b'\x01' # SSP of the VRU ITS-AID carried in the authorization ticket
|
|
traffic_class: int = 0xFF # "same GN traffic class value as for the CAM": the station default
|
|
lifetime: int = 0x05 # GN maximum packet lifetime 1 s (Table 4: shall not exceed 1 000 ms)
|
|
|
|
def due(self, now: PotiState) -> bool:
|
|
if not self.active:
|
|
return False
|
|
elapsed = now.timestamp_ms - self.last_vam_at_ms
|
|
if self.fixed_period_ms is not None:
|
|
return self.last_vam is None or elapsed >= self.fixed_period_ms
|
|
if self.last_vam is None:
|
|
return True
|
|
if elapsed < max(self.t_gen_vam_ms, T_GEN_VAM_MIN_MS):
|
|
return False
|
|
if elapsed > T_GEN_VAM_MAX_MS:
|
|
return True # item 1
|
|
if haversine_m(now, self.last_vam) > MIN_POSITION_CHANGE_M:
|
|
return True # item 2
|
|
if abs(now.speed_mps - self.last_vam.speed_mps) > MIN_SPEED_CHANGE_MPS:
|
|
return True # item 3
|
|
delta = abs((now.heading_deg - self.last_vam.heading_deg + 180) % 360 - 180)
|
|
return delta > MIN_ORIENTATION_CHANGE_DEG # item 4
|
|
|
|
def assemble(self, now: PotiState) -> bytes:
|
|
"""VAM per TS 103 300-3 clause 7.3: basic + HF container, LF container every T_GenVamLFMin."""
|
|
include_lf = now.timestamp_ms - self.last_lf_at_ms >= T_GEN_VAM_LF_MIN_MS
|
|
vam = {
|
|
'header': {'protocolVersion': 3, 'messageId': 16, 'stationId': self.station_id},
|
|
'vam': {
|
|
'generationDeltaTime': now.timestamp_ms % 65536,
|
|
'vamParameters': {
|
|
'basicContainer': {
|
|
'stationType': 2, # cyclist
|
|
'referencePosition': {
|
|
'latitude': int(round(now.latitude * 1e7)),
|
|
'longitude': int(round(now.longitude * 1e7)),
|
|
'positionConfidenceEllipse': {
|
|
'semiMajorAxisLength': min(4094, int(round(now.semi_major_m * 100))),
|
|
'semiMinorAxisLength': min(4094, int(round(now.semi_minor_m * 100))),
|
|
'semiMajorAxisOrientation': int(round(now.orientation_deg * 10)) % 3600,
|
|
},
|
|
'altitude': ({'altitudeValue': max(-100000, min(800000, int(round(now.altitude_m * 100)))),
|
|
'altitudeConfidence': 'alt-020-00'} if now.altitude_m is not None
|
|
else {'altitudeValue': 800001, 'altitudeConfidence': 'unavailable'}),
|
|
},
|
|
},
|
|
'vruHighFrequencyContainer': {
|
|
'heading': {'value': int(round(now.heading_deg * 10)) % 3600, 'confidence': 50},
|
|
'speed': {'speedValue': min(16382, int(round(now.speed_mps * 100))), 'speedConfidence': 50},
|
|
'longitudinalAcceleration': {'longitudinalAccelerationValue': 0, 'longitudinalAccelerationConfidence': 102},
|
|
},
|
|
},
|
|
},
|
|
}
|
|
if include_lf:
|
|
vam['vam']['vamParameters']['vruLowFrequencyContainer'] = {
|
|
'profileAndSubprofile': ('bicyclistAndLightVruVehicle', 1), # bicyclist
|
|
'sizeClass': 1, # low
|
|
}
|
|
self.last_lf_at_ms = now.timestamp_ms
|
|
octets = self.codec.encode_vam(vam)
|
|
self.last_vam = now
|
|
self.last_vam_at_ms = now.timestamp_ms
|
|
self.generated += 1
|
|
return octets
|
|
|
|
def btp_data_request(self, vam: bytes) -> m.BtpDataRequest:
|
|
"""The VBS networking PCI of TS 103 300-3 Table 4 for one VAM."""
|
|
return m.BtpDataRequest(
|
|
fl_sdu=vam, btp_type=1, destination_port=BTP_PORT_VAM, destination_port_info=0,
|
|
gn_packet_transport_type=m.TransportType.SHB, gn_communication_profile=1,
|
|
gn_security_profile=self.security_profile, gn_traffic_class=self.traffic_class,
|
|
gn_maximum_packet_lifetime=self.lifetime, its_aid=ITS_AID_VRU, permissions=self.ssp)
|
|
|
|
# TS 103 300-3 clause 5.3.5: stop on PREPARE, resume with a new StationId after COMMIT
|
|
def prepare(self):
|
|
self.active = False
|
|
|
|
def commit(self, new_station_id: int):
|
|
self.station_id = new_station_id
|
|
self.active = True
|
|
self.last_vam = None
|
|
|
|
def abort(self):
|
|
self.active = True
|