From 0680e3958fa20f82e3f43fd3f43d4b6a5ad2aebb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Tue, 30 Jul 2024 17:34:37 +0200
Subject: [PATCH] [RotatedIMU] Add rotated IMU sensor for rotating IMU data in
software
---
cmake/boardcore.cmake | 1 +
src/shared/sensors/RotatedIMU/IMUData.h | 60 ++++++++++
src/shared/sensors/RotatedIMU/RotatedIMU.cpp | 112 +++++++++++++++++++
src/shared/sensors/RotatedIMU/RotatedIMU.h | 111 ++++++++++++++++++
4 files changed, 284 insertions(+)
create mode 100644 src/shared/sensors/RotatedIMU/IMUData.h
create mode 100644 src/shared/sensors/RotatedIMU/RotatedIMU.cpp
create mode 100644 src/shared/sensors/RotatedIMU/RotatedIMU.h
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index edb5ef8fe..ae8a0541a 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -105,6 +105,7 @@ set(BOARDCORE_SRC
${BOARDCORE_PATH}/src/shared/sensors/MPU9250/MPU9250.cpp
${BOARDCORE_PATH}/src/shared/sensors/MS5803/MS5803.cpp
${BOARDCORE_PATH}/src/shared/sensors/MS5803/MS5803I2C.cpp
+ ${BOARDCORE_PATH}/src/shared/sensors/RotatedIMU/RotatedIMU.cpp
${BOARDCORE_PATH}/src/shared/sensors/SensorManager.cpp
${BOARDCORE_PATH}/src/shared/sensors/SensorSampler.cpp
${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
diff --git a/src/shared/sensors/RotatedIMU/IMUData.h b/src/shared/sensors/RotatedIMU/IMUData.h
new file mode 100644
index 000000000..d22b514f4
--- /dev/null
+++ b/src/shared/sensors/RotatedIMU/IMUData.h
@@ -0,0 +1,60 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <sensors/SensorData.h>
+
+namespace Boardcore
+{
+
+struct IMUData : AccelerometerData, GyroscopeData, MagnetometerData
+{
+ IMUData() = default;
+
+ IMUData(const AccelerometerData& accData, const GyroscopeData& gyroData,
+ const MagnetometerData& magData)
+ : AccelerometerData(accData), GyroscopeData(gyroData),
+ MagnetometerData(magData)
+ {
+ }
+
+ static std::string header()
+ {
+ return "accelerationTimestamp,accelerationX,accelerationY,"
+ "accelerationZ,angularSpeedTimestamp,angularSpeedX,"
+ "angularSpeedY,angularSpeedZ,magneticFieldTimestamp,"
+ "magneticFieldX,magneticFieldY,magneticFieldZ\n";
+ }
+
+ void print(std::ostream& os) const
+ {
+ os << accelerationTimestamp << "," << accelerationX << ","
+ << accelerationY << "," << accelerationZ << ","
+ << angularSpeedTimestamp << "," << angularSpeedX << ","
+ << angularSpeedY << "," << angularSpeedZ << ","
+ << magneticFieldTimestamp << "," << magneticFieldX << ","
+ << magneticFieldY << "," << magneticFieldZ << "\n";
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/RotatedIMU/RotatedIMU.cpp b/src/shared/sensors/RotatedIMU/RotatedIMU.cpp
new file mode 100644
index 000000000..476f75bb3
--- /dev/null
+++ b/src/shared/sensors/RotatedIMU/RotatedIMU.cpp
@@ -0,0 +1,112 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "RotatedIMU.h"
+
+using namespace Eigen;
+
+namespace Boardcore
+{
+
+RotatedIMU::RotatedIMU(SampleIMUFunction sampleImuFunction)
+ : sampleImu(std::move(sampleImuFunction))
+{
+}
+
+void RotatedIMU::addAccTransformation(const Matrix3f& t) { accT = t * accT; }
+void RotatedIMU::addGyroTransformation(const Matrix3f& t) { gyroT = t * gyroT; }
+void RotatedIMU::addMagTransformation(const Matrix3f& t) { magT = t * magT; }
+
+void RotatedIMU::resetTransformations()
+{
+ accT = Matrix3f::Identity();
+ gyroT = Matrix3f::Identity();
+ magT = Matrix3f::Identity();
+}
+
+Matrix3f RotatedIMU::rotateAroundX(float angle)
+{
+ Matrix3f rotation;
+ angle = angle * EIGEN_PI / 180.f;
+
+ // clang-format off
+ rotation = Matrix3f{{1, 0, 0},
+ {0, cosf(angle), -sinf(angle)},
+ {0, sinf(angle), cosf(angle)}};
+ // clang-format on
+
+ return rotation;
+}
+
+Matrix3f RotatedIMU::rotateAroundY(float angle)
+{
+ Matrix3f rotation;
+ angle = angle * EIGEN_PI / 180.f;
+
+ // clang-format off
+ rotation = Matrix3f{{cosf(angle), 0, sinf(angle)},
+ {0, 1, 0},
+ {-sinf(angle), 0, cosf(angle)}};
+ // clang-format on
+
+ return rotation;
+}
+
+Matrix3f RotatedIMU::rotateAroundZ(float angle)
+{
+ Matrix3f rotation;
+ angle = angle * EIGEN_PI / 180.f;
+
+ // clang-format off
+ rotation = Matrix3f{{cosf(angle), -sinf(angle), 0},
+ {sinf(angle), cosf(angle), 0},
+ {0, 0, 1}};
+ // clang-format on
+
+ return rotation;
+}
+
+IMUData RotatedIMU::sampleImpl()
+{
+ auto imuData = sampleImu();
+
+ AccelerometerData& accData = imuData;
+ GyroscopeData& gyroData = imuData;
+ MagnetometerData& magData = imuData;
+
+ Vector3f rotatedAcc = accT * static_cast<Vector3f>(accData);
+ Vector3f rotatedGyro = gyroT * static_cast<Vector3f>(gyroData);
+ Vector3f rotatedMag = magT * static_cast<Vector3f>(magData);
+
+ auto acc = AccelerometerData{rotatedAcc};
+ acc.accelerationTimestamp = accData.accelerationTimestamp;
+
+ auto gyro = GyroscopeData{rotatedGyro};
+ gyro.angularSpeedTimestamp = gyroData.angularSpeedTimestamp;
+
+ auto mag = MagnetometerData{rotatedMag};
+ mag.magneticFieldTimestamp = magData.magneticFieldTimestamp;
+
+ return {acc, gyro, mag};
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/RotatedIMU/RotatedIMU.h b/src/shared/sensors/RotatedIMU/RotatedIMU.h
new file mode 100644
index 000000000..408313579
--- /dev/null
+++ b/src/shared/sensors/RotatedIMU/RotatedIMU.h
@@ -0,0 +1,111 @@
+/* Copyright (c) 2024 Skyward Experimental Rocketry
+ * Author: Niccolò Betto
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <sensors/Sensor.h>
+
+#include <Eigen/Eigen>
+#include <functional>
+
+#include "IMUData.h"
+
+namespace Boardcore
+{
+
+/**
+ * @brief A software IMU sensor that allows applying transformations to the data
+ * after sampling via a callback. Defaults to identity transformations.
+ */
+class RotatedIMU : public Boardcore::Sensor<IMUData>
+{
+public:
+ using SampleIMUFunction = std::function<IMUData()>;
+
+ /**
+ * @brief Construct a new Rotated IMU object
+ *
+ * @param sampleFunction Callback to retrieve accelerometer, magnetometer
+ * and gyroscope data
+ */
+ explicit RotatedIMU(SampleIMUFunction sampleFunction);
+
+ bool init() override { return true; }
+ bool selfTest() override { return true; }
+
+ /**
+ * @brief Multiplies the current accelerometer transformation
+ * @param transformation Transformation matrix to be multiplied to the
+ * current one
+ */
+ void addAccTransformation(const Eigen::Matrix3f& t);
+
+ /**
+ * @brief Multiplies the current gyroscope transformation
+ * @param transformation Transformation matrix to be multiplied to the
+ * current one
+ */
+ void addGyroTransformation(const Eigen::Matrix3f& t);
+
+ /**
+ * @brief Multiplies the current magnetometer transformation
+ * @param transformation Transformation matrix to be multiplied to the
+ * current one
+ */
+ void addMagTransformation(const Eigen::Matrix3f& t);
+
+ /**
+ * @brief Resets all the transformations to the original (Identity) ones
+ */
+ void resetTransformations();
+
+ /**
+ * @brief Creates a rotation matrix around the X axis
+ * @param angle Angle in degrees
+ */
+ static Eigen::Matrix3f rotateAroundX(float angle);
+
+ /**
+ * @brief Creates a rotation matrix around the Y axis
+ * @param angle Angle in degrees
+ */
+ static Eigen::Matrix3f rotateAroundY(float angle);
+
+ /**
+ * @brief Creates a rotation matrix around the Z axis
+ * @param angle Angle in degrees
+ */
+ static Eigen::Matrix3f rotateAroundZ(float angle);
+
+protected:
+ IMUData sampleImpl() override;
+
+private:
+ SampleIMUFunction sampleImu;
+
+ // Transformation matrices
+ Eigen::Matrix3f accT = Eigen::Matrix3f::Identity();
+ Eigen::Matrix3f gyroT = Eigen::Matrix3f::Identity();
+ Eigen::Matrix3f magT = Eigen::Matrix3f::Identity();
+};
+
+} // namespace Boardcore
--
GitLab