diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake index 3cb66c2073de672883a6ace4b4bbfca3a426fd2e..6508dc5c7e23f0ff9b7fa50ac09146dabe2e0053 100644 --- a/cmake/dependencies.cmake +++ b/cmake/dependencies.cmake @@ -94,7 +94,6 @@ set(PAYLOAD_COMPUTER src/boards/Payload/Wing/Guidance/EarlyManeuverGuidanceAlgorithm.cpp src/boards/Payload/Wing/FileWingAlgorithm.cpp src/boards/Payload/Wing/WingAlgorithm.cpp - src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp src/boards/Payload/WindEstimationScheme/WindEstimation.cpp ) diff --git a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp index 694c664d5221905db7bf289cd8fae6ab288094cc..2790a1b327c35b0500b789d97745949e15981345 100644 --- a/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp +++ b/src/boards/Payload/FlightStatsRecorder/FlightStatsRecorder.cpp @@ -71,7 +71,7 @@ void FlightStatsRecorder::update() // Data gathering Sensors* sensor = getModule<Sensors>(); - AccelerometerData accData = sensor->getIMULastSample().accData; + AccelerometerData accData = sensor->getIMULastSample(); PressureData baroData = sensor->getStaticPressure(); PitotData pitotData = sensor->getPitotLastSample(); GPSData gpsData = sensor->getUBXGPSLastSample(); diff --git a/src/boards/Payload/Sensors/RotatedIMU/IMUData.h b/src/boards/Payload/Sensors/RotatedIMU/IMUData.h deleted file mode 100644 index d2f487a1a8c7cd294b13fce659022438f577f213..0000000000000000000000000000000000000000 --- a/src/boards/Payload/Sensors/RotatedIMU/IMUData.h +++ /dev/null @@ -1,50 +0,0 @@ -/* 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> - -struct IMUData -{ - Boardcore::AccelerometerData accData; - Boardcore::GyroscopeData gyroData; - Boardcore::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 << accData.accelerationTimestamp << "," << accData.accelerationX - << "," << accData.accelerationY << "," << accData.accelerationZ - << "," << gyroData.angularSpeedTimestamp << "," - << gyroData.angularSpeedX << "," << gyroData.angularSpeedY << "," - << gyroData.angularSpeedZ << "," << magData.magneticFieldTimestamp - << "," << magData.magneticFieldX << "," << magData.magneticFieldY - << "," << magData.magneticFieldZ << "\n"; - } -}; diff --git a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp deleted file mode 100644 index be845aab5b3631552d5ba0b7f00e456d89e95185..0000000000000000000000000000000000000000 --- a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp +++ /dev/null @@ -1,107 +0,0 @@ -/* Copyright (c) 2023 Skyward Experimental Rocketry - * Author: Matteo Pignataro - * - * 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 <Payload/Sensors/RotatedIMU/RotatedIMU.h> - -using namespace Eigen; -using namespace Boardcore; - -namespace Payload -{ - -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(); -} - -IMUData RotatedIMU::sampleImpl() -{ - auto imuData = sampleImu(); - Vector3f rotatedAcc = accT * static_cast<Vector3f>(imuData.accData); - Vector3f rotatedGyro = gyroT * static_cast<Vector3f>(imuData.gyroData); - Vector3f rotatedMag = magT * static_cast<Vector3f>(imuData.magData); - - auto acc = AccelerometerData{rotatedAcc}; - acc.accelerationTimestamp = imuData.accData.accelerationTimestamp; - - auto gyro = GyroscopeData{rotatedGyro}; - gyro.angularSpeedTimestamp = imuData.gyroData.angularSpeedTimestamp; - - auto mag = MagnetometerData{rotatedMag}; - mag.magneticFieldTimestamp = imuData.magData.magneticFieldTimestamp; - - return {acc, gyro, mag}; -} - -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; -} -} // namespace Payload diff --git a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h b/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h deleted file mode 100644 index 1be627c260f489312c4a285faa95aa299b25b9da..0000000000000000000000000000000000000000 --- a/src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h +++ /dev/null @@ -1,108 +0,0 @@ -/* Copyright (c) 2024 Skyward Experimental Rocketry - * Authors: Matteo Pignataro, 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 <Payload/Sensors/RotatedIMU/IMUData.h> -#include <sensors/Sensor.h> - -#include <Eigen/Eigen> -#include <functional> - -namespace Payload -{ - -/** - * @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 - */ - RotatedIMU(SampleIMUFunction sampleFunction); - - /** - * @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); - - bool init() override { return true; } - bool selfTest() override { return true; } - -private: - IMUData sampleImpl() override; - - SampleIMUFunction sampleImu; - - // Transformation matrices - Eigen::Matrix3f accT = Eigen::Matrix3f::Identity(); - Eigen::Matrix3f gyroT = Eigen::Matrix3f::Identity(); - Eigen::Matrix3f magT = Eigen::Matrix3f::Identity(); -}; -} // namespace Payload diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp index 94da66358bfd547cc5d35d50e3de10bb90e5349d..b9ead2aa8394f57ab1fbfb79ed419cf5354092ca 100644 --- a/src/boards/Payload/Sensors/Sensors.cpp +++ b/src/boards/Payload/Sensors/Sensors.cpp @@ -695,9 +695,9 @@ void Sensors::imuCreate() auto magSample = getCalibratedMagnetometerLastSample(); return IMUData{ - .accData = lsm6dsrxSample, - .gyroData = lsm6dsrxSample, - .magData = magSample, + lsm6dsrxSample, + lsm6dsrxSample, + magSample, }; }); diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h index da4af88ba807d716c2ac1a0bfa850180ab1e436a..35d7c425e6eed63f178d649ae210bedaf8b6fd23 100644 --- a/src/boards/Payload/Sensors/Sensors.h +++ b/src/boards/Payload/Sensors/Sensors.h @@ -23,7 +23,6 @@ #pragma once #include <Payload/Configs/SensorsConfig.h> -#include <Payload/Sensors/RotatedIMU/RotatedIMU.h> #include <drivers/adc/InternalADC.h> #include <sensors/ADS131M08/ADS131M08.h> #include <sensors/H3LIS331DL/H3LIS331DL.h> @@ -31,6 +30,7 @@ #include <sensors/LPS22DF/LPS22DF.h> #include <sensors/LPS28DFW/LPS28DFW.h> #include <sensors/LSM6DSRX/LSM6DSRX.h> +#include <sensors/RotatedIMU/RotatedIMU.h> #include <sensors/SensorManager.h> #include <sensors/UBXGPS/UBXGPSSpi.h> #include <sensors/analog/BatteryVoltageSensorData.h> @@ -87,7 +87,7 @@ public: Boardcore::PressureData getDynamicPressure(); Boardcore::PitotData getPitotLastSample(); - IMUData getIMULastSample(); + Boardcore::IMUData getIMULastSample(); Boardcore::BatteryVoltageSensorData getBatteryVoltage(); Boardcore::BatteryVoltageSensorData getCamBatteryVoltage(); @@ -172,7 +172,7 @@ protected: std::unique_ptr<Boardcore::MPXH6115A> staticPressure; std::unique_ptr<Boardcore::MPXH6115A> dynamicPressure; std::unique_ptr<Boardcore::Pitot> pitot; - std::unique_ptr<Payload::RotatedIMU> imu; + std::unique_ptr<Boardcore::RotatedIMU> imu; private: std::unique_ptr<Boardcore::SensorManager> manager; diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index c82842422059c9355ff679ff762d7049e1b0a2ef..93ace5d9b381962777ad3774dd40b70b425a9d4f 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -86,19 +86,19 @@ void NASController::update() LPS28DFWData baroData = getModule<Sensors>()->getLPS28DFWLastSample(); // NAS prediction - nas.predictGyro(imuData.gyroData); - nas.predictAcc(imuData.accData); + nas.predictGyro(imuData); + nas.predictAcc(imuData); // NAS correction - nas.correctMag(imuData.magData); + nas.correctMag(imuData); nas.correctGPS(gpsData); nas.correctBaro(baroData.pressure); // Correct with accelerometer if the acceleration is in specs - Vector3f acceleration = imuData.accData; + Vector3f acceleration = static_cast<AccelerometerData>(imuData); float accelerationNorm = acceleration.norm(); if (accelerationValid) { - nas.correctAcc(imuData.accData); + nas.correctAcc(imuData); } if ((accelerationNorm < (9.8 + (NASConfig::ACCELERATION_THRESHOLD) / 2) && diff --git a/src/scripts/logdecoder/Payload/logdecoder.cpp b/src/scripts/logdecoder/Payload/logdecoder.cpp index 369262397fa3f465ead57ab36862cc0fc175d131..8be474764e422fe09b30c22a953fd988a7ba8ee4 100644 --- a/src/scripts/logdecoder/Payload/logdecoder.cpp +++ b/src/scripts/logdecoder/Payload/logdecoder.cpp @@ -21,7 +21,6 @@ */ #include <Payload/PinHandler/PinData.h> -#include <Payload/Sensors/RotatedIMU/IMUData.h> #include <Payload/Sensors/SensorData.h> #include <Payload/StateMachines/FlightModeManager/FlightModeManagerData.h> #include <Payload/StateMachines/NASController/NASControllerData.h> @@ -61,7 +60,6 @@ void registerTypes(Deserializer& ds) ds.registerType<FlightModeManagerStatus>(); ds.registerType<NASControllerStatus>(); ds.registerType<WingControllerStatus>(); - ds.registerType<IMUData>(); ds.registerType<SensorsCalibrationParameter>(); ds.registerType<PinChangeData>(); }