Keep the colleague's microbu-esp32c5 tree in this repository
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).
This commit is contained in:
Ashin Walpola
@@ -0,0 +1,11 @@
|
||||
"""Phone side of the micrOBU station-internal link (implementation/station-link/README.md).
|
||||
|
||||
``messages`` is the transport-independent message layer, ``serial_transport`` the framing
|
||||
over native USB Serial/JTAG, ``ble_transport`` the laptop/phone-equivalent BLE GATT client,
|
||||
and ``client`` the shared request/reply API with unsolicited messages delivered to callbacks.
|
||||
"""
|
||||
from .messages import * # noqa: F401,F403
|
||||
from .transport import TransportAdapter # noqa: F401
|
||||
from .serial_transport import FrameType, SerialTransport, encode_frame, FrameDecoder # noqa: F401
|
||||
from .ble_transport import BleTransport # noqa: F401
|
||||
from .client import LinkClient, LinkError, TestChannel # noqa: F401
|
||||
@@ -0,0 +1,199 @@
|
||||
"""BLE GATT transport for the station-link message layer.
|
||||
|
||||
The ESP32 exposes the Nordic-UART-shaped service (NUS) documented in the station-link
|
||||
README. Values contain binary station-link messages (up to 512 bytes). Bleak
|
||||
runs on a private asyncio thread while the LinkClient remains a synchronous request/reply API.
|
||||
Fixed passkey bonding is used (passkey: 123456).
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import asyncio
|
||||
import queue
|
||||
import threading
|
||||
from typing import Callable, Optional
|
||||
|
||||
from . import messages as m
|
||||
from .serial_transport import FrameType
|
||||
|
||||
# Structured ETSI C-ITS Station Service UUIDs
|
||||
SERVICE_UUID = '0000c175-ba5e-4c17-8000-00805f9b34fb'
|
||||
BTP_REQUEST_UUID = '0000c176-ba5e-4c17-8000-00805f9b34fb'
|
||||
BTP_INDICATION_UUID = '0000c177-ba5e-4c17-8000-00805f9b34fb'
|
||||
POTI_UUID = '0000c178-ba5e-4c17-8000-00805f9b34fb'
|
||||
STATUS_UUID = '0000c179-ba5e-4c17-8000-00805f9b34fb'
|
||||
ID_EVENT_UUID = '0000c17a-ba5e-4c17-8000-00805f9b34fb'
|
||||
CONFIG_UUID = '0000c17b-ba5e-4c17-8000-00805f9b34fb'
|
||||
RESULT_UUID = '0000c17c-ba5e-4c17-8000-00805f9b34fb'
|
||||
FIXED_PASSKEY = 123456
|
||||
|
||||
|
||||
class BleTransport:
|
||||
"""Synchronous LinkClient adapter backed by Bleak on a private event loop.
|
||||
|
||||
Transmits complete station-link messages directly without ATT fragmentation chunking
|
||||
or queue delays.
|
||||
"""
|
||||
|
||||
def __init__(self, target: Optional[str], on_frame: Callable[[int, bytes], None],
|
||||
connect_timeout: float = 20.0, operation_timeout: float = 5.0):
|
||||
self.target = target or None
|
||||
self.on_frame = on_frame
|
||||
self.connect_timeout = connect_timeout
|
||||
self.operation_timeout = operation_timeout
|
||||
self.attribute_value = m.MAXIMUM_MESSAGE
|
||||
self.device_name = ''
|
||||
self.device_address = ''
|
||||
self.error: Optional[BaseException] = None
|
||||
self.client = None
|
||||
self.loop = asyncio.new_event_loop()
|
||||
self.stop_event = None
|
||||
self.ready = threading.Event()
|
||||
self.delivery: 'queue.Queue[Optional[bytes]]' = queue.Queue()
|
||||
self.delivery_thread = threading.Thread(target=self._deliver, name='microbu-ble-delivery', daemon=True)
|
||||
self.delivery_thread.start()
|
||||
self.thread = threading.Thread(target=self._thread_main, name='microbu-ble-asyncio', daemon=True)
|
||||
self.thread.start()
|
||||
if not self.ready.wait(connect_timeout + 5):
|
||||
self.close()
|
||||
raise TimeoutError('BLE scan/connect did not finish')
|
||||
if self.error:
|
||||
error = self.error
|
||||
self.close()
|
||||
raise ConnectionError('BLE station-link connection failed: %s' % error) from error
|
||||
|
||||
def _thread_main(self):
|
||||
asyncio.set_event_loop(self.loop)
|
||||
try:
|
||||
self.loop.run_until_complete(self._session())
|
||||
except BaseException as error:
|
||||
self.error = error
|
||||
self.ready.set()
|
||||
finally:
|
||||
self.loop.close()
|
||||
|
||||
async def _session(self):
|
||||
from bleak import BleakClient, BleakScanner
|
||||
|
||||
target = self.target.lower() if self.target else None
|
||||
|
||||
def matches(device, advertisement):
|
||||
names = (device.name or '', advertisement.local_name or '')
|
||||
services = [uuid.lower() for uuid in advertisement.service_uuids]
|
||||
if target:
|
||||
return target == device.address.lower() or any(target == name.lower() for name in names)
|
||||
return SERVICE_UUID in services or any(name.lower().startswith('microbu-') for name in names)
|
||||
|
||||
device = await BleakScanner.find_device_by_filter(matches, timeout=self.connect_timeout)
|
||||
if device is None:
|
||||
wanted = self.target or 'advertised micrOBU service'
|
||||
raise TimeoutError('no BLE device matching %s' % wanted)
|
||||
self.device_name = device.name or 'micrOBU'
|
||||
self.device_address = device.address
|
||||
|
||||
self.stop_event = asyncio.Event()
|
||||
self.client = BleakClient(device, timeout=self.connect_timeout, pair=True,
|
||||
disconnected_callback=self._disconnected)
|
||||
await self.client.connect()
|
||||
|
||||
# LE Secure Connections + passkey display takes ~1-3 s; wait for the link
|
||||
# to be encrypted before attempting CCCD writes on authenticated characteristics.
|
||||
for _ in range(30):
|
||||
await asyncio.sleep(0.1)
|
||||
try:
|
||||
# Attempt a read on STATUS (Read|Encrypt) as an encryption probe.
|
||||
# Once it succeeds (or gives ATT error other than InsufficientEncryption)
|
||||
# the link is encrypted and CCCD writes on notify characteristics will work.
|
||||
await self.client.read_gatt_char(STATUS_UUID)
|
||||
break
|
||||
except Exception as exc:
|
||||
exc_str = str(exc).lower()
|
||||
if 'insufficient' in exc_str or 'encrypt' in exc_str or 'auth' in exc_str:
|
||||
continue # still pairing
|
||||
break # any other error — proceed anyway
|
||||
|
||||
# Subscribe to peripheral notification characteristics that matter for normal operation
|
||||
# (BTP_INDICATION, ID_EVENT, RESULT). STATUS is polled or read on demand, but can be notified if supported.
|
||||
subscribed_uuids = set()
|
||||
for notif_uuid in (RESULT_UUID, BTP_INDICATION_UUID, ID_EVENT_UUID):
|
||||
for attempt in range(3):
|
||||
try:
|
||||
await self.client.start_notify(notif_uuid, self._notification)
|
||||
subscribed_uuids.add(notif_uuid)
|
||||
break
|
||||
except Exception as exc:
|
||||
if attempt < 2:
|
||||
await asyncio.sleep(0.5)
|
||||
else:
|
||||
print(f'BLE: start_notify on {notif_uuid[:8]} failed: {exc}')
|
||||
|
||||
self.attribute_value = min(self.client.mtu_size - 3, m.MAXIMUM_MESSAGE)
|
||||
self.ready.set()
|
||||
await self.stop_event.wait()
|
||||
if self.client.is_connected:
|
||||
await self.client.disconnect()
|
||||
|
||||
def _disconnected(self, _client):
|
||||
if self.stop_event and not self.stop_event.is_set():
|
||||
self.error = ConnectionError('micrOBU disconnected')
|
||||
self.stop_event.set()
|
||||
|
||||
def _notification(self, characteristic, data: bytearray):
|
||||
# Direct immediate message delivery: each GATT notification is one complete link message
|
||||
self.delivery.put(bytes(data))
|
||||
|
||||
def _deliver(self):
|
||||
while True:
|
||||
message = self.delivery.get()
|
||||
if message is None:
|
||||
return
|
||||
try:
|
||||
self.on_frame(FrameType.LINK, message)
|
||||
except Exception as error:
|
||||
print('BLE message handler error:', error)
|
||||
|
||||
def write(self, frame_type: int, payload: bytes):
|
||||
if frame_type != FrameType.LINK:
|
||||
raise NotImplementedError('the USB test/log channels do not exist over BLE')
|
||||
if self.error:
|
||||
raise ConnectionError(str(self.error))
|
||||
if self.client is None or not self.client.is_connected or self.loop.is_closed():
|
||||
raise ConnectionError('BLE station link is not ready')
|
||||
|
||||
# Route write to the specific ETSI C-ITS GATT characteristic.
|
||||
# Check opcode in link header (payload[0])
|
||||
target_uuid = CONFIG_UUID
|
||||
if payload:
|
||||
op = payload[0]
|
||||
if op == m.Opcode.BTP_DATA_REQUEST:
|
||||
target_uuid = BTP_REQUEST_UUID
|
||||
elif op == m.Opcode.POTI_UPDATE:
|
||||
target_uuid = POTI_UUID
|
||||
elif op == m.Opcode.SF_IDCHANGE_EVENT_RESPONSE:
|
||||
target_uuid = ID_EVENT_UUID
|
||||
else:
|
||||
target_uuid = CONFIG_UUID
|
||||
|
||||
async def send():
|
||||
for attempt in range(4):
|
||||
try:
|
||||
await self.client.write_gatt_char(target_uuid, bytes(payload), response=True)
|
||||
return
|
||||
except Exception as error:
|
||||
if 'insufficient resource' not in str(error).lower() or attempt == 3:
|
||||
raise
|
||||
await asyncio.sleep(0.1 * (attempt + 1))
|
||||
|
||||
future = asyncio.run_coroutine_threadsafe(send(), self.loop)
|
||||
try:
|
||||
future.result(timeout=self.operation_timeout)
|
||||
except Exception as error:
|
||||
raise ConnectionError('BLE write failed: %s' % error) from error
|
||||
|
||||
def close(self):
|
||||
if self.stop_event and not self.loop.is_closed():
|
||||
self.loop.call_soon_threadsafe(self.stop_event.set)
|
||||
if hasattr(self, 'thread') and self.thread.is_alive() and threading.current_thread() is not self.thread:
|
||||
self.thread.join(timeout=self.operation_timeout)
|
||||
self.delivery.put(None)
|
||||
if self.delivery_thread.is_alive() and threading.current_thread() is not self.delivery_thread:
|
||||
self.delivery_thread.join(timeout=1)
|
||||
@@ -0,0 +1,245 @@
|
||||
"""Request/reply client of the station-internal link plus the USB test channel."""
|
||||
from __future__ import annotations
|
||||
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from typing import Callable, Dict, List, Optional, Tuple
|
||||
|
||||
from . import messages as m
|
||||
from .serial_transport import FrameType, SerialTransport
|
||||
from .transport import TransportAdapter
|
||||
|
||||
|
||||
class LinkError(RuntimeError):
|
||||
def __init__(self, result: m.Result, request: str):
|
||||
super().__init__('%s refused: %s' % (request, result.name))
|
||||
self.result = result
|
||||
|
||||
|
||||
class LinkClient:
|
||||
"""The phone's view of the micrOBU: send requests, await their RESULT, receive the
|
||||
micrOBU's own messages (indications, identifier-change events, status, log lines)
|
||||
through callbacks. Thread-safe for one request at a time."""
|
||||
|
||||
def __init__(self, port: Optional[str] = None, timeout: float = 3.0,
|
||||
transport_factory: Optional[Callable[[Callable[[int, bytes], None]], TransportAdapter]] = None):
|
||||
self.timeout = timeout
|
||||
self.sequence = 0
|
||||
self.lock = threading.Lock()
|
||||
self.replies: 'queue.Queue[m.Message]' = queue.Queue()
|
||||
self.on_indication: Optional[Callable[[m.BtpDataIndication], None]] = None
|
||||
self.on_id_event: Optional[Callable[[m.IdChangeEvent, int], None]] = None
|
||||
self.on_mf_set: Optional[Callable[[m.MfSetRequest], None]] = None
|
||||
self.on_status: Optional[Callable[[m.Status], None]] = None
|
||||
self.on_log: Optional[Callable[[str], None]] = None
|
||||
self.last_status: Optional[m.Status] = None
|
||||
self.test_replies: 'queue.Queue[bytes]' = queue.Queue()
|
||||
if transport_factory:
|
||||
self.transport = transport_factory(self._on_frame)
|
||||
elif port:
|
||||
self.transport = SerialTransport(port, self._on_frame)
|
||||
else:
|
||||
raise ValueError('port or transport_factory is required')
|
||||
|
||||
def close(self):
|
||||
self.transport.close()
|
||||
|
||||
# ---- receive side ----
|
||||
def _on_frame(self, frame_type: int, payload: bytes):
|
||||
if frame_type == FrameType.LOG:
|
||||
if self.on_log:
|
||||
self.on_log(payload.decode('utf-8', 'replace'))
|
||||
return
|
||||
if frame_type == FrameType.TEST:
|
||||
self.test_replies.put(payload)
|
||||
return
|
||||
if frame_type != FrameType.LINK:
|
||||
return
|
||||
try:
|
||||
message = m.decode_message(payload)
|
||||
except m.DecodeError:
|
||||
return
|
||||
op = message.header.opcode
|
||||
if op == m.Opcode.RESULT or (op == m.Opcode.STATUS and message.header.sequence == self._awaited_status):
|
||||
self.replies.put(message)
|
||||
elif op == m.Opcode.BTP_DATA_INDICATION:
|
||||
if self.on_indication:
|
||||
self.on_indication(m.BtpDataIndication.decode(message.body))
|
||||
elif op == m.Opcode.SF_IDCHANGE_EVENT:
|
||||
if self.on_id_event:
|
||||
self.on_id_event(m.IdChangeEvent.decode(message.body), message.header.sequence)
|
||||
elif op == m.Opcode.MF_SET_REQUEST:
|
||||
if self.on_mf_set:
|
||||
self.on_mf_set(m.MfSetRequest.decode(message.body))
|
||||
elif op == m.Opcode.STATUS:
|
||||
self.last_status = m.Status.decode(message.body)
|
||||
if self.on_status:
|
||||
self.on_status(self.last_status)
|
||||
|
||||
_awaited_status = -1
|
||||
|
||||
# ---- send side ----
|
||||
def send(self, opcode: int, body: bytes = b'', sequence: Optional[int] = None) -> int:
|
||||
"""Send one message without waiting (indication responses, PoTi updates)."""
|
||||
if sequence is None:
|
||||
self.sequence = (self.sequence + 1) & 0xFFFF
|
||||
sequence = self.sequence
|
||||
self.transport.write(FrameType.LINK, m.encode_message(m.Message(m.Header(opcode, 0, sequence), body)))
|
||||
return sequence
|
||||
|
||||
def request(self, opcode: int, body: bytes = b'', timeout: Optional[float] = None) -> m.Result:
|
||||
"""Send a request and wait for its RESULT (raises LinkError unless accepted)."""
|
||||
with self.lock:
|
||||
while not self.replies.empty(): # stale replies of timed-out requests
|
||||
self.replies.get_nowait()
|
||||
sequence = self.send(opcode, body)
|
||||
deadline = time.monotonic() + (timeout or self.timeout)
|
||||
while True:
|
||||
remaining = deadline - time.monotonic()
|
||||
if remaining <= 0:
|
||||
raise TimeoutError('no RESULT for opcode 0x%02x (sequence %d)' % (opcode, sequence))
|
||||
try:
|
||||
reply = self.replies.get(timeout=remaining)
|
||||
except queue.Empty:
|
||||
continue
|
||||
if reply.header.sequence != sequence or reply.header.opcode != m.Opcode.RESULT:
|
||||
continue
|
||||
result = m.Result.decode(reply.body)
|
||||
if result.code != m.Code.accepted:
|
||||
raise LinkError(result, m.Opcode(opcode).name)
|
||||
return result
|
||||
|
||||
def request_async(self, opcode: int, body: bytes = b'') -> int:
|
||||
"""Submit a request without serializing on its RESULT.
|
||||
|
||||
The peer still emits the ordinary per-request RESULT. Call
|
||||
receive_result() to consume it. This is used for the 100 ms VAM stream,
|
||||
where a BLE connection interval must not become the service period.
|
||||
"""
|
||||
with self.lock:
|
||||
return self.send(opcode, body)
|
||||
|
||||
def receive_result(self, timeout: Optional[float] = None) -> Tuple[int, m.Result]:
|
||||
reply = self.replies.get(timeout=self.timeout if timeout is None else timeout)
|
||||
if reply.header.opcode != m.Opcode.RESULT:
|
||||
raise ValueError('expected RESULT, got opcode 0x%02x' % reply.header.opcode)
|
||||
return reply.header.sequence, m.Result.decode(reply.body)
|
||||
|
||||
# ---- primitives ----
|
||||
def configure(self, config: m.StationConfigure) -> m.StationInfo:
|
||||
# Starting the ESP32-C5 ITS-G5 radio initializes the Wi-Fi/PHY driver
|
||||
# before the station can reply. Keep ordinary request timeouts short,
|
||||
# but allow this idempotent lifecycle operation to finish on hardware.
|
||||
return m.StationInfo.decode(self.request(m.Opcode.STATION_CONFIGURE, config.encode(), timeout=30).detail)
|
||||
|
||||
def poti(self, update: m.PotiUpdate):
|
||||
"""Silent unless refused; a refusal arrives as a RESULT that request() ignores."""
|
||||
self.send(m.Opcode.POTI_UPDATE, update.encode())
|
||||
|
||||
def btp_data_request(self, request: m.BtpDataRequest) -> m.Result:
|
||||
return self.request(m.Opcode.BTP_DATA_REQUEST, request.encode())
|
||||
|
||||
def provision(self, bundle: bytes) -> Tuple[int, int, int]:
|
||||
result = None
|
||||
for segment in m.provision_segments(bundle):
|
||||
result = self.request(m.Opcode.CREDENTIALS_PROVISION, segment.encode(), timeout=20)
|
||||
assert result is not None and len(result.detail) == 3, 'apply report expected'
|
||||
return result.detail[0], result.detail[1], result.detail[2]
|
||||
|
||||
def erase_credentials(self):
|
||||
self.request(m.Opcode.CREDENTIALS_ERASE, timeout=10)
|
||||
|
||||
def subscribe(self, subscriber_data: bytes = b'') -> int:
|
||||
result = self.request(m.Opcode.SF_IDCHANGE_SUBSCRIBE, bytes([len(subscriber_data)]) + subscriber_data)
|
||||
return int.from_bytes(result.detail, 'little')
|
||||
|
||||
def unsubscribe(self, subscription: int):
|
||||
self.request(m.Opcode.SF_IDCHANGE_UNSUBSCRIBE, subscription.to_bytes(8, 'little'))
|
||||
|
||||
def event_response(self, event: m.IdChangeEvent, event_sequence: int, return_code: bool):
|
||||
self.send(m.Opcode.SF_IDCHANGE_EVENT_RESPONSE, m.IdChangeEventResponse(event.subscription, return_code).encode(),
|
||||
sequence=event_sequence)
|
||||
|
||||
def trigger(self):
|
||||
self.request(m.Opcode.SF_IDCHANGE_TRIGGER)
|
||||
|
||||
def lock(self, seconds: int) -> int:
|
||||
return int.from_bytes(self.request(m.Opcode.SF_ID_LOCK, bytes([seconds])).detail, 'little')
|
||||
|
||||
def unlock(self, handle: int):
|
||||
self.request(m.Opcode.SF_ID_UNLOCK, handle.to_bytes(8, 'little'))
|
||||
|
||||
def status(self, timeout: float = 5.0) -> m.Status:
|
||||
with self.lock:
|
||||
while not self.replies.empty():
|
||||
self.replies.get_nowait()
|
||||
self.sequence = (self.sequence + 1) & 0xFFFF
|
||||
self._awaited_status = self.sequence
|
||||
try:
|
||||
self.transport.write(FrameType.LINK, m.encode_message(m.Message(m.Header(m.Opcode.STATUS_REQUEST, 0, self.sequence))))
|
||||
deadline = time.monotonic() + timeout
|
||||
while True:
|
||||
remaining = deadline - time.monotonic()
|
||||
if remaining <= 0:
|
||||
raise TimeoutError('no STATUS')
|
||||
reply = self.replies.get(timeout=remaining)
|
||||
if reply.header.opcode == m.Opcode.STATUS and reply.header.sequence == self.sequence:
|
||||
self.last_status = m.Status.decode(reply.body)
|
||||
return self.last_status
|
||||
finally:
|
||||
self._awaited_status = -1
|
||||
|
||||
|
||||
class TestChannel:
|
||||
"""USB test channel (frame type 0x11): software lower tester and campaign hooks."""
|
||||
|
||||
MIRROR, INJECT, GN_REQUEST, DRAIN, RESET = 0x01, 0x02, 0x03, 0x04, 0x05
|
||||
|
||||
def __init__(self, client: LinkClient, timeout: float = 3.0):
|
||||
self.client = client
|
||||
self.timeout = timeout
|
||||
|
||||
def execute(self, request: bytes) -> Tuple[int, List[Tuple[int, bytes]]]:
|
||||
with self.client.lock:
|
||||
while not self.client.test_replies.empty():
|
||||
self.client.test_replies.get_nowait()
|
||||
self.client.transport.write(FrameType.TEST, request)
|
||||
reply = self.client.test_replies.get(timeout=self.timeout)
|
||||
result, count = reply[0], reply[1]
|
||||
records, at = [], 2
|
||||
for _ in range(count):
|
||||
kind = reply[at]
|
||||
length = int.from_bytes(reply[at + 1:at + 3], 'little')
|
||||
records.append((kind, reply[at + 3:at + 3 + length]))
|
||||
at += 3 + length
|
||||
if at != len(reply):
|
||||
raise ValueError('trailing test channel octets')
|
||||
return result, records
|
||||
|
||||
def mirror(self, mode: int):
|
||||
result, _ = self.execute(bytes([self.MIRROR, mode]))
|
||||
if result != 0:
|
||||
raise RuntimeError('mirror mode refused: %d' % result)
|
||||
|
||||
def inject(self, source: bytes, destination: bytes, gnpdu: bytes) -> int:
|
||||
result, _ = self.execute(bytes([self.INJECT]) + source + destination + gnpdu)
|
||||
return result
|
||||
|
||||
def gn_request(self, traffic_class: int, payload: bytes) -> int:
|
||||
result, _ = self.execute(bytes([self.GN_REQUEST, traffic_class]) + payload)
|
||||
return result
|
||||
|
||||
def drain(self) -> Tuple[bool, List[Tuple[int, bytes]]]:
|
||||
"""All queued records (several frames when needed); returns (overflow, records)."""
|
||||
records: List[Tuple[int, bytes]] = []
|
||||
overflow = False
|
||||
while True:
|
||||
result, batch = self.execute(bytes([self.DRAIN]))
|
||||
overflow = overflow or result == m.Code.resource_limit
|
||||
records.extend(batch)
|
||||
if not batch:
|
||||
return overflow, records
|
||||
|
||||
def reset(self):
|
||||
self.execute(bytes([self.RESET]))
|
||||
@@ -0,0 +1,466 @@
|
||||
"""Message layer version 1 of the micrOBU station-internal link.
|
||||
|
||||
Mirror of the firmware's ``link_protocol.hpp``; the two are checked against each other by
|
||||
``tests/test_station_link.py``. All integers little-endian; ETSI payloads opaque.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import enum
|
||||
import struct
|
||||
from dataclasses import dataclass, field
|
||||
from typing import List, Optional
|
||||
|
||||
MAXIMUM_MESSAGE = 512
|
||||
HEADER_SIZE = 4
|
||||
|
||||
|
||||
class Opcode(enum.IntEnum):
|
||||
STATION_CONFIGURE = 0x01
|
||||
POTI_UPDATE = 0x02
|
||||
BTP_DATA_REQUEST = 0x03
|
||||
CREDENTIALS_PROVISION = 0x04
|
||||
CREDENTIALS_ERASE = 0x05
|
||||
SF_IDCHANGE_SUBSCRIBE = 0x06
|
||||
SF_IDCHANGE_UNSUBSCRIBE = 0x07
|
||||
SF_IDCHANGE_EVENT_RESPONSE = 0x08
|
||||
SF_IDCHANGE_TRIGGER = 0x09
|
||||
SF_ID_LOCK = 0x0A
|
||||
SF_ID_UNLOCK = 0x0B
|
||||
STATUS_REQUEST = 0x0C
|
||||
RESULT = 0x80
|
||||
BTP_DATA_INDICATION = 0x81
|
||||
SF_IDCHANGE_EVENT = 0x82
|
||||
MF_SET_REQUEST = 0x83
|
||||
STATUS = 0x84
|
||||
|
||||
|
||||
class Code(enum.IntEnum):
|
||||
"""RESULT codes: vanetza_idf::Result first, then link codes."""
|
||||
accepted = 0
|
||||
invalid_argument = 1
|
||||
unsupported = 2
|
||||
wrong_entry_point = 3
|
||||
security_unavailable = 4
|
||||
resource_limit = 5
|
||||
rejected = 6
|
||||
time_regression = 7
|
||||
identity_change_pending = 8
|
||||
unknown_opcode = 0x10
|
||||
malformed = 0x11
|
||||
not_configured = 0x12
|
||||
busy = 0x13
|
||||
no_credentials = 0x14
|
||||
|
||||
|
||||
FLAG_FRAGMENT_FIRST = 0x01
|
||||
FLAG_FRAGMENT_MORE = 0x02
|
||||
|
||||
|
||||
class IdChangeCommand(enum.IntEnum):
|
||||
PREPARE = 0
|
||||
COMMIT = 1
|
||||
ABORT = 2
|
||||
DEREG = 3
|
||||
|
||||
|
||||
class TransportType(enum.IntEnum):
|
||||
SHB = 1
|
||||
GBC = 2
|
||||
TSB = 3
|
||||
GAC = 4
|
||||
GUC = 5
|
||||
|
||||
|
||||
VERIFICATION_REPORTS = ['unsecured', 'Success', 'False_Signature', 'Invalid_Certificate', 'Revoked_Certificate',
|
||||
'Inconsistent_Chain', 'Invalid_Timestamp', 'Duplicate_Message', 'Invalid_Mobility_Data',
|
||||
'Unsigned_Message', 'Signer_Certificate_Not_Found', 'Unsupported_Signer_Identifier_Type',
|
||||
'Incompatible_Protocol', 'Configuration_Problem']
|
||||
|
||||
|
||||
class DecodeError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
class _Reader:
|
||||
def __init__(self, data: bytes, at: int = 0):
|
||||
self.data = data
|
||||
self.at = at
|
||||
|
||||
def _take(self, n: int) -> bytes:
|
||||
if self.at + n > len(self.data):
|
||||
raise DecodeError('truncated body')
|
||||
chunk = self.data[self.at:self.at + n]
|
||||
self.at += n
|
||||
return chunk
|
||||
|
||||
def u8(self) -> int: return self._take(1)[0]
|
||||
def u16(self) -> int: return struct.unpack('<H', self._take(2))[0]
|
||||
def u32(self) -> int: return struct.unpack('<I', self._take(4))[0]
|
||||
def u64(self) -> int: return struct.unpack('<Q', self._take(8))[0]
|
||||
def i32(self) -> int: return struct.unpack('<i', self._take(4))[0]
|
||||
def bytes(self, n: int) -> bytes: return self._take(n)
|
||||
def rest(self) -> bytes:
|
||||
chunk = self.data[self.at:]
|
||||
self.at = len(self.data)
|
||||
return chunk
|
||||
|
||||
def done(self):
|
||||
if self.at != len(self.data):
|
||||
raise DecodeError('trailing octets')
|
||||
|
||||
|
||||
@dataclass
|
||||
class Header:
|
||||
opcode: int
|
||||
flags: int = 0
|
||||
sequence: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class Message:
|
||||
header: Header
|
||||
body: bytes = b''
|
||||
|
||||
|
||||
def encode_message(message: Message) -> bytes:
|
||||
if HEADER_SIZE + len(message.body) > MAXIMUM_MESSAGE:
|
||||
raise ValueError('message exceeds %d octets' % MAXIMUM_MESSAGE)
|
||||
return struct.pack('<BBH', int(message.header.opcode), message.header.flags, message.header.sequence) + bytes(message.body)
|
||||
|
||||
|
||||
def decode_message(octets: bytes) -> Message:
|
||||
if len(octets) < HEADER_SIZE or len(octets) > MAXIMUM_MESSAGE:
|
||||
raise DecodeError('bad message length %d' % len(octets))
|
||||
opcode, flags, sequence = struct.unpack('<BBH', octets[:HEADER_SIZE])
|
||||
return Message(Header(opcode, flags, sequence), bytes(octets[HEADER_SIZE:]))
|
||||
|
||||
|
||||
# ---- bodies ----------------------------------------------------------------------------
|
||||
|
||||
@dataclass
|
||||
class StationConfigure:
|
||||
station_type: int = 2
|
||||
security: int = 1
|
||||
address_configuration: int = 1
|
||||
mid: bytes = b'\x02\x00\x00\x00\x00\x01'
|
||||
beaconing: int = 1
|
||||
channel_number: int = 180
|
||||
transmit_power_dbm: int = 10
|
||||
radio: int = 0
|
||||
default_traffic_class: int = 2
|
||||
default_lifetime: int = 0x05
|
||||
|
||||
def encode(self) -> bytes:
|
||||
assert len(self.mid) == 6
|
||||
return struct.pack('<BBB6sBHBBBB', self.station_type, self.security, self.address_configuration, self.mid,
|
||||
self.beaconing, self.channel_number, self.transmit_power_dbm, self.radio,
|
||||
self.default_traffic_class, self.default_lifetime)
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'StationConfigure':
|
||||
if len(body) != 16:
|
||||
raise DecodeError('StationConfigure length')
|
||||
f = struct.unpack('<BBB6sBHBBBB', body)
|
||||
return cls(*f)
|
||||
|
||||
|
||||
@dataclass
|
||||
class StationInfo:
|
||||
"""RESULT detail of STATION_CONFIGURE."""
|
||||
gn_address: bytes
|
||||
identifier: bytes
|
||||
credentials_loaded: bool
|
||||
tickets: int
|
||||
|
||||
@classmethod
|
||||
def decode(cls, detail: bytes) -> 'StationInfo':
|
||||
if len(detail) != 18:
|
||||
raise DecodeError('StationInfo length')
|
||||
return cls(detail[:8], detail[8:16], detail[16] != 0, detail[17])
|
||||
|
||||
|
||||
@dataclass
|
||||
class PotiUpdate:
|
||||
timestamp_ms: int
|
||||
latitude: int
|
||||
longitude: int
|
||||
semi_major_cm: int = 0
|
||||
semi_minor_cm: int = 0
|
||||
orientation_deci_degree: int = 0
|
||||
altitude_cm: Optional[int] = None
|
||||
speed_cm_s: Optional[int] = None
|
||||
heading_deci_degree: Optional[int] = None
|
||||
pai: bool = False
|
||||
|
||||
def encode(self) -> bytes:
|
||||
flags = ((1 if self.altitude_cm is not None else 0) | (2 if self.speed_cm_s is not None else 0) |
|
||||
(4 if self.heading_deci_degree is not None else 0) | (8 if self.pai else 0))
|
||||
return struct.pack('<QiiHHHBiHH', self.timestamp_ms, self.latitude, self.longitude, self.semi_major_cm,
|
||||
self.semi_minor_cm, self.orientation_deci_degree, flags, self.altitude_cm or 0,
|
||||
self.speed_cm_s or 0, self.heading_deci_degree or 0)
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'PotiUpdate':
|
||||
if len(body) != 31:
|
||||
raise DecodeError('PotiUpdate length')
|
||||
t, lat, lon, a, b, o, flags, alt, spd, hdg = struct.unpack('<QiiHHHBiHH', body)
|
||||
return cls(t, lat, lon, a, b, o, alt if flags & 1 else None, spd if flags & 2 else None,
|
||||
hdg if flags & 4 else None, bool(flags & 8))
|
||||
|
||||
|
||||
@dataclass
|
||||
class DestinationArea:
|
||||
shape: int = 0
|
||||
latitude: int = 0
|
||||
longitude: int = 0
|
||||
distance_a: int = 0
|
||||
distance_b: int = 0
|
||||
angle: int = 0
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return struct.pack('<BiiHHH', self.shape, self.latitude, self.longitude, self.distance_a, self.distance_b, self.angle)
|
||||
|
||||
@classmethod
|
||||
def read(cls, r: _Reader) -> 'DestinationArea':
|
||||
return cls(r.u8(), r.i32(), r.i32(), r.u16(), r.u16(), r.u16())
|
||||
|
||||
|
||||
@dataclass
|
||||
class BtpDataRequest:
|
||||
fl_sdu: bytes
|
||||
btp_type: int = 1
|
||||
destination_port: int = 2018
|
||||
destination_port_info: int = 0
|
||||
gn_packet_transport_type: int = TransportType.SHB
|
||||
gn_communication_profile: int = 1
|
||||
gn_security_profile: int = 0
|
||||
gn_traffic_class: int = 0xFF
|
||||
gn_maximum_packet_lifetime: int = 0xFF
|
||||
gn_maximum_hop_limit: int = 0
|
||||
gn_repetition_interval_ms: int = 0
|
||||
gn_repetition_maximum_ms: int = 0
|
||||
its_aid: int = 638
|
||||
permissions: bytes = b''
|
||||
context: bytes = b''
|
||||
area: Optional[DestinationArea] = None
|
||||
|
||||
def encode(self) -> bytes:
|
||||
if len(self.permissions) > 31:
|
||||
raise ValueError('SSP longer than 31 octets')
|
||||
out = struct.pack('<BHHBBBBBBHHI', self.btp_type, self.destination_port, self.destination_port_info,
|
||||
self.gn_packet_transport_type, self.gn_communication_profile, self.gn_security_profile,
|
||||
self.gn_traffic_class, self.gn_maximum_packet_lifetime, self.gn_maximum_hop_limit,
|
||||
self.gn_repetition_interval_ms, self.gn_repetition_maximum_ms, self.its_aid)
|
||||
out += bytes([len(self.permissions)]) + self.permissions + bytes([len(self.context)]) + self.context
|
||||
if self.gn_packet_transport_type == TransportType.GBC:
|
||||
out += (self.area or DestinationArea()).encode()
|
||||
out += struct.pack('<H', len(self.fl_sdu)) + self.fl_sdu
|
||||
return out
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'BtpDataRequest':
|
||||
r = _Reader(body)
|
||||
f = struct.unpack('<BHHBBBBBBHHI', r.bytes(19))
|
||||
permissions = r.bytes(r.u8())
|
||||
context = r.bytes(r.u8())
|
||||
area = DestinationArea.read(r) if f[3] == TransportType.GBC else None
|
||||
sdu = r.bytes(r.u16())
|
||||
r.done()
|
||||
return cls(sdu, *f, permissions=permissions, context=context, area=area)
|
||||
|
||||
|
||||
@dataclass
|
||||
class BtpDataIndication:
|
||||
received_fl_sdu: bytes
|
||||
btp_type: int = 1
|
||||
destination_port: int = 0
|
||||
destination_port_info: int = 0
|
||||
gn_packet_transport_type: int = 1
|
||||
gn_traffic_class: int = 0
|
||||
gn_remaining_packet_lifetime: int = 0xFF
|
||||
gn_remaining_hop_limit: int = 0xFF
|
||||
source_gn_address: bytes = b'\0' * 8
|
||||
source_timestamp: int = 0
|
||||
source_latitude: int = 0
|
||||
source_longitude: int = 0
|
||||
security_report: int = 0
|
||||
its_aid: int = 0
|
||||
permissions: bytes = b''
|
||||
certificate_id: Optional[bytes] = None
|
||||
area: Optional[DestinationArea] = None
|
||||
|
||||
@property
|
||||
def report_name(self) -> str:
|
||||
return VERIFICATION_REPORTS[self.security_report] if self.security_report < len(VERIFICATION_REPORTS) else str(self.security_report)
|
||||
|
||||
def encode(self) -> bytes:
|
||||
out = struct.pack('<BHHBBBB8sIiiBI', self.btp_type, self.destination_port, self.destination_port_info,
|
||||
self.gn_packet_transport_type, self.gn_traffic_class, self.gn_remaining_packet_lifetime,
|
||||
self.gn_remaining_hop_limit, self.source_gn_address, self.source_timestamp,
|
||||
self.source_latitude, self.source_longitude, self.security_report, self.its_aid)
|
||||
out += bytes([len(self.permissions)]) + self.permissions
|
||||
out += bytes([1 if self.certificate_id else 0]) + (self.certificate_id or b'\0' * 8)
|
||||
out += bytes([1 if self.area else 0]) + (self.area.encode() if self.area else b'')
|
||||
out += struct.pack('<H', len(self.received_fl_sdu)) + self.received_fl_sdu
|
||||
return out
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'BtpDataIndication':
|
||||
r = _Reader(body)
|
||||
f = struct.unpack('<BHHBBBB8sIiiBI', r.bytes(34))
|
||||
permissions = r.bytes(r.u8())
|
||||
present = r.u8()
|
||||
certificate = r.bytes(8)
|
||||
area = DestinationArea.read(r) if r.u8() else None
|
||||
sdu = r.bytes(r.u16())
|
||||
r.done()
|
||||
return cls(sdu, *f, permissions=permissions, certificate_id=certificate if present else None, area=area)
|
||||
|
||||
|
||||
@dataclass
|
||||
class CredentialsProvision:
|
||||
total_length: int
|
||||
offset: int
|
||||
segment: bytes
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return struct.pack('<HHB', self.total_length, self.offset, len(self.segment)) + self.segment
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'CredentialsProvision':
|
||||
r = _Reader(body)
|
||||
total, offset = r.u16(), r.u16()
|
||||
segment = r.bytes(r.u8())
|
||||
r.done()
|
||||
return cls(total, offset, segment)
|
||||
|
||||
|
||||
def provision_segments(bundle: bytes, segment_size: int = 200) -> List[CredentialsProvision]:
|
||||
"""Split a VCR1 bundle into CREDENTIALS_PROVISION messages."""
|
||||
return [CredentialsProvision(len(bundle), at, bundle[at:at + segment_size]) for at in range(0, len(bundle), segment_size)]
|
||||
|
||||
|
||||
@dataclass
|
||||
class IdChangeEvent:
|
||||
subscription: int
|
||||
command: int
|
||||
id: bytes
|
||||
subscriber_data: bytes = b''
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return struct.pack('<QB8sB', self.subscription, self.command, self.id, len(self.subscriber_data)) + self.subscriber_data
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'IdChangeEvent':
|
||||
r = _Reader(body)
|
||||
s, c, i = struct.unpack('<QB8s', r.bytes(17))
|
||||
data = r.bytes(r.u8())
|
||||
r.done()
|
||||
return cls(s, c, i, data)
|
||||
|
||||
|
||||
@dataclass
|
||||
class IdChangeEventResponse:
|
||||
subscription: int
|
||||
return_code: bool
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return struct.pack('<QB', self.subscription, 1 if self.return_code else 0)
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'IdChangeEventResponse':
|
||||
if len(body) != 9:
|
||||
raise DecodeError('IdChangeEventResponse length')
|
||||
s, c = struct.unpack('<QB', body)
|
||||
return cls(s, c != 0)
|
||||
|
||||
|
||||
@dataclass
|
||||
class MfSetRequest:
|
||||
fac_id: int = 0
|
||||
command_ref: int = 0
|
||||
params: List[tuple] = field(default_factory=list) # (f_param_no, value)
|
||||
|
||||
def encode(self) -> bytes:
|
||||
out = struct.pack('<IBB', self.fac_id, self.command_ref, len(self.params))
|
||||
for no, value in self.params:
|
||||
out += struct.pack('<BI', no, value)
|
||||
return out
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'MfSetRequest':
|
||||
r = _Reader(body)
|
||||
fac, ref, count = struct.unpack('<IBB', r.bytes(6))
|
||||
params = [struct.unpack('<BI', r.bytes(5)) for _ in range(count)]
|
||||
r.done()
|
||||
return cls(fac, ref, params)
|
||||
|
||||
|
||||
STATUS_FORMAT = '<IB8s8sBB' + 'I' * 7 + 'I' * 3 + 'I' * 4 + 'I' * 4 + 'Q'
|
||||
STATUS_FIELDS = ['uptime_ms', 'configured', 'gn_address', 'identifier', 'change_pending', 'tickets',
|
||||
'signed_messages', 'refused_no_ticket', 'refused_change_pending', 'refused_permission',
|
||||
'sign_failed', 'verified', 'rejected', 'requests_accepted', 'requests_refused', 'indications',
|
||||
'radio_submitted', 'radio_failed', 'radio_received', 'radio_dropped', 'link_rx_frames',
|
||||
'link_crc_errors', 'link_malformed', 'poti_updates', 'its_time_ms']
|
||||
|
||||
|
||||
@dataclass
|
||||
class Status:
|
||||
uptime_ms: int = 0
|
||||
configured: int = 0
|
||||
gn_address: bytes = b'\0' * 8
|
||||
identifier: bytes = b'\0' * 8
|
||||
change_pending: int = 0
|
||||
tickets: int = 0
|
||||
signed_messages: int = 0
|
||||
refused_no_ticket: int = 0
|
||||
refused_change_pending: int = 0
|
||||
refused_permission: int = 0
|
||||
sign_failed: int = 0
|
||||
verified: int = 0
|
||||
rejected: int = 0
|
||||
requests_accepted: int = 0
|
||||
requests_refused: int = 0
|
||||
indications: int = 0
|
||||
radio_submitted: int = 0
|
||||
radio_failed: int = 0
|
||||
radio_received: int = 0
|
||||
radio_dropped: int = 0
|
||||
link_rx_frames: int = 0
|
||||
link_crc_errors: int = 0
|
||||
link_malformed: int = 0
|
||||
poti_updates: int = 0
|
||||
its_time_ms: int = 0
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return struct.pack(STATUS_FORMAT, *[getattr(self, name) for name in STATUS_FIELDS])
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'Status':
|
||||
if len(body) != struct.calcsize(STATUS_FORMAT):
|
||||
raise DecodeError('Status length %d' % len(body))
|
||||
return cls(**dict(zip(STATUS_FIELDS, struct.unpack(STATUS_FORMAT, body))))
|
||||
|
||||
|
||||
@dataclass
|
||||
class Result:
|
||||
code: int
|
||||
detail: bytes = b''
|
||||
|
||||
def encode(self) -> bytes:
|
||||
return bytes([int(self.code), len(self.detail)]) + self.detail
|
||||
|
||||
@classmethod
|
||||
def decode(cls, body: bytes) -> 'Result':
|
||||
r = _Reader(body)
|
||||
code = r.u8()
|
||||
detail = r.bytes(r.u8())
|
||||
r.done()
|
||||
return cls(code, detail)
|
||||
|
||||
@property
|
||||
def name(self) -> str:
|
||||
try:
|
||||
return Code(self.code).name
|
||||
except ValueError:
|
||||
return 'code_0x%02x' % self.code
|
||||
@@ -0,0 +1,136 @@
|
||||
"""Serial transport of the station-internal link (the app's Phase 03 framing).
|
||||
|
||||
``[AA][55][type][length LE][payload][crc16 LE]``, CRC-16/CCITT-FALSE over type+length+payload.
|
||||
Frame types: 0x10 link message, 0x11 test channel, 0x7F log line. Requires pyserial.
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import enum
|
||||
import struct
|
||||
import threading
|
||||
from typing import Callable, List, Tuple
|
||||
|
||||
MAXIMUM_PAYLOAD = 1536
|
||||
|
||||
|
||||
class FrameType(enum.IntEnum):
|
||||
LINK = 0x10
|
||||
TEST = 0x11
|
||||
LOG = 0x7F
|
||||
|
||||
|
||||
def crc16_ccitt_false(data: bytes) -> int:
|
||||
crc = 0xFFFF
|
||||
for octet in data:
|
||||
crc ^= octet << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def encode_frame(frame_type: int, payload: bytes) -> bytes:
|
||||
if len(payload) > MAXIMUM_PAYLOAD:
|
||||
raise ValueError('frame payload exceeds %d octets' % MAXIMUM_PAYLOAD)
|
||||
head = struct.pack('<BH', int(frame_type), len(payload)) + payload
|
||||
return b'\xaa\x55' + head + struct.pack('<H', crc16_ccitt_false(head))
|
||||
|
||||
|
||||
class FrameDecoder:
|
||||
"""Byte-at-a-time state machine, the app's SerialFrameDecoder with the larger payload limit."""
|
||||
|
||||
def __init__(self):
|
||||
self.state = 'SYNC0'
|
||||
self.type = 0
|
||||
self.length = 0
|
||||
self.payload = bytearray()
|
||||
self.crc = 0
|
||||
self.crc_errors = 0
|
||||
self.frames = 0
|
||||
|
||||
def feed(self, data: bytes) -> List[Tuple[int, bytes]]:
|
||||
out = []
|
||||
for b in data:
|
||||
s = self.state
|
||||
if s == 'SYNC0':
|
||||
self.state = 'SYNC1' if b == 0xAA else 'SYNC0'
|
||||
elif s == 'SYNC1':
|
||||
self.state = 'TYPE' if b == 0x55 else ('SYNC1' if b == 0xAA else 'SYNC0')
|
||||
elif s == 'TYPE':
|
||||
self.type = b
|
||||
self.state = 'LEN_LO'
|
||||
elif s == 'LEN_LO':
|
||||
self.length = b
|
||||
self.state = 'LEN_HI'
|
||||
elif s == 'LEN_HI':
|
||||
self.length |= b << 8
|
||||
self.payload = bytearray()
|
||||
if self.length > MAXIMUM_PAYLOAD:
|
||||
self.state = 'SYNC0'
|
||||
else:
|
||||
self.state = 'CRC_LO' if self.length == 0 else 'PAYLOAD'
|
||||
elif s == 'PAYLOAD':
|
||||
self.payload.append(b)
|
||||
if len(self.payload) >= self.length:
|
||||
self.state = 'CRC_LO'
|
||||
elif s == 'CRC_LO':
|
||||
self.crc = b
|
||||
self.state = 'CRC_HI'
|
||||
elif s == 'CRC_HI':
|
||||
self.crc |= b << 8
|
||||
head = struct.pack('<BH', self.type, self.length) + bytes(self.payload)
|
||||
if crc16_ccitt_false(head) == self.crc:
|
||||
self.frames += 1
|
||||
out.append((self.type, bytes(self.payload)))
|
||||
else:
|
||||
self.crc_errors += 1
|
||||
self.state = 'SYNC0'
|
||||
return out
|
||||
|
||||
|
||||
class SerialTransport:
|
||||
"""Reader thread plus a locked writer over one pyserial port."""
|
||||
|
||||
def __init__(self, port: str, on_frame: Callable[[int, bytes], None], baudrate: int = 115200):
|
||||
import serial
|
||||
self.port = serial.Serial(port=None, baudrate=baudrate, timeout=0.05, write_timeout=2)
|
||||
# do not toggle DTR/RTS: the USB Serial/JTAG controller would reset the board
|
||||
self.port.dtr = False
|
||||
self.port.rts = False
|
||||
self.port.port = port
|
||||
self.port.open()
|
||||
self.on_frame = on_frame
|
||||
self.decoder = FrameDecoder()
|
||||
self.write_lock = threading.Lock()
|
||||
self.running = True
|
||||
self.thread = threading.Thread(target=self._reader, name='microbu-link-rx', daemon=True)
|
||||
self.thread.start()
|
||||
|
||||
def _reader(self):
|
||||
while self.running:
|
||||
try:
|
||||
data = self.port.read(4096)
|
||||
except Exception:
|
||||
if self.running:
|
||||
continue
|
||||
return
|
||||
if data:
|
||||
for frame_type, payload in self.decoder.feed(data):
|
||||
try:
|
||||
self.on_frame(frame_type, payload)
|
||||
except Exception as error: # a handler failure must not stop the reader
|
||||
print('frame handler error:', error)
|
||||
|
||||
def write(self, frame_type: int, payload: bytes):
|
||||
frame = encode_frame(frame_type, payload)
|
||||
with self.write_lock:
|
||||
if self.port.write(frame) != len(frame):
|
||||
raise IOError('incomplete serial write')
|
||||
|
||||
def close(self):
|
||||
self.running = False
|
||||
try:
|
||||
self.port.cancel_read()
|
||||
except Exception:
|
||||
pass
|
||||
self.thread.join(timeout=1)
|
||||
self.port.close()
|
||||
@@ -0,0 +1,12 @@
|
||||
"""Common adapter boundary for station-link transports."""
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Protocol
|
||||
|
||||
|
||||
class TransportAdapter(Protocol):
|
||||
"""The only transport surface used by LinkClient."""
|
||||
|
||||
def write(self, frame_type: int, payload: bytes) -> None: ...
|
||||
|
||||
def close(self) -> None: ...
|
||||
@@ -0,0 +1,239 @@
|
||||
"""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
|
||||
Reference in New Issue
Block a user