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

@@ -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

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 *

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,25 +24,43 @@ 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
         self._wheel_speed: float = 0
         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)
 

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()

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)