Skip to content

90 add support to ekf debug radio protocol messages #95

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
SaidAlvarado wants to merge 3 commits into
base: main
Choose a base branch
from
Open

90 add support to ekf debug radio protocol messages #95

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
daddd27
add support for the ekf_debug protocol
SaidAlvarado Apr 3, 2023
0fb58b8
Test ekf messages by printing them
SaidAlvarado Apr 19, 2023
fca7f7d
Controller now logs raw lh2 count alongside LH2 x-y
SaidAlvarado Jul 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 24 additions & 2 deletions dotbot/controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -327,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
Expand All @@ -338,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)
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)
Expand Down Expand Up @@ -398,6 +407,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})
Expand Down
41 changes: 39 additions & 2 deletions dotbot/protocol.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
from dataclasses import dataclass


PROTOCOL_VERSION = 7
PROTOCOL_VERSION = 8


class PayloadType(Enum):
Expand All @@ -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):
Expand Down Expand Up @@ -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, signed=True),
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", signed=True),
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)."""
Expand Down Expand Up @@ -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}'"
Expand Down