From 56d34e88b2f38bd787213eec4265821c43ff62e4 Mon Sep 17 00:00:00 2001 From: "Tianhao (Terry) Fu" Date: Mon, 30 Sep 2024 12:03:24 -0400 Subject: [PATCH] Add Wheel Speed Correction by Acceleration (#411) * Disabled `GPSSpeedCorrection`. (#409) * Supported skipping outliers. (#409) * Using linear acceleration. (#409) * Bug fixed: unmatched units. (#409) * Bug fixed: `Vector` is not hashable. (#409) * Removed `GPSSpeedCorrection`. (#409) * Code reformatted. (#409) * Bug fixed: zero division. (#409) --- leads/data_persistence/core.py | 4 ++++ leads/plugin/__init__.py | 1 - leads/plugin/gps_speed_correction.py | 23 ----------------------- leads_arduino/wheel_speed_sensor.py | 25 ++++++++++++++++++++++--- leads_vec/cli.py | 5 ++--- leads_vec_rc/cli.py | 3 ++- 6 files changed, 30 insertions(+), 31 deletions(-) delete mode 100644 leads/plugin/gps_speed_correction.py diff --git a/leads/data_persistence/core.py b/leads/data_persistence/core.py index 3b7956e3..7e76273b 100644 --- a/leads/data_persistence/core.py +++ b/leads/data_persistence/core.py @@ -106,6 +106,10 @@ def __init__(self, *coordinates: E) -> None: self._d: int = len(coordinates) self._coordinates: tuple[E, ...] = coordinates + @_override + def __hash__(self) -> int: + return hash(self._coordinates) + @_override def __len__(self) -> int: return self._d diff --git a/leads/plugin/__init__.py b/leads/plugin/__init__.py index 44cab167..9990a77d 100644 --- a/leads/plugin/__init__.py +++ b/leads/plugin/__init__.py @@ -2,5 +2,4 @@ from leads.plugin.atbs import * from leads.plugin.dtcs import * from leads.plugin.ebi import * -from leads.plugin.gps_speed_correction import * from leads.plugin.plugin import * diff --git a/leads/plugin/gps_speed_correction.py b/leads/plugin/gps_speed_correction.py deleted file mode 100644 index fb8e9e4f..00000000 --- a/leads/plugin/gps_speed_correction.py +++ /dev/null @@ -1,23 +0,0 @@ -from typing import override as _override, Any as _Any - -from leads.context import Context -from leads.plugin.plugin import Plugin - - -class GPSSpeedCorrection(Plugin): - def __init__(self) -> None: - super().__init__() - self._loss: float = 0 - self._abs_loss: float = 0 - self._n: int = 0 - - @_override - def post_push(self, context: Context, kwargs: dict[str, _Any]) -> None: - d = context.data() - if not d.gps_valid: - return - self._loss += (current_loss := d.speed - d.gps_ground_speed) - self._abs_loss += abs(current_loss) - self._n += 1 - if d.speed < 5 and current_loss < self._abs_loss / self._n: - d.speed = d.gps_ground_speed + self._loss / self._n diff --git a/leads_arduino/wheel_speed_sensor.py b/leads_arduino/wheel_speed_sensor.py index 797eae79..1395b8de 100644 --- a/leads_arduino/wheel_speed_sensor.py +++ b/leads_arduino/wheel_speed_sensor.py @@ -4,6 +4,7 @@ from numpy import pi as _pi from leads import Device as _Device, get_device as _get_device, Odometer as _Odometer +from leads_arduino.accelerometer import Accelerometer def rpm2kmh(rpm: float, wheel_circumference: float) -> float: @@ -23,7 +24,8 @@ class WheelSpeedSensor(_Device): - Any Hall effect sensor (switch) """ - def __init__(self, wheel_diameter: float, num_divisions: int = 1, odometer_tag: str | None = None) -> None: + def __init__(self, wheel_diameter: float, num_divisions: int = 1, odometer_tag: str | None = None, + accelerometer_tag: str | None = None) -> None: super().__init__() self._wheel_circumference: float = wheel_diameter * 2.54 * _pi self._num_divisions: int = num_divisions @@ -31,17 +33,34 @@ def __init__(self, wheel_diameter: float, num_divisions: int = 1, odometer_tag: self._last_valid: float = 0 self._odometer_tag: str | None = odometer_tag self._odometer: _Odometer | None = None + self._accelerometer_tag: str | None = accelerometer_tag + self._accelerometer: Accelerometer | None = None + self._last_acceleration: float = 0 @_override def initialize(self, *parent_tags: str) -> None: super().initialize(*parent_tags) if self._odometer_tag: self._odometer = _get_device(self._odometer_tag) + if not isinstance(self._odometer, _Odometer): + raise TypeError(f"Unexpected odometer type: {type(self._odometer)}") + if self._accelerometer_tag: + self._accelerometer = _get_device(self._accelerometer_tag) + if not isinstance(self._accelerometer, Accelerometer): + raise TypeError(f"Unexpected accelerometer type: {type(self._accelerometer)}") @_override def update(self, data: str) -> None: - self._wheel_speed = rpm2kmh(float(data), self._wheel_circumference) / self._num_divisions - self._last_valid = _time() + t = _time() + ws = rpm2kmh(float(data), self._wheel_circumference) / self._num_divisions + if self._accelerometer and self._last_acceleration: + a = self._accelerometer.read().linear().forward_acceleration + v = (a + self._last_acceleration) * 1.8 * (t - self._last_valid) + if abs((ws - self._wheel_speed - v) / (v + .0000000001)) > 1.5: + return + self._last_acceleration = a + self._wheel_speed = ws + self._last_valid = t if self._odometer: self._odometer.write(self._wheel_circumference * .00001) diff --git a/leads_vec/cli.py b/leads_vec/cli.py index 9eae544e..755ba0e0 100644 --- a/leads_vec/cli.py +++ b/leads_vec/cli.py @@ -9,8 +9,8 @@ from pynput.keyboard import Listener as _Listener, Key as _Key, KeyCode as _KeyCode from screeninfo import get_monitors as _get_monitors -from leads import LEADS, SystemLiteral, require_config, register_context, DTCS, ABS, EBI, ATBS, GPSSpeedCorrection, \ - ESCMode, L, EventListener, DataPushedEvent, UpdateEvent, has_device, GPS_RECEIVER, get_device, InterventionEvent, \ +from leads import LEADS, SystemLiteral, require_config, register_context, DTCS, ABS, EBI, ATBS, ESCMode, L, \ + EventListener, DataPushedEvent, UpdateEvent, has_device, GPS_RECEIVER, get_device, InterventionEvent, \ SuspensionEvent, Event, LEFT_INDICATOR, RIGHT_INDICATOR, format_duration, BRAKE_INDICATOR, REAR_VIEW_CAMERA, \ FRONT_VIEW_CAMERA, LEFT_VIEW_CAMERA, RIGHT_VIEW_CAMERA, SuspensionExitEvent from leads.comm import Callback, Service, start_server, create_server, my_ip_addresses, ConnectionBase @@ -153,7 +153,6 @@ def main() -> int: ctx.plugin(SystemLiteral.ABS, ABS()) ctx.plugin(SystemLiteral.EBI, EBI()) ctx.plugin(SystemLiteral.ATBS, ATBS()) - ctx.plugin("GPS_SPEED_CORRECTION", GPSSpeedCorrection()) w = Window(cfg.width, cfg.height, cfg.refresh_rate, CustomRuntimeData(), fullscreen=cfg.fullscreen, no_title_bar=cfg.no_title_bar, theme_mode=cfg.theme_mode) root = w.root() diff --git a/leads_vec_rc/cli.py b/leads_vec_rc/cli.py index c069c58c..678817ac 100644 --- a/leads_vec_rc/cli.py +++ b/leads_vec_rc/cli.py @@ -65,7 +65,8 @@ def on_receive(self, service: Service, msg: bytes) -> None: self.super(service=service, msg=msg) try: self.current_data = d = loads(msg.decode()) - acceleration_record.append(Vector(d["forward_acceleration"], d["lateral_acceleration"])) + acceleration_record.append(Vector(d["forward_acceleration"], d["lateral_acceleration"], + d["vertical_acceleration"])) gps_record.append(Vector(d["latitude"], d["longitude"])) if config.save_data: try_create_csv(d)