ALPAKA/SPATZ
1
0
Fork 0
mirror of https://git.intern.spaceteamaachen.de/ALPAKA/SPATZ.git synced 2025-12-17 01:38:02 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Updated sensors

Browse Source
This commit is contained in:
dario
2024-06-20 18:13:59 +02:00
parent c0ccd93acf
commit 43bc71c742
11 changed files with 57112 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
spatz/sensors/imu/accelerometer.py
View File

@@ -29,7 +29,6 @@ class Accelerometer(Sensor):
def __init__(
self, dataset: DataSource,
logger: Logger,
coord_system: CoordSystem = CoordSystem.RIGHT_HANDED,
orientation: NDArray = np.identity(3),
offset: float = 0,
transforms: List[Transform] = []):
@@ -48,27 +47,22 @@ class Accelerometer(Sensor):
assert orientation.shape == (3, 3), 'Orientation has to be a 3x3 matrix.'
self._offset = np.array([offset, 0, 0])
self._coord_system = coord_system
self._orientation = orientation
def _get_data(self) -> ArrayLike | float:
acc = self._dataset.get_acceleration('global')
# self._logger.write('global_ax', acc[0], self._get_name())
# self._logger.write('global_ay', acc[1], self._get_name())
# self._logger.write('global_az', acc[2], self._get_name())
self._logger.write('global_ax', acc[0], self._get_name())
self._logger.write('global_ay', acc[1], self._get_name())
self._logger.write('global_az', acc[2], self._get_name())
# Convert FL to body
acc = self._dataset.global_to_local() @ acc
# acc += mat @ np.array([0, 0, g])
acc += self._dataset.global_to_local() @ np.array([0, 0, g])
# self._logger.write('local_x', acc[0], self._get_name())
# self._logger.write('local_y', acc[1], self._get_name())
# self._logger.write('local_z', acc[2], self._get_name())
# Flip axes to sensor's perspective. Spatz uses a right-handed coordinate system.
#if self._coord_system == CoordSystem.LEFT_HANDED:
# acc[1] *= -1
self._logger.write('local_x', acc[0], self._get_name())
self._logger.write('local_y', acc[1], self._get_name())
self._logger.write('local_z', acc[2], self._get_name())
# Rotate the acceleration vector to accomodate the accelerometer's orientation on the rocket.
acc = self._orientation @ acc