Supported odometer under GPS-only mode. (#462)

ProjectNeura · Jan 5, 2025 · 60f2167 · 60f2167
1 parent 702ee80
commit 60f2167
Showing 1 changed file with 14 additions and 3 deletions.
diff --git a/leads_vec/devices.py b/leads_vec/devices.py
@@ -4,7 +4,8 @@
     Controller, CENTER_REAR_WHEEL_SPEED_SENSOR, require_config, mark_device, ODOMETER, GPS_RECEIVER, \
     ConcurrentOdometer, LEFT_INDICATOR, RIGHT_INDICATOR, VOLTAGE_SENSOR, DataContainer, has_device, \
     FRONT_VIEW_CAMERA, LEFT_VIEW_CAMERA, RIGHT_VIEW_CAMERA, REAR_VIEW_CAMERA, VisualDataContainer, BRAKE_INDICATOR, \
-    SFT, read_device_marker, has_controller, POWER_CONTROLLER, WHEEL_SPEED_CONTROLLER, ACCELEROMETER, require_context
+    SFT, read_device_marker, has_controller, POWER_CONTROLLER, WHEEL_SPEED_CONTROLLER, ACCELEROMETER, require_context, \
+    ltm_get, ltm_set, distance_between
 from leads_arduino import ArduinoMicro, WheelSpeedSensor, VoltageSensor, Accelerometer, Acceleration
 from leads_comm_serial import SOBD
 from leads_gpio import NMEAGPSReceiver, LEDGroup, LED, LEDGroupCommand, LEDCommand, Entire, Transition, Button, \
@@ -54,16 +55,19 @@ def initialize(self, *parent_tags: str) -> None:
     @override
     def read(self) -> DataContainer:
         general = {
-            "mileage": self.device(ODOMETER).read(),
             "gps_valid": (gps := self.device(GPS_RECEIVER).read())[0],
             "gps_ground_speed": gps[1],
             "latitude": gps[2],
             "longitude": gps[3],
             **self.device(POWER_CONTROLLER).read()
         }
         wsc = self.device(WHEEL_SPEED_CONTROLLER).read()
+        odometer = self.device(ODOMETER)
         if GPS_ONLY:
             wsc["speed"] = gps[1]
+            prev = require_context().data()
+            odometer.write(odometer.read() + distance_between(prev.latitude, prev.longitude, gps[2], gps[3]))
+        general["mileage"] = odometer.read()
         visual = {}
         if has_device(FRONT_VIEW_CAMERA):
             cam = get_camera(FRONT_VIEW_CAMERA, Base64Camera)
@@ -138,10 +142,17 @@ class AverageOdometer(ConcurrentOdometer):
     def initialize(self, *parent_tags: str) -> None:
         mark_device(self, "WSC")
         super().initialize(*parent_tags)
+        if config.use_ltm:
+            self.write(ltm_get("mileage"))
+
+    @override
+    def write(self, payload: float) -> None:
+        super().write(payload)
+        ltm_set("mileage", payload)
 
     @override
     def read(self) -> float:
-        return super().read() / 3
+        return super().read() if GPS_ONLY else super().read() / 3
 
 
 @device(*(((LEFT_FRONT_WHEEL_SPEED_SENSOR, RIGHT_FRONT_WHEEL_SPEED_SENSOR, CENTER_REAR_WHEEL_SPEED_SENSOR),