From daddd275589e7c3a1811904d21962e75dd67ec08 Mon Sep 17 00:00:00 2001 From: Said Alvarado Date: Mon, 3 Apr 2023 15:26:16 +0200 Subject: [PATCH 1/3] add support for the ekf_debug protocol --- dotbot/protocol.py | 39 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index 95c95d85..ecc5b780 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -28,7 +28,9 @@ class PayloadType(Enum): LH2_WAYPOINTS = 8 GPS_WAYPOINTS = 9 SAILBOT_DATA = 10 - INVALID_PAYLOAD = 11 # Increase each time a new payload type is added + EKF_DEBUG = 11 + + INVALID_PAYLOAD = 12 # Increase each time a new payload type is added class ApplicationType(IntEnum): @@ -358,6 +360,39 @@ def from_bytes(_) -> ProtocolData: return GPSWaypoints(threshold=0, waypoints=[]) +@dataclass +class EKFDebugData(ProtocolData): + """Dataclass that holds the internal state and control variables """ + + x: int = 0xFFFF + y: int = 0xFFFF + theta: int = 0xFFFF + V: int = 0xFFFF + w: int = 0xFFFF + angle_to_target: int = 0xFFFF + + @property + def fields(self) -> List[ProtocolField]: + return [ + ProtocolField(self.x, name="x", length=4, signed=True), + ProtocolField(self.y, name="y", length=4, signed=True), + ProtocolField(self.theta, name="theta", length=4, signed=True), + ProtocolField(self.V, name="V", length=2), + ProtocolField(self.w, name="w", length=4, signed=True), + ProtocolField(self.angle_to_target, name="angle_to_target", length=4, signed=True), + ] + + @staticmethod + def from_bytes(bytes_) -> ProtocolData: + return EKFDebugData( + x=int.from_bytes(bytes_[0:4], "little", signed=True), + y=int.from_bytes(bytes_[4:8], "little", signed=True), + theta=int.from_bytes(bytes_[8:12], "little", signed=True), + V=int.from_bytes(bytes_[12:14], "little"), + w=int.from_bytes(bytes_[14:18], "little", signed=True), + angle_to_target=int.from_bytes(bytes_[18:22], "little", signed=True), + ) + @dataclass class ProtocolPayload: """Manage a protocol complete payload (header + type + values).""" @@ -411,6 +446,8 @@ def from_bytes(bytes_: bytes): values = LH2Waypoints.from_bytes(None) elif payload_type == PayloadType.GPS_WAYPOINTS: values = GPSWaypoints.from_bytes(None) + elif payload_type == PayloadType.EKF_DEBUG: + values = EKFDebugData.from_bytes(bytes_[21:41]) else: raise ProtocolPayloadParserException( f"Unsupported payload type '{payload_type}'" From 0fb58b88d460a656bee3020bd2f7c268e32d8725 Mon Sep 17 00:00:00 2001 From: Said Alvarado Date: Wed, 19 Apr 2023 13:51:03 +0200 Subject: [PATCH 2/3] Test ekf messages by printing them --- dotbot/controller.py | 18 +++++++++++++++++- dotbot/protocol.py | 6 +++--- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index a8d89b86..37fe1c6e 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -5,6 +5,8 @@ import math import time import webbrowser +from datetime import datetime +import pytz from abc import ABC, abstractmethod from binascii import hexlify @@ -137,6 +139,7 @@ def __init__(self, settings: ControllerSettings): self.websockets = [] self.lh2_manager = LighthouseManager() self.logger = LOGGER.bind(context=__name__) + self.now_utc = datetime.now(tz=pytz.utc) @abstractmethod def init(self): @@ -338,7 +341,7 @@ def handle_received_payload( y=dotbot.lh2_position.y, z=dotbot.lh2_position.z, ) - logger.info("lh2", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y) + logger.info("lh2", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y) ## TODO if ( not dotbot.position_history or lh2_distance(dotbot.position_history[-1], new_position) @@ -398,6 +401,19 @@ def handle_received_payload( else: notification = DotBotNotificationModel(cmd=notification_cmd.value) + ### EKF DATA PACKET + if payload.payload_type == PayloadType.EKF_DEBUG: + x = payload.values.x / 1e4 # [cm] + y = payload.values.y / 1e4 # [cm] + theta = payload.values.theta / 1e3 # [degrees] + V = payload.values.V / 10.0 # [cm/s] + w = payload.values.w / 1e3 # [degrees per second] + angle_to_target = payload.values.angle_to_target # [degrees] + # Print all this information + now_utc = datetime.now(tz=pytz.utc) + print(f"{now_utc.strftime('%Y-%m-%dT%H:%M:%S.%fZ')}\tx={x:<12.2f}\ty={y:<12.2f}\tthe={theta:<12.2f}\tV={V:<12.2f}\tw={w:<12.2f}\ttarget={angle_to_target:<12.2f}") + # print(payload) + if self.settings.verbose is True: print(payload) self.dotbots.update({dotbot.address: dotbot}) diff --git a/dotbot/protocol.py b/dotbot/protocol.py index ecc5b780..85179b4c 100644 --- a/dotbot/protocol.py +++ b/dotbot/protocol.py @@ -11,7 +11,7 @@ from dataclasses import dataclass -PROTOCOL_VERSION = 7 +PROTOCOL_VERSION = 8 class PayloadType(Enum): @@ -377,7 +377,7 @@ def fields(self) -> List[ProtocolField]: ProtocolField(self.x, name="x", length=4, signed=True), ProtocolField(self.y, name="y", length=4, signed=True), ProtocolField(self.theta, name="theta", length=4, signed=True), - ProtocolField(self.V, name="V", length=2), + ProtocolField(self.V, name="V", length=2, signed=True), ProtocolField(self.w, name="w", length=4, signed=True), ProtocolField(self.angle_to_target, name="angle_to_target", length=4, signed=True), ] @@ -388,7 +388,7 @@ def from_bytes(bytes_) -> ProtocolData: x=int.from_bytes(bytes_[0:4], "little", signed=True), y=int.from_bytes(bytes_[4:8], "little", signed=True), theta=int.from_bytes(bytes_[8:12], "little", signed=True), - V=int.from_bytes(bytes_[12:14], "little"), + V=int.from_bytes(bytes_[12:14], "little", signed=True), w=int.from_bytes(bytes_[14:18], "little", signed=True), angle_to_target=int.from_bytes(bytes_[18:22], "little", signed=True), ) From fca7f7d0e59b2eee86b2d88cfb75cd0be65a9da8 Mon Sep 17 00:00:00 2001 From: Said Alvarado Date: Thu, 27 Jul 2023 17:01:45 +0200 Subject: [PATCH 3/3] Controller now logs raw lh2 count alongside LH2 x-y --- dotbot/controller.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/dotbot/controller.py b/dotbot/controller.py index 37fe1c6e..4dad2a56 100644 --- a/dotbot/controller.py +++ b/dotbot/controller.py @@ -330,7 +330,13 @@ def handle_received_payload( dotbot.direction = payload.values.direction logger = logger.bind(direction=dotbot.direction) - dotbot.lh2_position = self._compute_lh2_position(payload) + zzzz = self._compute_lh2_position(payload) # Add this to get the raw counts into the logger + + if zzzz is not None: + counts, dotbot.lh2_position = zzzz + else: + dotbot.lh2_position = None + if ( dotbot.lh2_position is not None and 0 <= dotbot.lh2_position.x <= 1 @@ -341,7 +347,7 @@ def handle_received_payload( y=dotbot.lh2_position.y, z=dotbot.lh2_position.z, ) - logger.info("lh2", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y) ## TODO + logger.info("lh2", x=dotbot.lh2_position.x, y=dotbot.lh2_position.y, c0=counts[0], c1=counts[1]) ## TODO if ( not dotbot.position_history or lh2_distance(dotbot.position_history[-1], new_position)