Remove Gravity from Acceleration (#406)

* Added `linear()`. (#403) * Using linear acceleration. (#403) * Using a more accurate constant. (#403) * Bug fixed. (#403) (#407) * Bug fixed. (#403) (#407) * Code optimized. (#403) (#407)
ProjectNeura · Sep 30, 2024 · 7acb0c7 · 7acb0c7
1 parent d286c11
commit 7acb0c7
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 5 deletions.
diff --git a/arduino/leads_vec_wsc/Accelerometer.cpp b/arduino/leads_vec_wsc/Accelerometer.cpp
@@ -17,8 +17,8 @@ Acceleration Accelerometer::read() {
     r.yaw = heading.yaw;
     r.pitch = heading.pitch;
     r.roll = heading.roll;
-    r.forwardAcceleration = heading.x_accel;
-    r.lateralAcceleration = heading.y_accel;
+    r.forwardAcceleration = heading.y_accel;
+    r.lateralAcceleration = heading.x_accel;
     r.verticalAcceleration = heading.z_accel;
     _onUpdate(r);
     return r;

diff --git a/leads_arduino/accelerometer.py b/leads_arduino/accelerometer.py
@@ -1,9 +1,21 @@
 from dataclasses import dataclass as _dataclass
-from typing import override as _override
+from typing import override as _override, Self as _Self
+
+from numpy import ndarray as _ndarray, sin as _sin, cos as _cos, array as _array, deg2rad as _deg2rad
 
 from leads import Device as _Device, Serializable as _Serializable
 
 
+def rotation_matrix(yaw: float, pitch: float, roll: float) -> _ndarray:
+    yaw, pitch, roll = _deg2rad(yaw), _deg2rad(pitch), _deg2rad(roll)
+    sy, cy, sp, cp, sr, cr = _sin(yaw), _cos(yaw), _sin(pitch), _cos(pitch), _sin(roll), _cos(roll)
+    return _array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) @ _array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) @ _array(
+        [[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
+
+
+_G: _ndarray = _array([0, 0, 9.8067])
+
+
 @_dataclass
 class Acceleration(_Serializable):
     yaw: float
@@ -13,6 +25,11 @@ class Acceleration(_Serializable):
     lateral_acceleration: float
     vertical_acceleration: float
 
+    def linear(self) -> _Self:
+        fg = rotation_matrix(self.yaw, self.pitch, self.roll).T @ _G
+        return Acceleration(self.yaw, self.pitch, self.roll, self.forward_acceleration + fg[0],
+                            self.lateral_acceleration + fg[1], self.vertical_acceleration - fg[2])
+
 
 class Accelerometer(_Device):
     def __init__(self) -> None:

diff --git a/leads_vec/devices.py b/leads_vec/devices.py
@@ -5,7 +5,7 @@
     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
-from leads_arduino import ArduinoMicro, WheelSpeedSensor, VoltageSensor, Accelerometer
+from leads_arduino import ArduinoMicro, WheelSpeedSensor, VoltageSensor, Accelerometer, Acceleration
 from leads_gpio import NMEAGPSReceiver, LEDGroup, LED, LEDGroupCommand, LEDCommand, Entire, Transition
 from leads_vec.config import Config
 from leads_video import Base64Camera, get_camera
@@ -144,12 +144,16 @@ def initialize(self, *parent_tags: str) -> None:
 
 
 @device(ACCELEROMETER, WHEEL_SPEED_CONTROLLER)
-class Accele(Accelerometer):
+class LinearAccelerometer(Accelerometer):
     @override
     def initialize(self, *parent_tags: str) -> None:
         mark_device(self, "WSC", "ESC")
         super().initialize(*parent_tags)
 
+    @override
+    def read(self) -> Acceleration:
+        return super().read().linear()
+
 
 @device(GPS_RECEIVER, MAIN_CONTROLLER, (GPS_RECEIVER_PORT,))
 class GPS(NMEAGPSReceiver):