Dynalab implementation
JSON protocol implemented in this version to send the data to the server. Correction of the rpmhz value due to an offset angle of the hall effect captors.
This commit is contained in:
Martin Girard committed
1 parent
d2318b6903
commit
fdd4597233
4 files changed
+391
-20
No files matched your search
@@ -3,20 +3,211 @@ from serial import Serial
|
||||
from dataclasses import dataclass
|
||||
from serial_sync import SerialSync
|
||||
import struct
|
||||
from dynalab_core.protocols.json.wire import read_message, write_message
|
||||
from dynalab_core.protocols.packets import JsonConnectorHello, JsonDynaLabHello
|
||||
from dynalab_core.protocols.packets.handshake import JsonSignalDescriptor
|
||||
from dynalab_core.protocols.packets.heartbeat import JsonHeartbeat
|
||||
from dynalab_core.protocols.packets.data import JsonValueDescriptor
|
||||
from dynalab_core.protocols.constants import PROTOCOL_VERSION
|
||||
from time import monotonic
|
||||
import threading
|
||||
import asyncio
|
||||
import logging
|
||||
from uuid import uuid4
|
||||
from queue import Queue
|
||||
|
||||
|
||||
async def perform_handshake(
|
||||
reader: asyncio.StreamReader,
|
||||
writer: asyncio.StreamWriter,
|
||||
write_lock: asyncio.Lock,
|
||||
) -> None:
|
||||
try:
|
||||
message = await asyncio.wait_for(
|
||||
read_message(reader),
|
||||
timeout=30.0,
|
||||
)
|
||||
except TimeoutError as exc:
|
||||
raise TimeoutError("Timed out waiting for server hello") from exc
|
||||
|
||||
if not isinstance(message, JsonDynaLabHello):
|
||||
raise RuntimeError(f"Expected DynaLabHello, received {type(message).__name__}")
|
||||
|
||||
connector_hello = JsonConnectorHello(
|
||||
connector_uuid=uuid4(),
|
||||
protocol_version=PROTOCOL_VERSION,
|
||||
connector_name="Test connector",
|
||||
connector_version="V1",
|
||||
signals=[signal_1, signal_2],
|
||||
)
|
||||
|
||||
try:
|
||||
await asyncio.wait_for(
|
||||
send_message(
|
||||
writer,
|
||||
write_lock,
|
||||
connector_hello,
|
||||
),
|
||||
timeout=5.0,
|
||||
)
|
||||
except TimeoutError as exc:
|
||||
raise TimeoutError("Timed out sending connector hello") from exc
|
||||
|
||||
|
||||
def value_sender_thread(
|
||||
loop: asyncio.AbstractEventLoop,
|
||||
writer: asyncio.StreamWriter,
|
||||
write_lock: asyncio.Lock,
|
||||
stop_event: threading.Event,
|
||||
) -> None:
|
||||
"""
|
||||
Run the high-frequency timing loop in a dedicated OS thread.
|
||||
|
||||
The asyncio StreamWriter is not thread-safe, so each write is submitted
|
||||
back to the asyncio event loop with run_coroutine_threadsafe().
|
||||
"""
|
||||
while not stop_event.is_set():
|
||||
message = data_queue.get()
|
||||
future = asyncio.run_coroutine_threadsafe(
|
||||
send_message(
|
||||
writer,
|
||||
write_lock,
|
||||
message,
|
||||
),
|
||||
loop,
|
||||
)
|
||||
|
||||
try:
|
||||
future.result(timeout=2.0)
|
||||
|
||||
except Exception:
|
||||
return
|
||||
|
||||
|
||||
async def jsonhandler() -> None:
|
||||
writer: asyncio.StreamWriter | None = None
|
||||
sender_thread: threading.Thread | None = None
|
||||
sender_stop_event = threading.Event()
|
||||
try:
|
||||
write_lock = asyncio.Lock()
|
||||
reader, writer = await asyncio.open_connection(
|
||||
host=HOST,
|
||||
port=PORT,
|
||||
)
|
||||
await perform_handshake(
|
||||
reader,
|
||||
writer,
|
||||
write_lock,
|
||||
)
|
||||
|
||||
loop = asyncio.get_running_loop()
|
||||
sender_thread = threading.Thread(
|
||||
target=value_sender_thread,
|
||||
name="value_sender",
|
||||
args=(
|
||||
loop,
|
||||
writer,
|
||||
write_lock,
|
||||
sender_stop_event,
|
||||
),
|
||||
daemon=True,
|
||||
)
|
||||
sender_thread.start()
|
||||
await receive_messages(reader, writer, write_lock)
|
||||
|
||||
except (TimeoutError, asyncio.IncompleteReadError, ConnectionError, RuntimeError):
|
||||
pass
|
||||
|
||||
finally:
|
||||
sender_stop_event.set()
|
||||
|
||||
if sender_thread is not None:
|
||||
# Do not call thread.join() directly because that would block the
|
||||
# asyncio event loop while the thread may be waiting on it.
|
||||
await asyncio.to_thread(
|
||||
sender_thread.join,
|
||||
3.0,
|
||||
)
|
||||
|
||||
if writer is not None:
|
||||
writer.close()
|
||||
|
||||
try:
|
||||
await writer.wait_closed()
|
||||
except ConnectionError:
|
||||
pass
|
||||
|
||||
|
||||
def monotonic_ms() -> int:
|
||||
return round(monotonic() * 1_000)
|
||||
|
||||
|
||||
async def send_message(
|
||||
writer: asyncio.StreamWriter,
|
||||
write_lock: asyncio.Lock,
|
||||
message: object,
|
||||
) -> None:
|
||||
"""
|
||||
Serialize access to the asyncio StreamWriter.
|
||||
|
||||
Both heartbeat replies and value messages use this function.
|
||||
"""
|
||||
async with write_lock:
|
||||
await write_message(writer, message)
|
||||
|
||||
|
||||
async def receive_messages(
|
||||
reader: asyncio.StreamReader,
|
||||
writer: asyncio.StreamWriter,
|
||||
write_lock: asyncio.Lock,
|
||||
) -> None:
|
||||
while True:
|
||||
message = await read_message(reader)
|
||||
|
||||
if isinstance(message, JsonHeartbeat):
|
||||
message.return_timestamp = monotonic_ms()
|
||||
|
||||
await send_message(
|
||||
writer,
|
||||
write_lock,
|
||||
message,
|
||||
)
|
||||
|
||||
|
||||
def threadmain() -> None:
|
||||
asyncio.run(jsonhandler())
|
||||
|
||||
|
||||
DATA_STRUCT = struct.Struct("<BIf")
|
||||
HOST = "127.0.0.1"
|
||||
PORT = 8765
|
||||
JSONthread = threading.Thread(target=threadmain, name="Jsonthread", daemon=True)
|
||||
|
||||
signal_1 = JsonSignalDescriptor(
|
||||
id=uuid4(), name="Rpmhz", type="number", timeout_ms=5000
|
||||
)
|
||||
signal_2 = JsonSignalDescriptor(
|
||||
id=uuid4(), name="lambda", type="number", timeout_ms=1000
|
||||
)
|
||||
|
||||
data_queue: Queue[JsonValueDescriptor] = Queue()
|
||||
|
||||
|
||||
@dataclass
|
||||
class SensorData:
|
||||
data_type:int
|
||||
timestamp:int
|
||||
value:float
|
||||
data_type: int
|
||||
timestamp: int
|
||||
value: float
|
||||
|
||||
@classmethod
|
||||
def from_bytes(cls, payload: bytes):
|
||||
data_type, timestamp, value = DATA_STRUCT.unpack(payload)
|
||||
return cls(data_type=data_type, timestamp=timestamp, value=value)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
|
||||
JSONthread.start()
|
||||
try:
|
||||
print("Hello from bench-data-connector!")
|
||||
choice = None
|
||||
@@ -29,32 +220,30 @@ def main() -> None:
|
||||
try:
|
||||
choice = int(input("Choose a port to connect: "))
|
||||
except ValueError:
|
||||
pass
|
||||
pass
|
||||
|
||||
while choice is None or choice >= len(ports) or choice < 0:
|
||||
|
||||
try:
|
||||
choice = int(input("Unknown port. Try again : "))
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
|
||||
selected_port = ports[choice].device
|
||||
print(f"Selected port : {selected_port}")
|
||||
except KeyboardInterrupt:
|
||||
print("\n")
|
||||
exit(0)
|
||||
|
||||
with Serial(selected_port,baudrate=115200,timeout=1) as ser:
|
||||
with Serial(selected_port, baudrate=115200, timeout=1) as ser:
|
||||
sync = SerialSync(ser)
|
||||
sync.calibrate_sync(30)
|
||||
print(f"sync ok\nOffset:{sync._offset}\nScale:{sync._scale}")
|
||||
while True:
|
||||
start_sync(ser)
|
||||
#print("paquet trouvé")
|
||||
#print(DATA_STRUCT.size)
|
||||
# print("paquet trouvé")
|
||||
# print(DATA_STRUCT.size)
|
||||
payload = read_exactly(ser, DATA_STRUCT.size)
|
||||
|
||||
|
||||
if not payload:
|
||||
continue
|
||||
crc = read_exactly(ser, 2)
|
||||
@@ -64,12 +253,18 @@ def main() -> None:
|
||||
calculated_crc = crc16_ccitt(payload)
|
||||
if calculated_crc != received_crc:
|
||||
continue
|
||||
#print("paquet sécurisé")
|
||||
print(SensorData.from_bytes(payload))
|
||||
|
||||
|
||||
# print("paquet sécurisé")
|
||||
packet = SensorData.from_bytes(payload)
|
||||
data_queue.put(
|
||||
JsonValueDescriptor(
|
||||
signal_id=(signal_1.id if packet.data_type == 0 else signal_2.id),
|
||||
value=packet.value,
|
||||
timestamp=sync.convert_timestamp(packet.timestamp * 1000),
|
||||
)
|
||||
)
|
||||
|
||||
def read_exactly(ser: Serial, size: int) ->bytes | None:
|
||||
|
||||
def read_exactly(ser: Serial, size: int) -> bytes | None:
|
||||
data = bytearray()
|
||||
while len(data) < size:
|
||||
packet_exact = ser.read(size - len(data))
|
||||
@@ -78,6 +273,7 @@ def read_exactly(ser: Serial, size: int) ->bytes | None:
|
||||
data.extend(packet_exact)
|
||||
return bytes(data)
|
||||
|
||||
|
||||
def crc16_ccitt(data: bytes) -> int:
|
||||
crc = 0xFFFF
|
||||
|
||||
@@ -92,16 +288,15 @@ def crc16_ccitt(data: bytes) -> int:
|
||||
|
||||
crc &= 0xFFFF
|
||||
|
||||
return crc
|
||||
return crc
|
||||
|
||||
|
||||
def start_sync(ser: Serial) -> None:
|
||||
previous = None
|
||||
while True:
|
||||
byte = ser.read(1)
|
||||
if not byte :
|
||||
if not byte:
|
||||
continue
|
||||
if previous == b"\xEF" and byte == b"\xBE" :
|
||||
if previous == b"\xef" and byte == b"\xbe":
|
||||
return
|
||||
previous = byte
|
||||
|
||||
Reference in new issue
Block a user