From 62e06aafa443ed8d4a45b6c06f13a871e3d7a5a7 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:59:43 +0200
Subject: [PATCH] [Payload][Sensors] Use RotatedIMU from Boardcore
---
cmake/dependencies.cmake | 1 -
.../FlightStatsRecorder.cpp | 2 +-
.../Payload/Sensors/RotatedIMU/IMUData.h | 50 --------
.../Payload/Sensors/RotatedIMU/RotatedIMU.cpp | 107 -----------------
.../Payload/Sensors/RotatedIMU/RotatedIMU.h | 108 ------------------
src/boards/Payload/Sensors/Sensors.cpp | 6 +-
src/boards/Payload/Sensors/Sensors.h | 6 +-
.../NASController/NASController.cpp | 10 +-
src/scripts/logdecoder/Payload/logdecoder.cpp | 2 -
9 files changed, 12 insertions(+), 280 deletions(-)
delete mode 100644 src/boards/Payload/Sensors/RotatedIMU/IMUData.h
delete mode 100644 src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.cpp
delete mode 100644 src/boards/Payload/Sensors/RotatedIMU/RotatedIMU.h
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 3cb66c207..6508dc5c7 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 694c664d5..2790a1b32 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 d2f487a1a..000000000
--- 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 be845aab5..000000000
--- 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 1be627c26..000000000
--- 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 94da66358..b9ead2aa8 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 da4af88ba..35d7c425e 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 c82842422..93ace5d9b 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 369262397..8be474764 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>();
}
--
GitLab