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
|