63 lines
1.8 KiB
Python
63 lines
1.8 KiB
Python
import asyncio
|
|
import aioserial
|
|
import aiomqtt
|
|
import pyubx2
|
|
|
|
from command import command
|
|
from handler import MQTTHandler, task
|
|
|
|
|
|
class UBXHandler(MQTTHandler):
|
|
def __init__(
|
|
self,
|
|
mqtt_client: aiomqtt.Client,
|
|
handler_id: str,
|
|
serial_port: aioserial.AioSerial
|
|
):
|
|
super().__init__(mqtt_client, handler_id)
|
|
|
|
self.serial_port = serial_port
|
|
|
|
@command({"type": "number"}, description="An example command")
|
|
async def example_cmd(args):
|
|
print(f"Executing command with args {args}")
|
|
|
|
async def parse_serial(self):
|
|
buffer = bytearray()
|
|
|
|
class StreamWrapper:
|
|
def read(inner_self, n=1):
|
|
if not buffer:
|
|
raise BlockingIOError
|
|
out = buffer[:n]
|
|
del buffer[:n]
|
|
return bytes(out)
|
|
|
|
ubr = pyubx2.UBXReader(StreamWrapper(), parsing=True)
|
|
|
|
while True:
|
|
chunk = await self.serial_port.read_async(200)
|
|
if chunk:
|
|
buffer.extend(chunk)
|
|
|
|
try:
|
|
while True:
|
|
raw, parsed = ubr.read()
|
|
if raw is None:
|
|
break
|
|
yield raw, parsed
|
|
except (pyubx2.UBXStreamError, BlockingIOError, Exception):
|
|
pass
|
|
else:
|
|
await asyncio.sleep(0)
|
|
|
|
@task
|
|
async def serial_reader_task(self):
|
|
async for raw, parsed in self.parse_serial():
|
|
if isinstance(parsed, pyubx2.UBXMessage):
|
|
for name, value in vars(parsed).items():
|
|
if name.startswith("_"):
|
|
continue
|
|
topic = f"{self.topic_base}/{parsed.identity}/{name}"
|
|
await self.mqtt_client.publish(topic, value, qos=1, retain=True)
|