diff --git a/arduino/leads_vec_wsc/Accelerometer.cpp b/arduino/leads_vec_wsc/Accelerometer.cpp index 199771d..373bfe6 100644 --- 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 index 812571a..5433b5f 100644 --- 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 index da2e74b..f35bb79 100644 --- 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):