diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake index edb5ef8fea8950dde829c591ff98da600bea0348..ae8a0541ac65e343a7de3a3caeafa9c5af91233e 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 0000000000000000000000000000000000000000..d22b514f49541811847ce658ab9c6ddbea39d99f --- /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 0000000000000000000000000000000000000000..476f75bb31a64c9c28baed083785678f20cb4ee8 --- /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 0000000000000000000000000000000000000000..4083135797f18a7604846bd96a7a92e7cff02e6e --- /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