[RotatedIMU] Add rotated IMU sensor for rotating IMU data in software

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

src/shared/sensors/RotatedIMU/IMUData.h

+/* 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

src/shared/sensors/RotatedIMU/RotatedIMU.cpp

+/* 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

src/shared/sensors/RotatedIMU/RotatedIMU.h

+/* 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