Implement the G Force Recording System (#392)

* Added Accelerometer.h. (#87)

* Reformatted Accelerometer.h. (#87)

* Updated the accelerometer. (#87)

* Added the other 5 DOFs in the data container. (#87)

* Added the other 4 DOFs in the headers. (#87)

* Added `Accelerometer`. (#87)

* Added `toString()`. (#87)

* Code reformatted. (#87)

* Bug fixed. (#87)

* Bug fixed. (#87)

* Changed the output type to `Acceleration` (#87)

* Enabled the accelerometer. (#87)

Author: ATATC

arduino/leads_vec_wsc/Accelerometer.cpp

@@ -0,0 +1,25 @@
+#include "Accelerometer.h"
+
+String Acceleration::toString() {
+    return String(yaw) + "," + pitch + "," + roll + "," + forwardAcceleration + "," + lateralAcceleration + "," + verticalAcceleration;
+}
+Accelerometer::Accelerometer(OnAccelerometerUpdate onUpdate) : _onUpdate(onUpdate) {}
+void Accelerometer::initialize(const ArrayList<String> &parentTags) {
+    Device<Acceleration>::initialize(parentTags);
+    Serial1.begin(115200);
+    while (!Serial1) delay(10);
+    if (!_rvc.begin(&Serial1)) delay(10);
+}
+Acceleration Accelerometer::read() {
+    BNO08x_RVC_Data heading;
+    Acceleration r = Acceleration();
+    if (!_rvc.read(&heading)) return r;
+    r.yaw = heading.yaw;
+    r.pitch = heading.pitch;
+    r.roll = heading.roll;
+    r.forwardAcceleration = heading.x_accel;
+    r.lateralAcceleration = heading.y_accel;
+    r.verticalAcceleration = heading.z_accel;
+    _onUpdate(r);
+    return r;
+}

arduino/leads_vec_wsc/Accelerometer.h

@@ -0,0 +1,23 @@
+#include "Adafruit_BNO08x_RVC.h"
+#include "LEADS.h"
+
+
+struct Acceleration {
+    float yaw = 0, pitch = 0, roll = 0;
+    float forwardAcceleration = 0, lateralAcceleration = 0, verticalAcceleration = 0;
+    String toString();
+};
+
+
+typedef void (*OnAccelerometerUpdate)(Acceleration acceleration);
+
+
+class Accelerometer : public Device<Acceleration> {
+protected:
+    Adafruit_BNO08x_RVC _rvc = Adafruit_BNO08x_RVC();
+    const OnAccelerometerUpdate _onUpdate;
+public:
+    Accelerometer(OnAccelerometerUpdate onUpdate);
+    void initialize(const ArrayList<String> &parentTags) override;
+    Acceleration read() override;
+};

arduino/leads_vec_wsc/leads_vec_wsc.ino

@@ -1,4 +1,4 @@
-#include "LEADS.h"  // LEADS>=1.1.0
+#include "Accelerometer.h"
 
 const int PIN_LFWSS[] = {2};
 const int PIN_RFWSS[] = {3};
@@ -12,6 +12,7 @@ WheelSpeedSensor RFWSS{ArrayList<int>(PIN_RFWSS, 1), [](float ws) {returnFloat(P
 WheelSpeedSensor LRWSS{ArrayList<int>(PIN_LRWSS, 1), [](float ws) {returnFloat(P, LEFT_REAR_WHEEL_SPEED_SENSOR, ws);}};
 WheelSpeedSensor RRWSS{ArrayList<int>(PIN_RRWSS, 1), [](float ws) {returnFloat(P, RIGHT_REAR_WHEEL_SPEED_SENSOR, ws);}};
 WheelSpeedSensor CRWSS{ArrayList<int>(PIN_CRWSS, 1), [](float ws) {returnFloat(P, CENTER_REAR_WHEEL_SPEED_SENSOR, ws);}};
+Accelerometer ACCL{[](Acceleration acceleration) {returnString(P, ACCELEROMETER, acceleration.toString());}};
 
 void setup() {
     P.initializeAsRoot();
@@ -20,6 +21,7 @@ void setup() {
     LRWSS.initializeAsRoot();
     RRWSS.initializeAsRoot();
     CRWSS.initializeAsRoot();
+    ACCL.initializeAsRoot();
 }
 
 void loop() {
@@ -29,4 +31,5 @@ void loop() {
     LRWSS.read();
     RRWSS.read();
     CRWSS.read();
+    ACCL.read();
 }

leads/data.py

@@ -17,8 +17,12 @@ def __init__(self,
                  speed: float = 0,
                  front_wheel_speed: float = 0,
                  rear_wheel_speed: float = 0,
+                 yaw: float = 0,
+                 pitch: float = 0,
+                 roll: float = 0,
                  forward_acceleration: float = 0,
                  lateral_acceleration: float = 0,
+                 vertical_acceleration: float = 0,
                  front_proximity: float = -1,
                  left_proximity: float = -1,
                  right_proximity: float = -1,
@@ -37,8 +41,12 @@ def __init__(self,
         self.speed: float = speed
         self.front_wheel_speed: float = front_wheel_speed
         self.rear_wheel_speed: float = rear_wheel_speed
+        self.yaw: float = yaw
+        self.pitch: float = pitch
+        self.roll: float = roll
         self.forward_acceleration: float = forward_acceleration
         self.lateral_acceleration: float = lateral_acceleration
+        self.vertical_acceleration: float = vertical_acceleration
         self.front_proximity: float = front_proximity
         self.left_proximity: float = left_proximity
         self.right_proximity: float = right_proximity
@@ -96,8 +104,12 @@ def __init__(self,
                  speed: float = 0,
                  front_wheel_speed: float = 0,
                  rear_wheel_speed: float = 0,
+                 yaw: float = 0,
+                 pitch: float = 0,
+                 roll: float = 0,
                  forward_acceleration: float = 0,
                  lateral_acceleration: float = 0,
+                 vertical_acceleration: float = 0,
                  front_proximity: float = -1,
                  left_proximity: float = -1,
                  right_proximity: float = -1,
@@ -119,10 +131,10 @@ def __init__(self,
                  rear_view_base64: str = "",
                  rear_view_latency: int = 0,
                  **kwargs) -> None:
-        super().__init__(voltage, speed, front_wheel_speed, rear_wheel_speed, forward_acceleration,
-                         lateral_acceleration, front_proximity, left_proximity, right_proximity, rear_proximity,
-                         mileage, gps_valid, gps_ground_speed, latitude, longitude, steering_position, throttle, brake,
-                         **kwargs)
+        super().__init__(voltage, speed, front_wheel_speed, rear_wheel_speed, yaw, pitch, roll, forward_acceleration,
+                         lateral_acceleration, vertical_acceleration, front_proximity, left_proximity, right_proximity,
+                         rear_proximity, mileage, gps_valid, gps_ground_speed, latitude, longitude, steering_position,
+                         throttle, brake, **kwargs)
         self.front_view_base64: str = front_view_base64
         self.front_view_latency: int = front_view_latency
         self.left_view_base64: str = left_view_base64

leads/data_persistence/core.py

@@ -7,7 +7,7 @@
 from numpy import nan as _nan
 
 from leads.types import Compressor as _Compressor, VisualHeader as _VisualHeader, \
-    VisualHeaderFull as _VisualHeaderFull, DefaultHeaderFull as _DefaultHeaderFull
+    VisualHeaderFull as _VisualHeaderFull, DefaultHeaderFull as _DefaultHeaderFull, DefaultHeader as _DefaultHeader
 from ._computational import mean as _mean, array as _array, norm as _norm, read_csv as _read_csv, \
     DataFrame as _DataFrame, TextFileReader as _TextFileReader
 
@@ -253,10 +253,10 @@ def close(self) -> None:
             self._csv.close()
 
 
-DEFAULT_HEADER: tuple[str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str] = (
-    "t", "voltage", "speed", "front_wheel_speed", "rear_wheel_speed", "forward_acceleration", "lateral_acceleration",
-    "front_proximity", "left_proximity", "right_proximity", "rear_proximity", "mileage", "gps_valid",
-    "gps_ground_speed", "latitude", "longitude"
+DEFAULT_HEADER: _DefaultHeader = (
+    "t", "voltage", "speed", "front_wheel_speed", "rear_wheel_speed", "yaw", "pitch", "roll", "forward_acceleration",
+    "lateral_acceleration", "vertical_acceleration", "front_proximity", "left_proximity", "right_proximity",
+    "rear_proximity", "mileage", "gps_valid", "gps_ground_speed", "latitude", "longitude"
 )
 DEFAULT_HEADER_FULL: _DefaultHeaderFull = DEFAULT_HEADER + (
     "steering_position", "throttle", "brake"

leads/types.py

@@ -6,11 +6,13 @@
 type OnRegisterChain[T] = _Callable[[OnRegister[T]], OnRegister[T]]
 type SupportedConfigValue = bool | int | float | str | None
 type SupportedConfig = SupportedConfigValue | tuple[SupportedConfig, ...]
+type DefaultHeader = tuple[
+    str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str]
 type DefaultHeaderFull = tuple[
-    str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str]
+    str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str]
 type VisualHeader = tuple[
     str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str,
-    str]
+    str, str, str, str, str]
 type VisualHeaderFull = tuple[
     str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str, str,
-    str, str, str, str]
+    str, str, str, str, str, str, str, str]

leads_arduino/__init__.py

@@ -3,6 +3,7 @@
 if not _find_spec("serial"):
     raise ImportError("Please install `pyserial` to run this module\n>>>pip install pyserial")
 
+from leads_arduino.accelerometer import *
 from leads_arduino.arduino_proto import *
 from leads_arduino.arduino_nano import *
 from leads_arduino.arduino_micro import *

leads_arduino/accelerometer.py

@@ -0,0 +1,33 @@
+from dataclasses import dataclass as _dataclass
+from typing import override as _override
+
+from leads import Device as _Device, Serializable as _Serializable
+
+
+@_dataclass
+class Acceleration(_Serializable):
+    yaw: float
+    pitch: float
+    roll: float
+    forward_acceleration: float
+    lateral_acceleration: float
+    vertical_acceleration: float
+
+
+class Accelerometer(_Device):
+    def __init__(self) -> None:
+        super().__init__()
+        self._yaw: float = 0
+        self._pitch: float = 0
+        self._roll: float = 0
+        self._fa: float = 0
+        self._la: float = 0
+        self._va: float = 0
+
+    @_override
+    def update(self, data: str) -> None:
+        self._yaw, self._pitch, self._roll, self._fa, self._la, self._va = tuple(map(float, data.split(',')))
+
+    @_override
+    def read(self) -> Acceleration:
+        return Acceleration(self._yaw, self._pitch, self._roll, self._fa, self._la, self._va)

leads_can/prototype.py

@@ -12,7 +12,6 @@ def __init__(self) -> None:
         self._bus: _Bus | None = None
         self._notifier: _Notifier | None = None
 
-    @_override
     def on_message_received(self, msg: _Message) -> None:
         for device in self.devices():
             device.update(msg)

leads_vec/devices.py

@@ -4,8 +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
-from leads_arduino import ArduinoMicro, WheelSpeedSensor, VoltageSensor
+    SFT, read_device_marker, has_controller, POWER_CONTROLLER, WHEEL_SPEED_CONTROLLER, ACCELEROMETER
+from leads_arduino import ArduinoMicro, WheelSpeedSensor, VoltageSensor, Accelerometer
 from leads_gpio import NMEAGPSReceiver, LEDGroup, LED, LEDGroupCommand, LEDCommand, Entire, Transition
 from leads_vec.config import Config
 from leads_video import Base64Camera, get_camera
@@ -57,7 +57,9 @@ def read(self) -> DataContainer:
             "longitude": gps[3],
             **self.device(POWER_CONTROLLER).read()
         }
-        wsc = {"speed": gps[0]} if GPS_ONLY else self.device(WHEEL_SPEED_CONTROLLER).read()
+        wsc = self.device(WHEEL_SPEED_CONTROLLER).read()
+        if GPS_ONLY:
+            wsc["speed"] = gps[1]
         visual = {}
         if has_device(FRONT_VIEW_CAMERA):
             cam = get_camera(FRONT_VIEW_CAMERA, Base64Camera)
@@ -113,9 +115,9 @@ def initialize(self, *parent_tags: str) -> None:
     def read(self) -> dict[str, float]:
         lfws = self.device(LEFT_FRONT_WHEEL_SPEED_SENSOR).read()
         rfws = self.device(RIGHT_FRONT_WHEEL_SPEED_SENSOR).read()
-        front_wheel_speed = (lfws + rfws) * .5
         return {"speed": min(lfws, rfws, rws := self.device(CENTER_REAR_WHEEL_SPEED_SENSOR).read()),
-                "front_wheel_speed": front_wheel_speed, "rear_wheel_speed": rws}
+                "front_wheel_speed": (lfws + rfws) * .5, "rear_wheel_speed": rws,
+                **self.device(ACCELEROMETER).read().to_dict()}
 
 
 @device(ODOMETER, MAIN_CONTROLLER)
@@ -141,6 +143,14 @@ def initialize(self, *parent_tags: str) -> None:
         super().initialize(*parent_tags)
 
 
+@device(ACCELEROMETER, WHEEL_SPEED_CONTROLLER)
+class Accele(Accelerometer):
+    @override
+    def initialize(self, *parent_tags: str) -> None:
+        mark_device(self, "WSC", "ESC")
+        super().initialize(*parent_tags)
+
+
 @device(GPS_RECEIVER, MAIN_CONTROLLER, (GPS_RECEIVER_PORT,))
 class GPS(NMEAGPSReceiver):
     @override