diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 78209a5f4446e7dc5eaef63de2a9bf1202863192..897e004670cc70a17c97858293aba85ad96709ab 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -159,6 +159,59 @@
],
"limitSymbolsToIncludedHeaders": true
}
+ },
+ {
+ "name": "stm32f429zi_parafoil",
+ "cStandard": "c11",
+ "cppStandard": "c++11",
+ "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+ "defines": [
+ "DEBUG",
+ "_ARCH_CORTEXM4_STM32F4",
+ "_BOARD_STM32F429ZI_PARAFOIL",
+ "_MIOSIX_BOARDNAME=stm32f429zi_parafoil",
+ "HSE_VALUE=8000000",
+ "SYSCLK_FREQ_168MHz=168000000",
+ "_MIOSIX",
+ "__cplusplus=201103L"
+ ],
+ "includePath": [
+ "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+ "${workspaceFolder}/libs/miosix-kernel/miosix",
+ "${workspaceFolder}/libs/mavlink_skyward_lib",
+ "${workspaceFolder}/libs/fmt/include",
+ "${workspaceFolder}/libs/eigen",
+ "${workspaceFolder}/libs/tscpp",
+ "${workspaceFolder}/libs",
+ "${workspaceFolder}/src/shared",
+ "${workspaceFolder}/src/tests"
+ ],
+ "browse": {
+ "path": [
+ "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_parafoil",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/stdlib_integration",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/arch/common",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/interfaces",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/filesystem",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/kernel",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/util",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/e20",
+ "${workspaceFolder}/libs/miosix-kernel/miosix/*",
+ "${workspaceFolder}/libs/mavlink_skyward_lib",
+ "${workspaceFolder}/libs/eigen",
+ "${workspaceFolder}/libs/tscpp",
+ "${workspaceFolder}/libs/mxgui",
+ "${workspaceFolder}/libs/fmt",
+ "${workspaceFolder}/src/shared",
+ "${workspaceFolder}/src/tests"
+ ],
+ "limitSymbolsToIncludedHeaders": true
+ }
}
],
"version": 4
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 3ff8448432b4a1cd1bc1d6a76fe234a3aa7003b2..43e514f2603ebb9bb2b90fdab6c1c80067e2f17d 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -131,6 +131,7 @@
"cpitch",
"cppcheck",
"croll",
+ "cwise",
"cyaw",
"deleteme",
"DMEIE",
@@ -176,6 +177,7 @@
"miosix",
"mkdir",
"MPXHZ",
+ "Musso",
"NATT",
"NBAR",
"NDTR",
@@ -194,6 +196,7 @@
"Qput",
"Qwait",
"Qwakeup",
+ "Riccardo",
"RXNE",
"RXNEIE",
"sats",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index df86a35b4ef43b1bf5a2a1e5e8b8f2b60e505dda..1c07b46355e37135c849ff6faa1c81f512fb09d7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,9 @@ sbs_target(bmx160-calibration-entry stm32f429zi_skyward_death_stack_x)
add_executable(config-dsgamma src/entrypoints/config-dsgamma.cpp)
sbs_target(config-dsgamma stm32f429zi_stm32f4discovery)
+add_executable(imu-calibration src/entrypoints/imu-calibration.cpp)
+sbs_target(imu-calibration stm32f429zi_parafoil)
+
add_executable(mxgui-helloworld src/entrypoints/examples/mxgui-helloworld.cpp)
sbs_target(mxgui-helloworld stm32f429zi_stm32f4discovery)
@@ -129,18 +132,30 @@ sbs_target(test-stepper stm32f429zi_stm32f4discovery)
add_executable(test-kalman-eigen-benchmark src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp)
sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery)
-add_executable(test-ekf src/tests/algorithms/ExtendedKalman/test-ekf.cpp)
-sbs_target(test-ekf stm32f429zi_skyward_death_stack_x)
+add_executable(test-tmp src/tests/algorithms/NAS/test-tmp.cpp)
+sbs_target(test-tmp stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-attitude-parafoil src/tests/algorithms/NAS/test-attitude-parafoil.cpp)
+sbs_target(test-attitude-parafoil stm32f429zi_parafoil)
+
+add_executable(test-attitude-stack src/tests/algorithms/NAS/test-attitude-stack.cpp)
+sbs_target(test-attitude-stack stm32f429zi_skyward_death_stack_x)
-add_executable(test-ekf-with-triad src/tests/algorithms/ExtendedKalman/test-ekf-with-triad.cpp)
-sbs_target(test-ekf-with-triad stm32f429zi_skyward_death_stack_x)
+add_executable(test-nas-parafoil src/tests/algorithms/NAS/test-nas-parafoil.cpp)
+sbs_target(test-nas-parafoil stm32f429zi_parafoil)
-add_executable(test-ekf-with-logs src/tests/algorithms/ExtendedKalman/test-ekf-with-logs.cpp)
-sbs_target(test-ekf-with-logs stm32f429zi_skyward_death_stack_x)
+add_executable(test-nas-stack src/tests/algorithms/NAS/test-nas-stack.cpp)
+sbs_target(test-nas-stack stm32f429zi_skyward_death_stack_x)
-add_executable(test-triad src/tests/algorithms/ExtendedKalman/test-triad.cpp)
+add_executable(test-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp)
+sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-triad src/tests/algorithms/NAS/test-triad.cpp)
sbs_target(test-triad stm32f429zi_skyward_death_stack_x)
+add_executable(test-triad-parafoil src/tests/algorithms/NAS/test-triad-parafoil.cpp)
+sbs_target(test-triad-parafoil stm32f429zi_parafoil)
+
#-----------------------------------------------------------------------------#
# Tests - Drivers #
#-----------------------------------------------------------------------------#
@@ -329,13 +344,13 @@ add_executable(test-max31855 src/tests/sensors/test-max31855.cpp)
sbs_target(test-max31855 stm32f429zi_stm32f4discovery)
add_executable(test-mpu9250 src/tests/sensors/test-mpu9250.cpp)
-sbs_target(test-mpu9250 stm32f429zi_skyward_death_stack_x)
+sbs_target(test-mpu9250 stm32f429zi_parafoil)
add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp)
sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
add_executable(test-ubloxgps-serial src/tests/sensors/test-ubloxgps.cpp)
-sbs_target(test-ubloxgps-serial stm32f407vg_stm32f4discovery)
+sbs_target(test-ubloxgps-serial stm32f429zi_skyward_death_stack_x)
add_executable(test-ubloxgps-spi src/tests/sensors/test-ubloxgps.cpp)
target_compile_definitions(test-ubloxgps-spi PRIVATE USE_SPI)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index ddf3bfd6666888e1f4273c80001e6f69ae006d47..2fdea04d4a7b2628845b1bcfc62952433eee4e2b 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -32,7 +32,7 @@ foreach(OPT_BOARD ${BOARDS})
# Algorithms
- ${SBS_BASE}/src/shared/algorithms/ExtendedKalman/ExtendedKalman.cpp
+ ${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp
# Debug
${SBS_BASE}/src/shared/utils/Debug.cpp
@@ -86,6 +86,9 @@ foreach(OPT_BOARD ${BOARDS})
${SBS_BASE}/src/shared/sensors/UbloxGPS/UbloxGPS.cpp
${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp
+ # Calibration
+ ${SBS_BASE}/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
+
# Utils
${SBS_BASE}/src/shared/utils/AeroUtils/AeroUtils.cpp
${SBS_BASE}/src/shared/utils/ButtonHandler/ButtonHandler.cpp
diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index 0cb79ca6f0a9ce835c6a5d3950ed91fc91ff1ece..441aca88b44e5438e04abbef1639cd7f9df7bb37 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit 0cb79ca6f0a9ce835c6a5d3950ed91fc91ff1ece
+Subproject commit 441aca88b44e5438e04abbef1639cd7f9df7bb37
diff --git a/libs/mxgui b/libs/mxgui
index a3f44e0cc0c7965f947b18621470d22fcabddc7b..0052374bc85c2bf90753faba61ecc08905658c83 160000
--- a/libs/mxgui
+++ b/libs/mxgui
@@ -1 +1 @@
-Subproject commit a3f44e0cc0c7965f947b18621470d22fcabddc7b
+Subproject commit 0052374bc85c2bf90753faba61ecc08905658c83
diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp
index 0124a2310951b7803879e9fce2c79466ba04880f..d2aa4746b97f41ee1b47c5306c6ea884f1f859ef 100644
--- a/src/entrypoints/bmx160-calibration-entry.cpp
+++ b/src/entrypoints/bmx160-calibration-entry.cpp
@@ -24,7 +24,8 @@
#include <drivers/timer/TimestampTimer.h>
#include <sensors/BMX160/BMX160.h>
#include <sensors/BMX160/BMX160WithCorrection.h>
-#include <sensors/calibration/SoftIronCalibration.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
#include <utils/Debug.h>
#include <fstream>
@@ -41,9 +42,9 @@ constexpr int ACC_CALIBRATION_SLEEP_TIME = 25; // [us]
constexpr int ACC_CALIBRATION_N_ORIENTATIONS = 6;
constexpr int MAG_CALIBRATION_SAMPLES = 500;
-constexpr int MAG_CALIBRATION_DURAITON = 60; // [s]
+constexpr int MAG_CALIBRATION_DURATION = 60; // [s]
constexpr int MAG_CALIBRATION_SLEEP_TIME =
- MAG_CALIBRATION_DURAITON * 1000 / MAG_CALIBRATION_SAMPLES; // [us]
+ MAG_CALIBRATION_DURATION * 1000 / MAG_CALIBRATION_SAMPLES; // [us]
BMX160* bmx160;
@@ -330,7 +331,7 @@ BMX160CorrectionParameters calibrateMagnetometer(
BMX160CorrectionParameters correctionParameters)
{
Eigen::Matrix<float, 3, 2> m;
- auto* calibrationModel = new SoftIronCalibration<MAG_CALIBRATION_SAMPLES>;
+ auto* calibrationModel = new SoftAndHardIronCalibration;
Eigen::Vector3f avgMag{0, 0, 0}, vec;
SPIBus bus(SPI1);
@@ -389,7 +390,7 @@ BMX160CorrectionParameters calibrateMagnetometer(
"the "
"most different directions.\n");
printf("The calibration will run for %d seconds\n",
- MAG_CALIBRATION_DURAITON);
+ MAG_CALIBRATION_DURATION);
if (!askToContinue())
{
@@ -412,26 +413,6 @@ BMX160CorrectionParameters calibrateMagnetometer(
printf("Computing the result....");
auto corrector = calibrationModel->computeResult();
- // Save the calibration data in the sd card
- // Save the correction parameters in the file
- {
- std::ofstream magnetometerCalibrationDataFile(
- MAG_CALIBRATION_DATA_FILE);
-
- auto samples = calibrationModel->getSamples();
- int numberOfSamples = samples.rows();
-
- for (int i = 0; i < numberOfSamples; i++)
- {
- auto row = samples.row(i);
- for (int j = 0; j < row.cols(); j++)
- {
- magnetometerCalibrationDataFile << samples.row(i)(j) << ",";
- }
- magnetometerCalibrationDataFile << "\n";
- }
- }
-
corrector >> m;
corrector >> correctionParameters.magnetoParams;
diff --git a/src/entrypoints/imu-calibration.cpp b/src/entrypoints/imu-calibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fbb00378cb7c5c7e0d21d41616847012ce166ad4
--- /dev/null
+++ b/src/entrypoints/imu-calibration.cpp
@@ -0,0 +1,131 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Alberto Nidasio
+ *
+ * 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 <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+#include <utils/Debug.h>
+
+#include <iostream>
+
+using namespace Boardcore;
+using namespace Eigen;
+using namespace miosix;
+
+int menu();
+bool askToContinue();
+
+void calibrateMagnetometer();
+
+constexpr int MAG_CALIBRATION_DURATION = 30; // [s]
+constexpr int IMU_SAMPLE_PERIOD = 10; // [ms] 100Hz
+
+int main()
+{
+ // Greet the user
+ printf("\nWelcome to the calibration procedure!\n");
+
+ // Make the user choose what to do
+ switch (menu())
+ {
+ case 2:
+ calibrateMagnetometer();
+ break;
+
+ default:
+ break;
+ }
+
+ printf("end\n");
+ reboot();
+}
+
+int menu()
+{
+ int choice;
+
+ printf("\nWhat do you want to do?\n");
+ printf("1. Calibrate accelerometer\n");
+ printf("2. Calibrate magnetometer\n");
+ printf("3. Set minimum gyroscope samples for calibration\n");
+ printf("\n>> ");
+ scanf("%d", &choice);
+
+ return choice;
+}
+
+bool askToContinue()
+{
+ char c;
+
+ std::cout << "Write 'c' to continue, otherwise stop:\n";
+ scanf("%c", &c);
+
+ return c != 'c';
+}
+
+void calibrateMagnetometer()
+{
+ SPIBus spiBus(SPI1);
+ MPU9250 mpu(spiBus, sensors::mpu9250::cs::getPin());
+ mpu.init();
+
+ printf("Now the magnetometer calibration will begin\n");
+ printf("Please, rotate the gyroscope A LOT!\n");
+ printf("The calibration will run for %d seconds\n",
+ MAG_CALIBRATION_DURATION);
+
+ // if (!askToContinue())
+ // return;
+ printf("Starting...\n");
+
+ // Prepare the calibration model
+ SoftAndHardIronCalibration calibration;
+
+ // Prepare and start the sensor manager
+ TaskScheduler scheduler;
+ scheduler.addTask([]() { printf("...\n"); }, 500);
+ scheduler.start();
+
+ // Wait and then stop the sampling
+ auto startTick = getTick();
+ auto lastTick = startTick;
+ while (getTick() - startTick < MAG_CALIBRATION_DURATION * 1e3)
+ {
+ mpu.sample();
+ calibration.feed(mpu.getLastSample());
+ Thread::sleepUntil(lastTick + IMU_SAMPLE_PERIOD);
+ lastTick = getTick();
+ }
+
+ scheduler.stop();
+
+ printf("Computing the result...\n");
+
+ auto correction = calibration.computeResult();
+
+ printf("b: the bias vector\n");
+ std::cout << correction.getOffset().transpose() << std::endl;
+ printf("g: the gain to be multiplied to the input vector\n");
+ std::cout << correction.getGain().transpose() << std::endl;
+}
diff --git a/src/shared/algorithms/ExtendedKalman/ExtendedKalman.cpp b/src/shared/algorithms/NAS/NAS.cpp
similarity index 89%
rename from src/shared/algorithms/ExtendedKalman/ExtendedKalman.cpp
rename to src/shared/algorithms/NAS/NAS.cpp
index 2d1a04976a0441066a5e26f7dfa0cfd8fd34f214..1296b639b48483d3a0309d5136e77c5a97c5fc7e 100644
--- a/src/shared/algorithms/ExtendedKalman/ExtendedKalman.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -20,7 +20,7 @@
* THE SOFTWARE.
*/
-#include "ExtendedKalman.h"
+#include "NAS.h"
#include <drivers/timer/TimestampTimer.h>
#include <utils/SkyQuaternion/SkyQuaternion.h>
@@ -33,7 +33,7 @@ using namespace Eigen;
namespace Boardcore
{
-ExtendedKalman::ExtendedKalman(ExtendedKalmanConfig config) : config(config)
+NAS::NAS(NASConfig config) : config(config)
{
// Covariance setup
{
@@ -59,9 +59,11 @@ ExtendedKalman::ExtendedKalman(ExtendedKalmanConfig config) : config(config)
// GPS matrixes
{
- H_gps = Matrix<float, 4, 6>::Identity();
- H_gps.coeffRef(2, 2) = 0;
- H_gps.coeffRef(5, 5) = 0;
+ H_gps = Matrix<float, 4, 6>::Zero();
+ H_gps.coeffRef(0, 0) = 1;
+ H_gps.coeffRef(1, 1) = 1;
+ H_gps.coeffRef(2, 3) = 1;
+ H_gps.coeffRef(3, 4) = 1;
H_gps_tr = H_gps.transpose();
R_gps << config.SIGMA_GPS * Matrix<float, 4, 4>::Identity();
}
@@ -96,7 +98,7 @@ ExtendedKalman::ExtendedKalman(ExtendedKalmanConfig config) : config(config)
}
}
-void ExtendedKalman::predictAcc(const Vector3f& acceleration)
+void NAS::predictAcc(const Vector3f& acceleration)
{
Matrix3f A = body2ned(x.block<4, 1>(IDX_QUAT, 0));
Vector3f pos = x.block<3, 1>(IDX_POS, 0);
@@ -120,7 +122,7 @@ void ExtendedKalman::predictAcc(const Vector3f& acceleration)
P.block<6, 6>(0, 0) = F_lin * Pl * F_lin_tr + Q_lin;
}
-void ExtendedKalman::predictGyro(const Vector3f& angularVelocity)
+void NAS::predictGyro(const Vector3f& angularVelocity)
{
Vector3f bias = x.block<3, 1>(IDX_BIAS, 0);
Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
@@ -149,8 +151,8 @@ void ExtendedKalman::predictGyro(const Vector3f& angularVelocity)
P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
}
-void ExtendedKalman::correctBaro(const float pressure, const float mslPress,
- const float mslTemp)
+void NAS::correctBaro(const float pressure, const float mslPress,
+ const float mslTemp)
{
Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero();
@@ -174,7 +176,7 @@ void ExtendedKalman::correctBaro(const float pressure, const float mslPress,
x.head<6>() = x.head<6>() + K * (pressure - y_hat);
}
-void ExtendedKalman::correctGPS(const Vector4f& gps)
+void NAS::correctGPS(const Vector4f& gps)
{
Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
@@ -191,7 +193,7 @@ void ExtendedKalman::correctGPS(const Vector4f& gps)
x.head<6>() = x.head<6>() + K * (gps - H);
}
-void ExtendedKalman::correctMag(const Vector3f& mag)
+void NAS::correctMag(const Vector3f& mag)
{
Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
Matrix3f A = body2ned(q).transpose();
@@ -224,7 +226,7 @@ void ExtendedKalman::correctMag(const Vector3f& mag)
P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
}
-void ExtendedKalman::correctAcc(const Eigen::Vector3f& acceleration)
+void NAS::correctAcc(const Eigen::Vector3f& acc)
{
Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
Matrix3f A = body2ned(q).transpose();
@@ -249,10 +251,8 @@ void ExtendedKalman::correctAcc(const Eigen::Vector3f& acceleration)
Matrix<float, 6, 3> K = Pq * H.transpose() * S.inverse();
- Eigen::Vector3f a = acceleration;
- a.normalize();
aEst.normalize();
- Vector3f e = a - aEst;
+ Vector3f e = acc - aEst;
Matrix<float, 6, 1> dx = K * e;
Vector4f r{0.5f * dx(0), 0.5f * dx(1), 0.5f * dx(2),
sqrtf(1.0f - 0.25f * dx.head<3>().squaredNorm())};
@@ -264,7 +264,7 @@ void ExtendedKalman::correctAcc(const Eigen::Vector3f& acceleration)
P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
}
-void ExtendedKalman::correctPitot(const float deltaP, const float staticP)
+void NAS::correctPitot(const float deltaP, const float staticP)
{
if (deltaP >= 0)
{
@@ -301,21 +301,18 @@ void ExtendedKalman::correctPitot(const float deltaP, const float staticP)
}
}
-ExtendedKalmanState ExtendedKalman::getState() const
+NASState NAS::getState() const
{
- return ExtendedKalmanState(TimestampTimer::getInstance().getTimestamp(), x);
+ return NASState(TimestampTimer::getInstance().getTimestamp(), x);
}
-Eigen::Matrix<float, 13, 1> ExtendedKalman::getX() const { return x; }
+Eigen::Matrix<float, 13, 1> NAS::getX() const { return x; }
-void ExtendedKalman::setState(const ExtendedKalmanState& state)
-{
- this->x = state.getX();
-}
+void NAS::setState(const NASState& state) { this->x = state.getX(); }
-void ExtendedKalman::setX(const Eigen::Matrix<float, 13, 1>& x) { this->x = x; }
+void NAS::setX(const Eigen::Matrix<float, 13, 1>& x) { this->x = x; }
-Matrix3f ExtendedKalman::body2ned(const Vector4f& q)
+Matrix3f NAS::body2ned(const Vector4f& q)
{
// clang-format off
return Matrix3f{
diff --git a/src/shared/algorithms/ExtendedKalman/ExtendedKalman.h b/src/shared/algorithms/NAS/NAS.h
similarity index 93%
rename from src/shared/algorithms/ExtendedKalman/ExtendedKalman.h
rename to src/shared/algorithms/NAS/NAS.h
index 5e9ec6a0f9c687fc7d5f237bdf99cfcfb5556721..d232f4beb6f6951c1d8ecded27b3aea6c2c9a77f 100644
--- a/src/shared/algorithms/ExtendedKalman/ExtendedKalman.h
+++ b/src/shared/algorithms/NAS/NAS.h
@@ -27,13 +27,13 @@
#include <Eigen/Dense>
-#include "ExtendedKalmanConfig.h"
-#include "ExtendedKalmanState.h"
+#include "NASConfig.h"
+#include "NASState.h"
namespace Boardcore
{
-class ExtendedKalman
+class NAS
{
public:
///< Index of position elements in the state.
@@ -48,7 +48,7 @@ public:
///< Index of bias elements in the state.
static constexpr uint16_t IDX_BIAS = 10;
- explicit ExtendedKalman(ExtendedKalmanConfig config);
+ explicit NAS(NASConfig config);
/**
* @brief Prediction with accelerometer data.
@@ -92,7 +92,7 @@ public:
/**
* @brief Correction with accelerometer data.
*
- * @param u Vector with acceleration data [x y z][m/s^2]
+ * @param u Normaliezed vector with acceleration data [x y z]
*/
void correctAcc(const Eigen::Vector3f& acceleration);
@@ -109,7 +109,7 @@ public:
/**
* @return EKF state.
*/
- ExtendedKalmanState getState() const;
+ NASState getState() const;
/**
* @return State vector [n e d vn ve vd qx qy qz qw bx by bz].
@@ -119,7 +119,7 @@ public:
/**
* @param state EKF state.
*/
- void setState(const ExtendedKalmanState& state);
+ void setState(const NASState& state);
/**
* @param state State vector [n e d vn ve vd qx qy qz qw bx by bz].
@@ -135,7 +135,7 @@ private:
Eigen::Matrix3f body2ned(const Eigen::Vector4f& q);
///< Extended Kalman filter configuration parameters
- ExtendedKalmanConfig config;
+ NASConfig config;
///< State vector [n e d vn ve vd qx qy qz qw bx by bz]
Eigen::Matrix<float, 13, 1> x;
diff --git a/src/shared/algorithms/ExtendedKalman/ExtendedKalmanConfig.h b/src/shared/algorithms/NAS/NASConfig.h
similarity index 98%
rename from src/shared/algorithms/ExtendedKalman/ExtendedKalmanConfig.h
rename to src/shared/algorithms/NAS/NASConfig.h
index f852d2fe0b158e8cdbdd5db8c7ffbc4d01ac0b82..b73b2f8f8b3bdf6a5eb1d48194d81f9a8ad0ad75 100644
--- a/src/shared/algorithms/ExtendedKalman/ExtendedKalmanConfig.h
+++ b/src/shared/algorithms/NAS/NASConfig.h
@@ -27,7 +27,7 @@
namespace Boardcore
{
-struct ExtendedKalmanConfig
+struct NASConfig
{
float T; ///< [s] Sample period
float SIGMA_BETA; ///< [rad/s^2] Estimated gyroscope bias variance
diff --git a/src/shared/algorithms/ExtendedKalman/ExtendedKalmanState.h b/src/shared/algorithms/NAS/NASState.h
similarity index 94%
rename from src/shared/algorithms/ExtendedKalman/ExtendedKalmanState.h
rename to src/shared/algorithms/NAS/NASState.h
index 6e8d59b6b7f592d9a707d02537ea0959fa5f0668..6c5a39db9c12ba68a643b5755a1bd26085ebd205 100644
--- a/src/shared/algorithms/ExtendedKalman/ExtendedKalmanState.h
+++ b/src/shared/algorithms/NAS/NASState.h
@@ -27,7 +27,7 @@
namespace Boardcore
{
-struct ExtendedKalmanState
+struct NASState
{
uint64_t timestamp;
@@ -54,9 +54,9 @@ struct ExtendedKalmanState
float by = 0;
float bz = 0;
- ExtendedKalmanState() {}
+ NASState() {}
- ExtendedKalmanState(uint64_t timestamp, Eigen::Matrix<float, 13, 1> x)
+ NASState(uint64_t timestamp, Eigen::Matrix<float, 13, 1> x)
: timestamp(timestamp), n(x(0)), e(x(1)), d(x(2)), vn(x(3)), ve(x(4)),
vd(x(5)), qx(x(6)), qy(x(7)), qz(x(8)), qw(x(9)), bx(x(10)),
by(x(11)), bz(x(12))
diff --git a/src/shared/algorithms/ExtendedKalman/StateInitializer.h b/src/shared/algorithms/NAS/StateInitializer.h
similarity index 94%
rename from src/shared/algorithms/ExtendedKalman/StateInitializer.h
rename to src/shared/algorithms/NAS/StateInitializer.h
index 7765cd7b2be3affdfcbe90fe7f8712d9bddcf4ad..959597beb70db8e4b2133eacf12df22d90f10add 100644
--- a/src/shared/algorithms/ExtendedKalman/StateInitializer.h
+++ b/src/shared/algorithms/NAS/StateInitializer.h
@@ -28,7 +28,7 @@
#include <Eigen/Dense>
-#include "ExtendedKalman.h"
+#include "NAS.h"
namespace Boardcore
{
@@ -117,10 +117,10 @@ void StateInitializer::eCompass(const Eigen::Vector3f acc,
Eigen::Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R);
- x_init(ExtendedKalman::IDX_QUAT) = x_quat(0);
- x_init(ExtendedKalman::IDX_QUAT + 1) = x_quat(1);
- x_init(ExtendedKalman::IDX_QUAT + 2) = x_quat(2);
- x_init(ExtendedKalman::IDX_QUAT + 3) = x_quat(3);
+ x_init(NAS::IDX_QUAT) = x_quat(0);
+ x_init(NAS::IDX_QUAT + 1) = x_quat(1);
+ x_init(NAS::IDX_QUAT + 2) = x_quat(2);
+ x_init(NAS::IDX_QUAT + 3) = x_quat(3);
}
void StateInitializer::triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag,
@@ -151,7 +151,7 @@ void StateInitializer::triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag,
Eigen::Vector4f q = SkyQuaternion::rotationMatrix2quat(A);
// Save the orientation in the state
- x_init.block<4, 1>(ExtendedKalman::IDX_QUAT, 0) = q;
+ x_init.block<4, 1>(NAS::IDX_QUAT, 0) = q;
}
void StateInitializer::positionInit(const float pressure,
diff --git a/src/shared/logger/LogTypes.h b/src/shared/logger/LogTypes.h
index 469878485393277f447fce1e2f3ecfe8e03ea113..bb2a1c06dfee3da621fe2676f56cdce841af3904 100644
--- a/src/shared/logger/LogTypes.h
+++ b/src/shared/logger/LogTypes.h
@@ -22,7 +22,7 @@
#pragma once
-#include <algorithms/ExtendedKalman/ExtendedKalmanState.h>
+#include <algorithms/NAS/NASState.h>
#include <diagnostic/PrintLoggerData.h>
#include <drivers/adc/InternalADCData.h>
#include <events/EventData.h>
@@ -85,7 +85,7 @@ void registerType(Deserializer& ds)
void registerTypes(Deserializer& ds)
{
- registerType<ExtendedKalmanState>(ds);
+ registerType<NASState>(ds);
registerType<LoggingString>(ds);
registerType<InternalADCData>(ds);
registerType<EventData>(ds);
diff --git a/src/shared/sensors/BMX160/BMX160WithCorrection.h b/src/shared/sensors/BMX160/BMX160WithCorrection.h
index 627685ec1fec4a5c62dc6907cfe3a2178d40bd0f..fa139ef6bca95e2b6296a97be26185c050fedfd4 100644
--- a/src/shared/sensors/BMX160/BMX160WithCorrection.h
+++ b/src/shared/sensors/BMX160/BMX160WithCorrection.h
@@ -24,6 +24,7 @@
#include <diagnostic/PrintLogger.h>
#include <sensors/BMX160/BMX160.h>
+#include <sensors/calibration/AxisOrientation.h>
#include <sensors/calibration/BiasCalibration.h>
#include <sensors/calibration/SixParameterCalibration.h>
diff --git a/src/shared/sensors/MPU9250/MPU9250.cpp b/src/shared/sensors/MPU9250/MPU9250.cpp
index d852cc0f6df1f52404d6925ac5b54fa1a47b57cc..0348e7afe5ae76c272fc2cbea109439f795dbcea 100644
--- a/src/shared/sensors/MPU9250/MPU9250.cpp
+++ b/src/shared/sensors/MPU9250/MPU9250.cpp
@@ -29,11 +29,12 @@
namespace Boardcore
{
-MPU9250::MPU9250(SPISlave spiSlave_, unsigned short samplingRate_,
- MPU9250GyroFSR gyroFsr_, MPU9250AccelFSR accelFsr_,
- SPI::ClockDivider highSpeedSpiClockDivider_)
- : spiSlave(spiSlave_), samplingRate(samplingRate_), gyroFsr(gyroFsr_),
- accelFsr(accelFsr_), highSpeedSpiClockDivider(highSpeedSpiClockDivider_)
+MPU9250::MPU9250(SPIBusInterface& bus, miosix::GpioPin cs, SPIBusConfig config,
+ unsigned short samplingRate, MPU9250GyroFSR gyroFsr,
+ MPU9250AccelFSR accelFsr,
+ SPI::ClockDivider highSpeedSpiClockDivider)
+ : spiSlave(bus, cs, config), samplingRate(samplingRate), gyroFsr(gyroFsr),
+ accelFsr(accelFsr), highSpeedSpiClockDivider(highSpeedSpiClockDivider)
{
}
diff --git a/src/shared/sensors/MPU9250/MPU9250.h b/src/shared/sensors/MPU9250/MPU9250.h
index e6cf27e7f66ab86346e95c59c6605f841098270c..039eda52f5177318bdea77a24679c5e31079e1cb 100644
--- a/src/shared/sensors/MPU9250/MPU9250.h
+++ b/src/shared/sensors/MPU9250/MPU9250.h
@@ -189,14 +189,16 @@ public:
/**
* @brief Instantiates the driver
*
- * @param highSpeedSpiClockDivider_ Clocl diver for 20MHz SPI communication
+ * @param highSpeedSpiClockDivider Clocl diver for 20MHz SPI communication
* with the device
*/
explicit MPU9250(
- SPISlave spiSlave_, unsigned short samplingRate_ = 100,
- MPU9250GyroFSR gyroFsr_ = GYRO_FSR_250DPS,
- MPU9250AccelFSR accelFsr_ = ACCEL_FSR_2G,
- SPI::ClockDivider highSpeedSpiClockDivider_ = SPI::ClockDivider::DIV_4);
+ SPIBusInterface& bus, miosix::GpioPin cs,
+ SPIBusConfig config = getDefaultSPIConfig(),
+ unsigned short samplingRate = 100,
+ MPU9250GyroFSR gyroFsr = GYRO_FSR_250DPS,
+ MPU9250AccelFSR accelFsr = ACCEL_FSR_2G,
+ SPI::ClockDivider highSpeedSpiClockDivider = SPI::ClockDivider::DIV_4);
/**
* @brief Constructs the default config for SPI Bus.
diff --git a/src/shared/sensors/MS5803/MS5803.cpp b/src/shared/sensors/MS5803/MS5803.cpp
index 1a2735f642e15deb572aa2e0c8bcd7be29d67636..52eec1bb6c3f0c15e833b75eaea66205c26b56e2 100644
--- a/src/shared/sensors/MS5803/MS5803.cpp
+++ b/src/shared/sensors/MS5803/MS5803.cpp
@@ -28,15 +28,9 @@
namespace Boardcore
{
-MS5803::MS5803(SPISlave spiSlave_, uint16_t temperatureDivider_)
- : spiSlave(spiSlave_), temperatureDivider(temperatureDivider_)
-{
-}
-
-MS5803::MS5803(SPIBusInterface& spiBus_, miosix::GpioPin cs_,
- SPIBusConfig spiConfig_, uint16_t temperatureDivider_)
- : spiSlave(spiBus_, cs_, spiConfig_),
- temperatureDivider(temperatureDivider_)
+MS5803::MS5803(SPIBusInterface& spiBus, miosix::GpioPin cs,
+ SPIBusConfig spiConfig, uint16_t temperatureDivider)
+ : spiSlave(spiBus, cs, spiConfig), temperatureDivider(temperatureDivider)
{
}
diff --git a/src/shared/sensors/MS5803/MS5803.h b/src/shared/sensors/MS5803/MS5803.h
index 427cb78ca57671c6afda72e3c4e4c0941f106132..6065ebcbab862231cf4fcd37e12d857cb4119198 100644
--- a/src/shared/sensors/MS5803/MS5803.h
+++ b/src/shared/sensors/MS5803/MS5803.h
@@ -73,9 +73,8 @@ public:
static constexpr uint8_t TIMEOUT = 5;
- explicit MS5803(SPISlave spiSlave_, uint16_t temperatureDivider_ = 1);
- MS5803(SPIBusInterface& spiBus_, miosix::GpioPin cs_,
- SPIBusConfig spiConfig_, uint16_t temperatureDivider_ = 1);
+ MS5803(SPIBusInterface& spiBus, miosix::GpioPin cs,
+ SPIBusConfig spiConfig = {}, uint16_t temperatureDivider = 1);
bool init() override;
diff --git a/src/shared/sensors/MS5803/MS5803Data.h b/src/shared/sensors/MS5803/MS5803Data.h
index 596dcec13c84878696b79119a5be30b1be0f5a3a..10eb959bd39c60b15e4c1abac74986f517d7ce3d 100644
--- a/src/shared/sensors/MS5803/MS5803Data.h
+++ b/src/shared/sensors/MS5803/MS5803Data.h
@@ -55,7 +55,7 @@ struct MS5803Data : public PressureData, TemperatureData
static std::string header()
{
- return "pressureTimestamp,press,temperatureTimestamp,temp\n";
+ return "pressureTimestamp,pressure,temperatureTimestamp,temperature\n";
}
void print(std::ostream& os) const
diff --git a/src/shared/sensors/calibration/AxisOrientation.h b/src/shared/sensors/calibration/AxisOrientation.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b6924e7f9ebb9878995dcec88e2cc65345b8fd4
--- /dev/null
+++ b/src/shared/sensors/calibration/AxisOrientation.h
@@ -0,0 +1,233 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, Alberto Nidasio
+ *
+ * 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/Core>
+#include <Eigen/Geometry>
+
+namespace Boardcore
+{
+
+/**
+ * This enum act like versors towards the chosen axis.
+ *
+ * X, Y and Z always set according to the right hand rule, so that:
+ * X is the index
+ * Y is the second finger
+ * Z is the thumb
+ *
+ */
+enum class Direction : uint8_t
+{
+ POSITIVE_X = 0,
+ NEGATIVE_X,
+ POSITIVE_Y,
+ NEGATIVE_Y,
+ POSITIVE_Z,
+ NEGATIVE_Z,
+};
+
+constexpr const char* humanFriendlyDirection[]{
+ "North", "South", "East", "West", "Down", "Up",
+};
+
+inline Eigen::Vector3f orientationToVector(Direction val)
+{
+ switch (val)
+ {
+ case Direction::POSITIVE_X:
+ return {1, 0, 0};
+ case Direction::NEGATIVE_X:
+ return {-1, 0, 0};
+ case Direction::POSITIVE_Y:
+ return {0, 1, 0};
+ case Direction::NEGATIVE_Y:
+ return {0, -1, 0};
+ case Direction::POSITIVE_Z:
+ return {0, 0, 1};
+ case Direction::NEGATIVE_Z:
+ return {0, 0, -1};
+ default:
+ /* never happens, added just to shut up the warnings */
+ return {0, 0, 0};
+ }
+}
+
+/**
+ * This struct represents in the most general way any kind of transformation of
+ * the reference frame (axis X, Y and Z).
+ * This data type is intended to simplify the code, so you shouldn't instantiate
+ * this struct directly, but rather use the structures AxisAngleOrientation or
+ * AxisOrthoOrientation that will be automatically corrected to this one, thanks
+ * to the implicit cast in favor of AxisOrientation.
+ *
+ * For example:
+ *
+ * AxisAngleOrientation angles ( PI/2, PI, 0);
+ * AxisOrthoOrientation ortho ( Direction::NEGATIVE_X,
+ * Direction::POSITIVE_Z );
+ *
+ * // The implicit cast is supported and recommended
+ * AxisOrientation converted1 = angles;
+ * AxisOrientation converted2 = ortho;
+ *
+ * // Now we can use the generated matrix:
+ * Eigen::Vector3f zeta = convertedX.getMatrix() * Eigen::Vector3f { 0, 0, 1 }
+ *
+ */
+struct AxisOrientation
+{
+ Eigen::Matrix3f mat;
+
+ AxisOrientation() : mat(Eigen::Matrix3f::Identity()) {}
+
+ AxisOrientation(Eigen::Matrix3f _mat) : mat(_mat) {}
+
+ void setMatrix(Eigen::Matrix3f _mat) { mat = _mat; }
+
+ Eigen::Matrix3f getMatrix() const { return mat; }
+};
+
+/**
+ * This struct uses the three angles yaw, pitch and roll to define the
+ * transformation of the reference frame, so according to N.E.D standard we get:
+ *
+ * X (north)
+ * /
+ * /
+ * .----> Y (east)
+ * |
+ * |
+ * v
+ * Z (down)
+ *
+ * Where:
+ * Yaw is rotation of Z axis
+ * Pitch is rotation of Y axis
+ * Roll is rotation of X axis
+ */
+struct AxisAngleOrientation
+{
+ float yaw, pitch, roll;
+
+ AxisAngleOrientation() : yaw(0), pitch(0), roll(0) {}
+
+ AxisAngleOrientation(float _yaw, float _pitch, float _roll)
+ : yaw(_yaw), pitch(_pitch), roll(_roll)
+ {
+ }
+
+ operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
+
+ Eigen::Matrix3f getMatrix() const
+ {
+ return (Eigen::AngleAxisf(yaw, Eigen::Vector3f{0, 0, 1}) *
+ Eigen::AngleAxisf(pitch, Eigen::Vector3f{0, 1, 0}) *
+ Eigen::AngleAxisf(roll, Eigen::Vector3f{1, 0, 0}))
+ .toRotationMatrix();
+ }
+};
+
+/**
+ * This struct represents the orientation of the reference frame relative
+ * to X, Y, Z in the start orientation.
+ * If we know the orientation of the X and Y axis, using the right hand rule
+ * we can infer the Z axis.
+ *
+ * For example, if the base reference is:
+ *
+ * z
+ * ^
+ * |
+ * |
+ * /----> y
+ * /
+ * x
+ *
+ * Then if we set x = NEGATIVE_Y, y = POSITIVE_Z, we get:
+ *
+ * y z
+ * ^ ^
+ * | /
+ * | /
+ * x <----/
+ *
+ */
+struct AxisOrthoOrientation
+{
+ Direction xAxis, yAxis;
+
+ AxisOrthoOrientation()
+ : xAxis(Direction::POSITIVE_X), yAxis(Direction::POSITIVE_Y)
+ {
+ }
+
+ AxisOrthoOrientation(Direction _xAxis, Direction _yAxis)
+ : xAxis(_xAxis), yAxis(_yAxis)
+ {
+ }
+
+ operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
+
+ Eigen::Matrix3f getMatrix() const
+ {
+ Eigen::Vector3f vx, vy, vz;
+
+ vx = orientationToVector(xAxis);
+ vy = orientationToVector(yAxis);
+ vz = vx.cross(vy);
+
+ Eigen::Matrix3f mat;
+ mat.col(0) << vx;
+ mat.col(1) << vy;
+ mat.col(2) << vz;
+ return mat;
+ }
+};
+
+/**
+ * This struct represents axis orientation relative to a reference system.
+ * Operatively it simply combines two transformations. It is particularly useful
+ * to obtain an AxisOrientation from a reference system generally not N.E.D.
+ */
+struct AxisRelativeOrientation
+{
+ AxisOrientation systemOrientation, orientation;
+
+ AxisRelativeOrientation(const AxisOrientation& _systemOrientation,
+ const AxisOrientation& _orientation)
+ : systemOrientation(_systemOrientation), orientation(_orientation)
+ {
+ }
+
+ operator AxisOrientation() const { return {getMatrix()}; }
+
+ Eigen::Matrix3f getMatrix() const
+ {
+ return systemOrientation.getMatrix() * orientation.getMatrix();
+ }
+};
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/BiasCalibration.h b/src/shared/sensors/calibration/BiasCalibration.h
index 5fc2ed071900c11446e6e5d25c27f34ba050e2b2..be84f860e0118b37dff9bc6e4b6c600ff74aa473 100644
--- a/src/shared/sensors/calibration/BiasCalibration.h
+++ b/src/shared/sensors/calibration/BiasCalibration.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2020 Skyward Experimental Rocketry
- * Author: Riccardo Musso
+/* Copyright (c) 2020-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -23,93 +23,163 @@
#pragma once
#include <sensors/SensorData.h>
+#include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/Calibration.h>
+#include <sensors/calibration/SensorDataExtra.h>
#include <Eigen/Core>
-#include "Calibration.h"
-#include "SensorDataExtra.h"
-
namespace Boardcore
{
/**
- * This is the dumbest type of calibration possible: it stores a 3d vector
- * (called "bias") that will be added to every measurement.
- * During the calibration phase it will use a given reference vector (for
- * example the gravitational acceleration for the accelerometer), and every time
- * you'll feed the model with a new value, you have to give it the orientation
- * of the sensor, so it can guess the bias.
+ * @brief Applies an offset to the data
+ *
+ * It stores a vector that will be added to every measurement.
+ * Note: The type T must implement >> and << operators with a Vector3f.
+ *
+ * @tparam T Type used for data items.
*/
template <typename T>
class BiasCorrector : public ValuesCorrector<T>
{
-
public:
- BiasCorrector() : bias(0, 0, 0) {}
- BiasCorrector(const Eigen::Vector3f& _bias) : bias(_bias) {}
-
- void operator>>(Eigen::Vector3f& rhs) { rhs = bias; }
+ BiasCorrector();
- void operator<<(const Eigen::Vector3f& rhs) { bias = rhs; }
+ BiasCorrector(const Eigen::Vector3f& bias);
- void setIdentity() override { bias = {0, 0, 0}; }
+ void operator>>(Eigen::Vector3f& rhs);
- T correct(const T& data) const override
- {
- Eigen::Vector3f tmp;
- T out;
+ void operator<<(const Eigen::Vector3f& rhs);
- data >> tmp;
- tmp += bias;
- out << tmp;
+ void setIdentity() override;
- return out;
- }
+ T correct(const T& data) const override;
private:
- Eigen::Vector3f bias;
+ Eigen::Vector3f bias = Eigen::Vector3f::Zero();
};
+/**
+ * @brief This is the dumbest type of calibration possible: an offset.
+ *
+ * During the calibration phase it will use a given reference vector (for
+ * example the gravitational acceleration), and every time you'll feed the model
+ * with a new value, you have to give it the orientation of the sensor, so it
+ * can guess the bias.
+ *
+ * @tparam T
+ */
template <typename T>
class BiasCalibration
- : public AbstractCalibrationModel<T, BiasCorrector<T>, AxisOrientation>
+ : public AbstractCalibration<T, BiasCorrector<T>, AxisOrientation>
{
public:
- BiasCalibration() : sum(0, 0, 0), ref(0, 0, 0), numSamples(0) {}
-
- void setReferenceVector(Eigen::Vector3f vec) { ref = vec; }
- Eigen::Vector3f getReferenceVector() { return ref; }
-
- /**
- * BiasCalibration accepts an indefinite number of samples,
- * so feed(...) never returns false.
- */
- bool feed(const T& measured, const AxisOrientation& transform) override
- {
- Eigen::Vector3f vec;
- measured >> vec;
-
- sum += (transform.getMatrix().transpose() * ref) - vec;
- numSamples++;
-
- return true;
- }
-
- bool feed(const T& measured)
- {
- return feed(measured, AxisOrthoOrientation());
- }
-
- BiasCorrector<T> computeResult()
- {
- if (numSamples == 0)
- return {Eigen::Vector3f{0, 0, 0}};
- return {sum / numSamples};
- }
+ BiasCalibration();
+
+ void setReferenceVector(Eigen::Vector3f vec);
+
+ Eigen::Vector3f getReferenceVector();
+
+ bool feed(const T& measured, const AxisOrientation& transform) override;
+
+ bool feed(const T& measured);
+
+ BiasCorrector<T> computeResult();
private:
Eigen::Vector3f sum, ref;
unsigned numSamples;
};
+template <typename T>
+BiasCorrector<T>::BiasCorrector()
+{
+}
+
+template <typename T>
+BiasCorrector<T>::BiasCorrector(const Eigen::Vector3f& bias) : bias(bias)
+{
+}
+
+template <typename T>
+void BiasCorrector<T>::operator>>(Eigen::Vector3f& rhs)
+{
+ rhs = bias;
+}
+
+template <typename T>
+void BiasCorrector<T>::operator<<(const Eigen::Vector3f& rhs)
+{
+ bias = rhs;
+}
+
+template <typename T>
+void BiasCorrector<T>::setIdentity()
+{
+ bias = {0, 0, 0};
+}
+
+template <typename T>
+T BiasCorrector<T>::correct(const T& data) const
+{
+ T output;
+ Eigen::Vector3f tmp;
+
+ data >> tmp;
+ tmp += bias;
+ output << tmp;
+
+ return output;
+}
+
+template <typename T>
+BiasCalibration<T>::BiasCalibration()
+ : sum(0, 0, 0), ref(0, 0, 0), numSamples(0)
+{
+}
+
+template <typename T>
+void BiasCalibration<T>::setReferenceVector(Eigen::Vector3f vec)
+{
+ ref = vec;
+}
+
+template <typename T>
+Eigen::Vector3f BiasCalibration<T>::getReferenceVector()
+{
+ return ref;
+}
+
+/**
+ * BiasCalibration accepts an indefinite number of samples,
+ * so feed(...) never returns false.
+ */
+template <typename T>
+bool BiasCalibration<T>::feed(const T& measured,
+ const AxisOrientation& transform)
+{
+ Eigen::Vector3f vec;
+ measured >> vec;
+
+ sum += (transform.getMatrix().transpose() * ref) - vec;
+ numSamples++;
+
+ return true;
+}
+
+template <typename T>
+bool BiasCalibration<T>::feed(const T& measured)
+{
+ return feed(measured, AxisOrthoOrientation());
+}
+
+template <typename T>
+BiasCorrector<T> BiasCalibration<T>::computeResult()
+{
+ if (numSamples == 0)
+ return {Eigen::Vector3f{0, 0, 0}};
+ return {sum / numSamples};
+}
+
} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/Calibration.h b/src/shared/sensors/calibration/Calibration.h
index 8fa838df73d4de676d18c9256e4f9d86bb21caec..64673246b4ef425db93d3cddd2219d48533ee8da 100644
--- a/src/shared/sensors/calibration/Calibration.h
+++ b/src/shared/sensors/calibration/Calibration.h
@@ -1,5 +1,5 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Riccardo Musso
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, Alberto Nidasio
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -22,315 +22,67 @@
#pragma once
-#include <sensors/Sensor.h>
-
#include <Eigen/Core>
-#include <Eigen/Geometry>
namespace Boardcore
{
/**
- * This class is used to adjust the values given by sensors during the flight.
- * An object can be obtained only via deserialization or if produced by an
- * instance of the "CalibrationModel" class.
- *
- * T is the type of sensor data the model is applied to (e.g.
- * GyroscopeData, MagnetometerData). This is needed because certain models could
- * work slightly differently depending on it
- *
- * Note: derived classes of ValuesCorrector<XX> MUST implement the following
- * operators (to make possible to store and load the coefficients):
+ * @brief This class is used to adjust the values given by a sensor.
*
- * operator << (const XX& t);
- * operator >> (XX& t);
- *
- * where XX is a datatype that can fully contain all the coefficients used
- * by the function correct(input).
- *
- * Also, an empty constructor must create a neutral instance (identity
- * transformation).
+ * @tparam T Type of sensor data the model is applied to (e.g.
+ * GyroscopeData, MagnetometerData).
*/
template <typename T>
class ValuesCorrector
{
public:
/**
- * This method will sets the internal coefficients so that the corrected
+ * @brief Sets the internal coefficients so that the corrected
* values are exactly the same of the inputted ones.
*/
virtual void setIdentity() = 0;
+ /**
+ * @brief Applies the correction to the given input.
+ */
virtual T correct(const T& input) const = 0;
};
/**
- * AbstractCalibrationModel represents a "factory" of ValuesCorrector instances,
- * and it's necessary to create one. You will always use one of its derived
- * classes, of course.
+ * @brief Utility to compute calibration parameters for a specific sensor.
*
- * Values given to the feed() function are needed for the training of the model.
+ * The calibration appens in two phases:
+ * - Sample collection: feed is called to store values;
+ * - Coefficient computation: computeResult is used to calculate the correction
+ * parameters.
*
- * T is the sensor data type
- * C is a class that extends ValuesCorrector<T>
+ * @tparam T Sensor's data type used in the feed function.
+ * @tparam C ValuesCorrector returned by computeResult.
*/
template <typename T, typename C, typename... AdditionalFeedParams>
-class AbstractCalibrationModel
+class AbstractCalibration
{
public:
/**
- * Gives to the model a single measurement to store and use to produce the
- * adapted ValueCorrector instance
+ * @brief Stores the measurement for further processing.
*
* @returns false if the model can't accept the given data (usually because
- * the internal buffers are full)
+ * the internal buffers are full).
*/
virtual bool feed(const T& measurement,
const AdditionalFeedParams&... params) = 0;
/**
- * Creates the best ValuesCorrector instance for the given measurements.
- * Note: you must feed some data to the model before getting the result!
+ * @brief Uses the recorded measurements to computer the correction
+ * parameters needed to correct sensor's data.
+ *
+ * Note: You need to feed the algorithm enough samples. Otherwise the method
+ * returns an ineffective corrector.
+ *
+ * @return ValuesCorrector containing the correction parameters.
*/
virtual C computeResult() = 0;
-
- virtual ~AbstractCalibrationModel(){};
-};
-
-/**
- * This class acts like a Sensor driver but incorporates both a Sensor<T>
- * instance and a ValuesCorrector. It can be useful to add a calibration step to
- * alredy existing code that uses the Sensor API.
- */
-template <typename SensorData>
-class SensorWrapper : public Sensor<SensorData>
-{
- using S = Sensor<SensorData>;
- using C = ValuesCorrector<SensorData>;
-
-public:
- SensorWrapper(S* _sensor, C* _calib) : sensor(_sensor), calib(_calib) {}
-
- S* getSensor() { return sensor; }
- void setSensor(S* s) { sensor = s; }
-
- C* getValuesCorrector() { return calib; }
- void setValuesCorrector(C* c) { calib = c; }
-
- void init() override { sensor->init(); }
-
- bool test() override { return sensor->test(); }
-
- SensorData sampleImpl() override
- {
- return calib->correct(sensor->sampleImpl());
- }
-
- SensorErrors getWrappedSensorError() { return sensor->getLastError(); }
-
-private:
- S* sensor;
- C* calib;
-};
-
-/**
- * This enum act like versors towards the chosen axis.
- *
- * X, Y and Z always set according to the right hand rule, so that:
- * X is the index
- * Y is the second finger
- * Z is the thumb
- *
- */
-enum class Direction : uint8_t
-{
- POSITIVE_X = 0,
- NEGATIVE_X,
- POSITIVE_Y,
- NEGATIVE_Y,
- POSITIVE_Z,
- NEGATIVE_Z,
-};
-
-constexpr const char* humanFriendlyDirection[]{
- "North", "South", "East", "West", "Down", "Up",
-};
-
-inline Eigen::Vector3f orientationToVector(Direction val)
-{
- switch (val)
- {
- case Direction::POSITIVE_X:
- return {1, 0, 0};
- case Direction::NEGATIVE_X:
- return {-1, 0, 0};
- case Direction::POSITIVE_Y:
- return {0, 1, 0};
- case Direction::NEGATIVE_Y:
- return {0, -1, 0};
- case Direction::POSITIVE_Z:
- return {0, 0, 1};
- case Direction::NEGATIVE_Z:
- return {0, 0, -1};
- default:
- /* never happens, added just to shut up the warnings */
- return {0, 0, 0};
- }
-}
-
-/**
- * This struct represents in the most general way any kind of transformation of
- * the reference frame (axis X, Y and Z).
- * This data type is intended to simplify the code, so you shouldn't instantiate
- * this struct directly, but rather use the structures AxisAngleOrientation or
- * AxisOrthoOrientation that will be automatically corrected to this one, thanks
- * to the implicit cast in favor of AxisOrientation.
- *
- * For example:
- *
- * AxisAngleOrientation angles ( PI/2, PI, 0);
- * AxisOrthoOrientation ortho ( Direction::NEGATIVE_X,
- * Direction::POSITIVE_Z );
- *
- * // The implicit cast is supported and recommended
- * AxisOrientation converted1 = angles;
- * AxisOrientation converted2 = ortho;
- *
- * // Now we can use the generated matrix:
- * Eigen::Vector3f zeta = convertedX.getMatrix() * Eigen::Vector3f { 0, 0, 1 }
- *
- */
-struct AxisOrientation
-{
- Eigen::Matrix3f mat;
-
- AxisOrientation() : mat(Eigen::Matrix3f::Identity()) {}
-
- AxisOrientation(Eigen::Matrix3f _mat) : mat(_mat) {}
-
- void setMatrix(Eigen::Matrix3f _mat) { mat = _mat; }
-
- Eigen::Matrix3f getMatrix() const { return mat; }
-};
-
-/**
- * This struct uses the three angles yaw, pitch and roll to define the
- * transformation of the reference frame, so according to N.E.D standard we get:
- *
- * X (north)
- * /
- * /
- * .----> Y (east)
- * |
- * |
- * v
- * Z (down)
- *
- * Where:
- * Yaw is rotation of Z axis
- * Pitch is rotation of Y axis
- * Roll is rotation of X axis
- */
-struct AxisAngleOrientation
-{
- float yaw, pitch, roll;
-
- AxisAngleOrientation() : yaw(0), pitch(0), roll(0) {}
-
- AxisAngleOrientation(float _yaw, float _pitch, float _roll)
- : yaw(_yaw), pitch(_pitch), roll(_roll)
- {
- }
-
- operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
-
- Eigen::Matrix3f getMatrix() const
- {
- return (Eigen::AngleAxisf(yaw, Eigen::Vector3f{0, 0, 1}) *
- Eigen::AngleAxisf(pitch, Eigen::Vector3f{0, 1, 0}) *
- Eigen::AngleAxisf(roll, Eigen::Vector3f{1, 0, 0}))
- .toRotationMatrix();
- }
-};
-
-/**
- * This struct represents the orientation of the reference frame relative
- * to X, Y, Z in the start orientation.
- * If we know the orientation of the X and Y axis, using the right hand rule
- * we can infer the Z axis.
- *
- * For example, if the base reference is:
- *
- * z
- * ^
- * |
- * |
- * /----> y
- * /
- * x
- *
- * Then if we set x = NEGATIVE_Y, y = POSITIVE_Z, we get:
- *
- * y z
- * ^ ^
- * | /
- * | /
- * x <----/
- *
- */
-struct AxisOrthoOrientation
-{
- Direction xAxis, yAxis;
-
- AxisOrthoOrientation()
- : xAxis(Direction::POSITIVE_X), yAxis(Direction::POSITIVE_Y)
- {
- }
-
- AxisOrthoOrientation(Direction _xAxis, Direction _yAxis)
- : xAxis(_xAxis), yAxis(_yAxis)
- {
- }
-
- operator AxisOrientation() const { return AxisOrientation(getMatrix()); }
-
- Eigen::Matrix3f getMatrix() const
- {
- Eigen::Vector3f vx, vy, vz;
-
- vx = orientationToVector(xAxis);
- vy = orientationToVector(yAxis);
- vz = vx.cross(vy);
-
- Eigen::Matrix3f mat;
- mat.col(0) << vx;
- mat.col(1) << vy;
- mat.col(2) << vz;
- return mat;
- }
-};
-
-/**
- * This struct represents axis orientation relative to a reference system.
- * Operatively it simply combines two transformations. It is particularly useful
- * to obtain an AxisOrientation from a reference system generally not N.E.D.
- */
-struct AxisRelativeOrientation
-{
- AxisOrientation systemOrientation, orientation;
-
- AxisRelativeOrientation(const AxisOrientation& _systemOrientation,
- const AxisOrientation& _orientation)
- : systemOrientation(_systemOrientation), orientation(_orientation)
- {
- }
-
- operator AxisOrientation() const { return {getMatrix()}; }
-
- Eigen::Matrix3f getMatrix() const
- {
- return systemOrientation.getMatrix() * orientation.getMatrix();
- }
};
} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/HardIronCalibration.h b/src/shared/sensors/calibration/HardIronCalibration.h
index 720f9a7edaae543870665af1e92f54ce9a4484ca..2cd616ac58379671d3958859f3eea3d12585ff64 100644
--- a/src/shared/sensors/calibration/HardIronCalibration.h
+++ b/src/shared/sensors/calibration/HardIronCalibration.h
@@ -68,7 +68,7 @@ private:
template <unsigned MaxSamples>
class HardIronCalibration
- : public AbstractCalibrationModel<MagnetometerData, HardIronCorrector>
+ : public AbstractCalibration<MagnetometerData, HardIronCorrector>
{
public:
HardIronCalibration() : samples(), numSamples(0) {}
diff --git a/src/shared/sensors/calibration/SensorDataExtra.cpp b/src/shared/sensors/calibration/SensorDataExtra.cpp
index f8ffd7ea87afd7c786ca4b59b98bf88453ad62c9..ed69028b1d522410bb381d021002bcd39bc0739c 100644
--- a/src/shared/sensors/calibration/SensorDataExtra.cpp
+++ b/src/shared/sensors/calibration/SensorDataExtra.cpp
@@ -34,6 +34,13 @@ void operator<<(AccelerometerData& lhs, const Vector3f& rhs)
lhs.accelerationZ = rhs[2];
}
+void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs)
+{
+ lhs[0] = rhs.accelerationX;
+ lhs[1] = rhs.accelerationY;
+ lhs[2] = rhs.accelerationZ;
+}
+
void operator<<(GyroscopeData& lhs, const Vector3f& rhs)
{
lhs.angularVelocityX = rhs[0];
@@ -41,6 +48,13 @@ void operator<<(GyroscopeData& lhs, const Vector3f& rhs)
lhs.angularVelocityZ = rhs[2];
}
+void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs)
+{
+ lhs[0] = rhs.angularVelocityX;
+ lhs[1] = rhs.angularVelocityY;
+ lhs[2] = rhs.angularVelocityZ;
+}
+
void operator<<(MagnetometerData& lhs, const Vector3f& rhs)
{
lhs.magneticFieldX = rhs[0];
@@ -48,25 +62,35 @@ void operator<<(MagnetometerData& lhs, const Vector3f& rhs)
lhs.magneticFieldZ = rhs[2];
}
-void operator>>(const AccelerometerData& lhs, Vector3f& rhs)
+void operator<<(Eigen::Vector3f& lhs, const MagnetometerData& rhs)
+{
+ lhs[0] = rhs.magneticFieldX;
+ lhs[1] = rhs.magneticFieldY;
+ lhs[2] = rhs.magneticFieldZ;
+}
+
+void operator>>(const AccelerometerData& lhs, Eigen::Vector3f& rhs)
{
- rhs[0] = lhs.accelerationX;
- rhs[1] = lhs.accelerationY;
- rhs[2] = lhs.accelerationZ;
+ rhs << lhs;
}
-void operator>>(const GyroscopeData& lhs, Vector3f& rhs)
+void operator>>(const Eigen::Vector3f& lhs, AccelerometerData& rhs)
+{
+ rhs << lhs;
+}
+
+void operator>>(const GyroscopeData& lhs, Eigen::Vector3f& rhs) { rhs << lhs; }
+
+void operator>>(const Eigen::Vector3f& lhs, GyroscopeData& rhs) { rhs << lhs; }
+
+void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs)
{
- rhs[0] = lhs.angularVelocityX;
- rhs[1] = lhs.angularVelocityY;
- rhs[2] = lhs.angularVelocityZ;
+ rhs << lhs;
}
-void operator>>(const MagnetometerData& lhs, Vector3f& rhs)
+void operator>>(const Eigen::Vector3f& lhs, MagnetometerData& rhs)
{
- rhs[0] = lhs.magneticFieldX;
- rhs[1] = lhs.magneticFieldY;
- rhs[2] = lhs.magneticFieldZ;
+ rhs << lhs;
}
} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SensorDataExtra.h b/src/shared/sensors/calibration/SensorDataExtra.h
index 0885a3a5ead12f3eb67c718451031f4388a882f8..cc52c472f14c904ff56913c3bb7d8e54ef8f6bae 100644
--- a/src/shared/sensors/calibration/SensorDataExtra.h
+++ b/src/shared/sensors/calibration/SensorDataExtra.h
@@ -46,16 +46,26 @@ namespace Boardcore
void operator<<(AccelerometerData& lhs, const Eigen::Vector3f& rhs);
+void operator<<(Eigen::Vector3f& lhs, const AccelerometerData& rhs);
+
void operator<<(GyroscopeData& lhs, const Eigen::Vector3f& rhs);
+void operator<<(Eigen::Vector3f& lhs, const GyroscopeData& rhs);
+
void operator<<(MagnetometerData& lhs, const Eigen::Vector3f& rhs);
+void operator<<(Eigen::Vector3f& lhs, const MagnetometerData& rhs);
+
void operator>>(const AccelerometerData& lhs, Eigen::Vector3f& rhs);
+void operator>>(const Eigen::Vector3f& lhs, AccelerometerData& rhs);
+
void operator>>(const GyroscopeData& lhs, Eigen::Vector3f& rhs);
-void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs);
+void operator>>(const Eigen::Vector3f& lhs, GyroscopeData& rhs);
void operator>>(const MagnetometerData& lhs, Eigen::Vector3f& rhs);
+void operator>>(const Eigen::Vector3f& lhs, MagnetometerData& rhs);
+
} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SixParameterCalibration.h b/src/shared/sensors/calibration/SixParameterCalibration.h
index b080020861fb33ac622ad94047d82b4d71ba859d..8296a1ac974718cc02c0b174d1cd2c4f56a1482f 100644
--- a/src/shared/sensors/calibration/SixParameterCalibration.h
+++ b/src/shared/sensors/calibration/SixParameterCalibration.h
@@ -89,8 +89,7 @@ private:
template <typename T, unsigned MaxSamples>
class SixParameterCalibration
- : public AbstractCalibrationModel<T, SixParameterCorrector<T>,
- AxisOrientation>
+ : public AbstractCalibration<T, SixParameterCorrector<T>, AxisOrientation>
{
public:
SixParameterCalibration() : samples(), ref(1, 0, 0), numSamples(0) {}
diff --git a/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c4d49d703425c8ec7d84fb1052e135b0b655ae97
--- /dev/null
+++ b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
@@ -0,0 +1,159 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, Alberto Nidasio
+ *
+ * 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 "SoftAndHardIronCalibration.h"
+
+#include <sensors/calibration/SensorDataExtra.h>
+
+#include <iostream>
+
+using namespace Eigen;
+
+namespace Boardcore
+{
+
+SoftAndHardIronCorrector::SoftAndHardIronCorrector()
+ : SoftAndHardIronCorrector({0, 0, 0}, {1, 1, 1})
+{
+}
+
+SoftAndHardIronCorrector::SoftAndHardIronCorrector(const Vector3f& offset,
+ const Vector3f& gain)
+ : offset(offset), gain(gain)
+{
+}
+
+void SoftAndHardIronCorrector::setIdentity()
+{
+ offset = {0, 0, 0};
+ gain = {1, 1, 1};
+}
+
+MagnetometerData SoftAndHardIronCorrector::correct(
+ const MagnetometerData& sample) const
+{
+ MagnetometerData output;
+ Vector3f tmp;
+
+ tmp << sample;
+ tmp = (tmp + offset).cwiseProduct(gain);
+ output << tmp;
+
+ return output;
+}
+
+Eigen::Vector3f SoftAndHardIronCorrector::getOffset() const { return offset; }
+
+void SoftAndHardIronCorrector::setOffset(const Eigen::Vector3f& offset)
+{
+ this->offset = offset;
+}
+
+Eigen::Vector3f SoftAndHardIronCorrector::getGain() const { return gain; }
+
+void SoftAndHardIronCorrector::setGain(const Eigen::Vector3f& gain)
+{
+ this->gain = gain;
+}
+
+SoftAndHardIronCalibration::SoftAndHardIronCalibration() {}
+
+bool SoftAndHardIronCalibration::feed(const MagnetometerData& data)
+{
+ // Let S a matrix of Nx7 composed as [x^2, y^2, z^2, x, y, z, 1]
+ // D need to be S^T * S
+ // To avoid storing all measurements we just need to incrementally add to D
+
+ Vector3f vector;
+ vector << data;
+ Vector<float, 7> S;
+ // cppcheck-suppress constStatement
+ S << vector.cwiseProduct(vector), vector, 1;
+
+ for (int i = 0; i < 7; i++)
+ for (int j = 0; j < 7; j++)
+ D(i, j) += S(i) * S(j);
+
+ return true;
+}
+
+SoftAndHardIronCorrector SoftAndHardIronCalibration::computeResult()
+{
+ // Compute eigen value and vectors of D
+ SelfAdjointEigenSolver<Matrix<float, 7, 7>> solver(D);
+ auto eigenValues = solver.eigenvalues();
+
+ // Find the smallest eigen value and vector
+ float minValue = eigenValues[0];
+ int minIdx = 0;
+
+ for (int i = 0; i < eigenValues.rows(); i++)
+ if (minValue > eigenValues[i])
+ {
+ minValue = eigenValues[i];
+ minIdx = i;
+ }
+ Eigen::Matrix<float, 7, 1> vec = solver.eigenvectors().col(minIdx);
+
+ // Invert the vector if necessary
+ float det = vec[0] * vec[1] * vec[2];
+ if (det)
+ {
+ vec *= -1;
+ det *= -1;
+ }
+
+ // Compute offset and gain
+ Vector3f offset{vec[3] / vec[0] / 2, vec[4] / vec[1] / 2,
+ vec[5] / vec[2] / 2};
+ Vector3f gain = (vec.block(0, 0, 3, 1) / cbrt(det)).cwiseSqrt();
+
+ return {offset, gain};
+}
+
+void operator<<(Eigen::Matrix<float, 3, 2>& lhs,
+ const SoftAndHardIronCorrector& rhs)
+{
+ lhs.col(0) = rhs.getOffset();
+ lhs.col(0) = rhs.getGain();
+}
+
+void operator<<(SoftAndHardIronCorrector& lhs,
+ const Eigen::Matrix<float, 3, 2>& rhs)
+{
+ lhs.setOffset(rhs.col(0));
+ lhs.setGain(rhs.col(1));
+}
+
+void operator>>(const Eigen::Matrix<float, 3, 2>& lhs,
+ SoftAndHardIronCorrector& rhs)
+{
+ rhs << lhs;
+}
+
+void operator>>(const SoftAndHardIronCorrector& lhs,
+ Eigen::Matrix<float, 3, 2>& rhs)
+{
+ rhs << lhs;
+}
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h
new file mode 100644
index 0000000000000000000000000000000000000000..61b36c35fdcb91117a604a5f4cecc278ee5794f4
--- /dev/null
+++ b/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h
@@ -0,0 +1,108 @@
+/* Copyright (c) 2021-2022 Skyward Experimental Rocketry
+ * Authors: Riccardo Musso, Alberto Nidasio
+ *
+ * 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>
+#include <sensors/calibration/Calibration.h>
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+#include <vector>
+
+namespace Boardcore
+{
+
+/*
+ * The Soft-iron calibration removes the measurement error given by both the
+ * Hard and Soft Iron distortion of the magnetic field.
+ */
+class SoftAndHardIronCorrector : public ValuesCorrector<MagnetometerData>
+{
+public:
+ SoftAndHardIronCorrector();
+
+ SoftAndHardIronCorrector(const Eigen::Vector3f& offset,
+ const Eigen::Vector3f& gain);
+
+ void setIdentity() override;
+
+ MagnetometerData correct(const MagnetometerData& sample) const override;
+
+ Eigen::Vector3f getOffset() const;
+
+ void setOffset(const Eigen::Vector3f& offset);
+
+ Eigen::Vector3f getGain() const;
+
+ void setGain(const Eigen::Vector3f& gain);
+
+private:
+ Eigen::Vector3f offset;
+ Eigen::Vector3f gain;
+};
+
+/**
+ * @brief Soft and hard iron calibration utility.
+ *
+ * Fits a non-rotated ellipsoid to the calibration data and then derives the
+ * correction parameters.
+ *
+ * Reference:
+ * https://www.st.com/resource/en/design_tip/dt0059-ellipsoid-or-sphere-fitting-for-sensor-calibration-stmicroelectronics.pdf
+ *
+ * @tparam MaxSamples
+ */
+class SoftAndHardIronCalibration
+ : public AbstractCalibration<MagnetometerData, SoftAndHardIronCorrector>
+{
+public:
+ SoftAndHardIronCalibration();
+
+ bool feed(const MagnetometerData& data) override;
+
+ /**
+ * @brief Uses the recorded measurements to compute the correction
+ * parameters needed to correct sensor's data.
+ *
+ * Note: Feed at leas 9 measurements!
+ *
+ * @return SoftAndHardIronCorrector containing the correction parameters.
+ */
+ SoftAndHardIronCorrector computeResult() override;
+
+private:
+ Eigen::Matrix<float, 7, 7> D = Eigen::Matrix<float, 7, 7>::Zero();
+};
+
+void operator<<(Eigen::Matrix<float, 3, 2>& lhs,
+ const SoftAndHardIronCorrector& rhs);
+
+void operator<<(SoftAndHardIronCorrector& lhs,
+ const Eigen::Matrix<float, 3, 2>& rhs);
+
+void operator>>(const Eigen::Matrix<float, 3, 2>& lhs,
+ SoftAndHardIronCorrector& rhs);
+
+void operator>>(const SoftAndHardIronCorrector& lhs,
+ Eigen::Matrix<float, 3, 2>& rhs);
+
+} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/SoftIronCalibration.h b/src/shared/sensors/calibration/SoftIronCalibration.h
deleted file mode 100644
index 3b6dba4a9e4dc761798c71343191c7f68b994843..0000000000000000000000000000000000000000
--- a/src/shared/sensors/calibration/SoftIronCalibration.h
+++ /dev/null
@@ -1,194 +0,0 @@
-/* Copyright (c) 2021 Skyward Experimental Rocketry
- * Author: Riccardo Musso
- *
- * 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>
-
-#include <Eigen/Core>
-#include <Eigen/Eigenvalues>
-
-#include "Calibration.h"
-
-namespace Boardcore
-{
-
-/*
- * The Soft-iron calibration removes the measurement error given by both the
- * Hard and Soft Iron distortion of the magnetic field.
- */
-class SoftIronCorrector : public ValuesCorrector<MagnetometerData>
-{
-public:
- SoftIronCorrector() : SoftIronCorrector({1, 1, 1}, {0, 0, 0}) {}
-
- SoftIronCorrector(const Eigen::Vector3f& _p, const Eigen::Vector3f& _q)
- : p(_p), q(_q)
- {
- }
-
- void setIdentity() override
- {
- p = {1, 1, 1};
- q = {0, 0, 0};
- }
-
- void operator>>(Eigen::Matrix<float, 3, 2>& rhs)
- {
- rhs.col(0) = p.transpose();
- rhs.col(1) = q.transpose();
- }
-
- void operator<<(const Eigen::Matrix<float, 3, 2>& rhs)
- {
- p = rhs.col(0).transpose();
- q = rhs.col(1).transpose();
- }
-
- MagnetometerData correct(const MagnetometerData& input) const override
- {
- MagnetometerData output;
- Eigen::Vector3f vec;
-
- input >> vec;
- vec = vec.cwiseProduct(p) + q;
- output << vec;
-
- return output;
- }
-
-private:
- Eigen::Vector3f p, q;
-};
-
-template <unsigned MaxSamples>
-class SoftIronCalibration
- : public AbstractCalibrationModel<MagnetometerData, SoftIronCorrector>
-{
-public:
- SoftIronCalibration() : samples(), numSamples(0) {}
-
- bool feed(const MagnetometerData& data) override
- {
- if (numSamples >= MaxSamples)
- return false;
-
- Eigen::Vector3f vec;
- data >> vec;
-
- for (int i = 0; i < 3; ++i)
- {
- samples(numSamples, i) = vec[i] * vec[i];
- samples(numSamples, i + 3) = vec[i];
- }
- samples(numSamples, 6) = 1;
-
- numSamples++;
- return true;
- }
-
- SoftIronCorrector computeResult() override
- {
- using Mx = Eigen::Matrix<float, 7, 7>;
- using Vec7 = Eigen::Matrix<float, 7, 1>;
-
- float minValue, det;
- int minIdx;
-
- Eigen::MatrixXf tmp1, tmp2;
- Eigen::Vector3f p, q;
- Vec7 vec;
- Mx mat;
-
- tmp1 = samples.block(0, 0, numSamples, 7);
- tmp2 = tmp1.transpose();
-
- // mat = tmp2 * tmp1;
-
- /*
- * tmp1: N x 7
- * tmp2: 7 x N
- *
- * Big matrices multiplication: if done with Eigen, the program crashes
- * (bus fault), so we'll do that with good old for's
- */
- for (unsigned i = 0; i < 7; ++i)
- {
- for (unsigned j = 0; j < 7; ++j)
- {
- mat(i, j) = 0;
-
- for (unsigned k = 0; k < numSamples; ++k)
- {
- mat(i, j) += tmp2(i, k) * tmp1(k, j);
- }
- }
- }
-
- Eigen::SelfAdjointEigenSolver<Mx> solver(mat);
- auto eigenvalues = solver.eigenvalues();
-
- minValue = eigenvalues[0];
- minIdx = 0;
-
- for (int i = 1; i < eigenvalues.rows(); ++i)
- {
- if (minValue > eigenvalues[i])
- {
- minValue = eigenvalues[i];
- minIdx = i;
- }
- }
-
- vec = solver.eigenvectors().col(minIdx);
- det = vec[0] * vec[1] * vec[2];
-
- if (det < 0)
- {
- vec *= -1;
- det *= -1;
- }
-
- p = vec.block(0, 0, 3, 1) / cbrt(det);
- q = {vec[3] / vec[0] / 2, vec[4] / vec[1] / 2, vec[5] / vec[2] / 2};
-
- p[0] = sqrt(p[0]);
- p[1] = sqrt(p[1]);
- p[2] = sqrt(p[2]);
-
- q = q.cwiseProduct(p);
-
- return {p, q};
- }
-
- Eigen::Matrix<float, MaxSamples, 7> getSamples() { return samples; }
-
-private:
- /*
- * The matrix contains x, y, z, x^2, y^2, z^2 and a column of 1s
- * row. Its shape is (N x 7)
- */
- Eigen::Matrix<float, MaxSamples, 7> samples;
- unsigned numSamples;
-};
-
-} // namespace Boardcore
diff --git a/src/shared/sensors/calibration/TwelveParameterCalibration.h b/src/shared/sensors/calibration/TwelveParameterCalibration.h
index 061fbbb25d61f7e76a4f1091a32f17aa00f10b9a..832eac3d4466512f365aeee270d0946915f7d184 100644
--- a/src/shared/sensors/calibration/TwelveParameterCalibration.h
+++ b/src/shared/sensors/calibration/TwelveParameterCalibration.h
@@ -97,8 +97,8 @@ private:
template <typename T, unsigned MaxSamples>
class TwelveParameterCalibration
- : public AbstractCalibrationModel<T, TwelveParameterCorrector<T>,
- AxisOrientation>
+ : public AbstractCalibration<T, TwelveParameterCorrector<T>,
+ AxisOrientation>
{
public:
TwelveParameterCalibration() : samples(), numSamples(0), ref({1, 0, 0}) {}
diff --git a/src/shared/utils/AeroUtils/AeroUtils.cpp b/src/shared/utils/AeroUtils/AeroUtils.cpp
index 190baaa16e1b3f196d626a63395a180712f8f160..c984cdfc247550036c4d88eef544fc4cf4d68663 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.cpp
+++ b/src/shared/utils/AeroUtils/AeroUtils.cpp
@@ -24,6 +24,8 @@
#include <utils/Constants.h>
+using namespace Eigen;
+
namespace Boardcore
{
@@ -66,6 +68,18 @@ float verticalSpeed(float p, float dpDt, float pRef, float tRef)
return -(tRef * dpDt * powf(p / pRef, nInv)) / (a * n * p);
}
+Vector2f geodetic2NED(const Vector2f& gpsData, const Vector2f& offset)
+{
+ float mPerDegLat = 111132.95225;
+ float mPerDegLon =
+ fabsf(111412.87733 * cosf(gpsData[0] * Constants::DEGREES_TO_RADIANS));
+
+ return {
+ mPerDegLat * (gpsData[0] - offset[0]),
+ mPerDegLon * (gpsData[1] - offset[1]),
+ };
+}
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/shared/utils/AeroUtils/AeroUtils.h b/src/shared/utils/AeroUtils/AeroUtils.h
index acc37325f4c6e52e2c20d74edb6839c7e343e448..80abb8dac650fc6054e359b34e65442bf1402d41 100644
--- a/src/shared/utils/AeroUtils/AeroUtils.h
+++ b/src/shared/utils/AeroUtils/AeroUtils.h
@@ -22,6 +22,7 @@
#pragma once
+#include <Eigen/Core>
#include <cmath>
namespace Boardcore
@@ -96,13 +97,24 @@ float mslTemperature(float temperatureRef, float altitudeRef);
* level
*
* @param p Current pressure (must be > 0) [Pa]
- * @param dpDt [Rate of change of pressure [Pa/s]]
+ * @param dpDt Rate of change of pressure [Pa/s]
* @param pRef Reference pressure (must be > 0) [Pa]
* @param tRef Reference temperature [K]
* @return Vertical speed, positive upwards [m/s]
*/
float verticalSpeed(float p, float dpDt, float pRef, float tRef);
+/**
+ * @brief Converts decimal degrees of latitude and longitude into displacement
+ * in meters between two positions the with an ellipsoidal earth model.
+ *
+ * @param position1 Latitude and longitude of current position [lat lon][deg]
+ * @param position2 Initial position used as an offset [lat lon][deg]
+ * @return Distance between the two coordinates [n e][m]
+ */
+Eigen::Vector2f geodetic2NED(const Eigen::Vector2f& position1,
+ const Eigen::Vector2f& position2);
+
} // namespace Aeroutils
} // namespace Boardcore
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index 418175edcb3bc0144e711fc7214d79ae565f1b36..ddc1e810901159f6b92d95041a8a3606dd8ef939 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -34,10 +34,13 @@ static constexpr const float RADIANS_TO_DEGREES = 180.0f / PI;
static constexpr const float g = 9.80665f; // [m^s^2]
-constexpr float a = 0.0065f; // Troposphere temperature gradient [deg/m]
-constexpr float R = 287.05f; // Air gas constant [J/Kg/K]
-constexpr float n = g / (R * a);
-constexpr float nInv = (R * a) / g;
+static constexpr float a = 0.0065f; // Troposphere temperature gradient [deg/m]
+static constexpr float R = 287.05f; // Air gas constant [J/Kg/K]
+static constexpr float n = g / (R * a);
+static constexpr float nInv = (R * a) / g;
+
+static constexpr const float MSL_PRESSURE = 101325.0f; // [Pa]
+static constexpr const float MSL_TEMPERATURE = 288.15f; // [Kelvin]
} // namespace Constants
diff --git a/src/tests/algorithms/ExtendedKalman/test-ekf-with-logs.cpp b/src/tests/algorithms/ExtendedKalman/test-ekf-with-logs.cpp
deleted file mode 100644
index b253e527581913119ee7d03b50618102d5186b94..0000000000000000000000000000000000000000
--- a/src/tests/algorithms/ExtendedKalman/test-ekf-with-logs.cpp
+++ /dev/null
@@ -1,206 +0,0 @@
-/* Copyright (c) 2022 Skyward Experimental Rocketry
- * Author: Alberto Nidasio
- *
- * 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 <algorithms/ExtendedKalman/ExtendedKalman.h>
-#include <algorithms/ExtendedKalman/StateInitializer.h>
-#include <sensors/BMX160/BMX160.h>
-#include <utils/CSVReader/CSVReader.h>
-#include <utils/SkyQuaternion/SkyQuaternion.h>
-
-#include <iostream>
-
-using namespace miosix;
-using namespace Boardcore;
-using namespace Eigen;
-
-ExtendedKalmanConfig getEKConfig();
-ExtendedKalmanState readInitialState();
-void updateKalman(BMX160Data data);
-void printState();
-std::istream& operator>>(std::istream& input, BMX160Data& data);
-std::istream& operator>>(std::istream& input, ExtendedKalmanState& data);
-
-// Normalized NED magnetic field in Milan
-Vector3f nedMag = Vector3f(0.47338841, 0.02656764, 0.88045305);
-
-ExtendedKalman* kalman;
-bool triad = false;
-
-int main()
-{
- // Prepare the Kalman
- kalman = new ExtendedKalman(getEKConfig());
- kalman->setState(readInitialState());
-
- // Retrieve all the data
- auto logData = CSVParser<BMX160Data>("/sd/imu_data.csv", true).collect();
- printf("Log size: %d\n", logData.size());
-
- // Wait to start
- printf("Waiting...\n");
- std::cin.get();
-
- // Loop through the logs
- for (size_t index = 0; index < logData.size(); index++)
- {
- printf("Iteration %lu\n", (unsigned long)index);
- auto iterationData = logData.at(index);
-
- // Predict and correct
- updateKalman(iterationData);
-
- // Print results
- printState();
-
- // Wait to continue
- std::cin.get();
- }
-}
-
-ExtendedKalmanConfig getEKConfig()
-{
- ExtendedKalmanConfig config;
-
- config.T = 0.02f;
- config.SIGMA_BETA = 0.0001f;
- config.SIGMA_W = 0.3f;
- config.SIGMA_MAG = 0.1f;
- config.SIGMA_GPS = 10.0f;
- config.SIGMA_BAR = 4.3f;
- config.SIGMA_POS = 10.0;
- config.SIGMA_VEL = 10.0;
- config.P_POS = 1.0f;
- config.P_POS_VERTICAL = 10.0f;
- config.P_VEL = 1.0f;
- config.P_VEL_VERTICAL = 10.0f;
- config.P_ATT = 0.01f;
- config.P_BIAS = 0.01f;
- config.SATS_NUM = 6.0f;
-
- // Normalized magnetic field in Milan
- config.NED_MAG = nedMag;
-
- return config;
-}
-
-ExtendedKalmanState readInitialState()
-{
- auto initialState =
- CSVParser<ExtendedKalmanState>("/sd/initial_state.csv", true).collect();
-
- printf("Initial state count: %d\n", initialState.size());
-
- if (initialState.size() == 0)
- return ExtendedKalmanState();
- else
- return initialState.at(0);
-}
-
-void updateKalman(BMX160Data data)
-{
- // Vector3f acceleration(data.accelerationX, data.accelerationY,
- // data.accelerationZ);
- // kalman->predict(acceleration);
-
- Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
- data.angularVelocityZ);
- kalman->predictGyro(angularVelocity);
-
- Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
- data.magneticFieldZ);
- magneticField.normalize();
- kalman->correctMag(magneticField);
-}
-
-void printState()
-{
- auto kalmanState = kalman->getState().getX();
- auto kalmanRotation = SkyQuaternion::quat2eul(
- kalmanState.block<4, 1>(ExtendedKalman::IDX_QUAT, 0));
-
- std::cout << "Kalman state:" << std::endl;
- std::cout << kalmanState << std::endl;
- printf("Orientation: %6.4f, %6.2f, %6.4f\n", kalmanRotation(0),
- kalmanRotation(1), kalmanRotation(2));
-}
-
-std::istream& operator>>(std::istream& input, BMX160Data& data)
-{
- input >> data.accelerationTimestamp;
- input.ignore(1, ',');
- input >> data.accelerationX;
- input.ignore(1, ',');
- input >> data.accelerationY;
- input.ignore(1, ',');
- input >> data.accelerationZ;
- input.ignore(1, ',');
- input >> data.angularVelocityTimestamp;
- input.ignore(1, ',');
- input >> data.angularVelocityX;
- input.ignore(1, ',');
- input >> data.angularVelocityY;
- input.ignore(1, ',');
- input >> data.angularVelocityZ;
- input.ignore(1, ',');
- input >> data.magneticFieldTimestamp;
- input.ignore(1, ',');
- input >> data.magneticFieldX;
- input.ignore(1, ',');
- input >> data.magneticFieldY;
- input.ignore(1, ',');
- input >> data.magneticFieldZ;
-
- return input;
-}
-
-std::istream& operator>>(std::istream& input, ExtendedKalmanState& data)
-{
- input >> data.timestamp;
- input.ignore(1, ',');
- input >> data.n;
- input.ignore(1, ',');
- input >> data.e;
- input.ignore(1, ',');
- input >> data.d;
- input.ignore(1, ',');
- input >> data.vn;
- input.ignore(1, ',');
- input >> data.ve;
- input.ignore(1, ',');
- input >> data.vd;
- input.ignore(1, ',');
- input >> data.qx;
- input.ignore(1, ',');
- input >> data.qy;
- input.ignore(1, ',');
- input >> data.qz;
- input.ignore(1, ',');
- input >> data.qw;
- input.ignore(1, ',');
- input >> data.bx;
- input.ignore(1, ',');
- input >> data.by;
- input.ignore(1, ',');
- input >> data.bz;
-
- return input;
-}
diff --git a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f8af2b692b965d8873f721c62d57308931475a8
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
@@ -0,0 +1,141 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void imuInit();
+void imuStep();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+MPU9250* imu = nullptr;
+
+int main()
+{
+ imuInit();
+
+ nas = new NAS(getEKConfig());
+ setInitialOrientation();
+
+ TaskScheduler scheduler;
+ scheduler.addTask(imuStep, 20);
+ scheduler.start();
+
+ while (true)
+ Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+ NASConfig config;
+
+ config.T = 0.02f;
+ config.SIGMA_BETA = 0.0001f;
+ config.SIGMA_W = 0.3f;
+ config.SIGMA_MAG = 0.1f;
+ config.SIGMA_GPS = 10.0f;
+ config.SIGMA_BAR = 4.3f;
+ config.SIGMA_POS = 10.0;
+ config.SIGMA_VEL = 10.0;
+ config.P_POS = 1.0f;
+ config.P_POS_VERTICAL = 10.0f;
+ config.P_VEL = 1.0f;
+ config.P_VEL_VERTICAL = 10.0f;
+ config.P_ATT = 0.01f;
+ config.P_BIAS = 0.01f;
+ config.SATS_NUM = 6.0f;
+ config.NED_MAG = nedMag;
+
+ return config;
+}
+
+void setInitialOrientation()
+{
+ Eigen::Matrix<float, 13, 1> x;
+
+ // Set quaternions
+ Eigen::Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+ x(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ nas->setX(x);
+}
+
+void imuInit()
+{
+ imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+ imu->init();
+}
+
+void imuStep()
+{
+ imu->sample();
+ auto data = imu->getLastSample();
+ Vector3f acceleration(data.accelerationX, data.accelerationY,
+ data.accelerationZ);
+ Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+ data.angularVelocityZ);
+ Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+ data.magneticFieldZ);
+
+ // Calibration
+ {
+ Vector3f bias(0.0218462708018154, 0.0281755574886535,
+ 0.0264119470499244);
+ angularVelocity -= bias;
+ Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+ magneticField -= offset;
+ magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+ }
+
+ acceleration.normalize();
+ magneticField.normalize();
+
+ // Predict step
+ nas->predictGyro(angularVelocity);
+
+ // Correct step
+ nas->correctMag(magneticField);
+ nas->correctAcc(acceleration);
+
+ auto nasState = nas->getState();
+ printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+ nasState.qz);
+}
diff --git a/src/tests/algorithms/ExtendedKalman/test-ekf.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp
similarity index 62%
rename from src/tests/algorithms/ExtendedKalman/test-ekf.cpp
rename to src/tests/algorithms/NAS/test-attitude-stack.cpp
index 95c36ae7bbda099209eb8f9d88f9b1897342a51d..beacab318c26c6005805ca9f7bc24aa7e1147e6b 100644
--- a/src/tests/algorithms/ExtendedKalman/test-ekf.cpp
+++ b/src/tests/algorithms/NAS/test-attitude-stack.cpp
@@ -20,10 +20,10 @@
* THE SOFTWARE.
*/
-#include <algorithms/ExtendedKalman/ExtendedKalman.h>
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
#include <miosix.h>
#include <sensors/BMX160/BMX160.h>
-#include <sensors/LIS3MDL/LIS3MDL.h>
#include <sensors/SensorManager.h>
#include <utils/SkyQuaternion/SkyQuaternion.h>
@@ -33,36 +33,36 @@ using namespace miosix;
using namespace Boardcore;
using namespace Eigen;
-ExtendedKalmanConfig getEKConfig();
+NASConfig getEKConfig();
void setInitialOrientation();
-void bmxInit();
-void bmxCallback();
+void imuInit();
+void imuStep();
-ExtendedKalman* kalman;
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
-SPIBus spi1(SPI1);
-BMX160* bmx160 = nullptr;
+NAS* nas;
-Boardcore::SensorManager::SensorMap_t sensorsMap;
-Boardcore::SensorManager* sensorManager = nullptr;
+SPIBus spi1(SPI1);
+BMX160* imu = nullptr;
int main()
{
- bmxInit();
+ imuInit();
- sensorManager = new SensorManager(sensorsMap);
- kalman = new ExtendedKalman(getEKConfig());
+ nas = new NAS(getEKConfig());
setInitialOrientation();
- sensorManager->start();
+ TaskScheduler scheduler;
+ scheduler.addTask(imuStep, 20);
+ scheduler.start();
while (true)
Thread::sleep(1000);
}
-ExtendedKalmanConfig getEKConfig()
+NASConfig getEKConfig()
{
- ExtendedKalmanConfig config;
+ NASConfig config;
config.T = 0.02f;
config.SIGMA_BETA = 0.0001f;
@@ -70,8 +70,8 @@ ExtendedKalmanConfig getEKConfig()
config.SIGMA_MAG = 0.1f;
config.SIGMA_GPS = 10.0f;
config.SIGMA_BAR = 4.3f;
- config.SIGMA_POS = 10.0f;
- config.SIGMA_VEL = 10.0f;
+ config.SIGMA_POS = 10.0;
+ config.SIGMA_VEL = 10.0;
config.P_POS = 1.0f;
config.P_POS_VERTICAL = 10.0f;
config.P_VEL = 1.0f;
@@ -79,9 +79,7 @@ ExtendedKalmanConfig getEKConfig()
config.P_ATT = 0.01f;
config.P_BIAS = 0.01f;
config.SATS_NUM = 6.0f;
-
- // Normalized magnetic field in Milan
- config.NED_MAG = Vector3f(0.0812241, -0.963085, 0.256649);
+ config.NED_MAG = nedMag;
return config;
}
@@ -97,10 +95,10 @@ void setInitialOrientation()
x(8) = q(2);
x(9) = q(3);
- kalman->setX(x);
+ nas->setX(x);
}
-void bmxInit()
+void imuInit()
{
SPIBusConfig spiConfig;
spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
@@ -122,55 +120,48 @@ void bmxInit()
bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
- bmx160 = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
- spiConfig);
-
- SensorInfo info("BMX160", 20, &bmxCallback);
-
- sensorsMap.emplace(std::make_pair(bmx160, info));
+ imu = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+ spiConfig);
+ imu->init();
}
-void bmxCallback()
+void imuStep()
{
- auto data = bmx160->getLastSample();
-
- // Predict step
+ imu->sample();
+ auto data = imu->getLastSample();
+ Vector3f acceleration(data.accelerationX, data.accelerationY,
+ data.accelerationZ);
+ Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+ data.angularVelocityZ);
+ Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+ data.magneticFieldZ);
+
+ // Calibration
{
- Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
- data.angularVelocityZ);
Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
angularVelocity = angularVelocity - offset;
angularVelocity = angularVelocity / 180 * Constants::PI / 10;
-
- kalman->predictGyro(angularVelocity);
+ Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+ Matrix3f A{{0.688760050772712, 0, 0},
+ {0, 0.637715211784480, 0},
+ {0, 0, 2.27669720320908}};
+ magneticField = (magneticField - b).transpose() * A;
}
- // Correct with acclelerometer
- // {
- // Vector3f angularVelocity(data.accelerationX, data.accelerationY,
- // data.accelerationZ);
+ acceleration.normalize();
+ magneticField.normalize();
- // kalman->correctAcc(angularVelocity);
- // }
+ // Predict step
+ nas->predictGyro(angularVelocity);
// Correct step
- {
- Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
- data.magneticFieldZ);
-
- // Apply correction
- Vector3f b{21.0730033648425, -24.3997259703105, -2.32621524742862};
- Matrix3f A{{0.659926504672263, 0, 0},
- {0, 0.662442130094073, 0},
- {0, 0, 2.28747567094359}};
- magneticField = (magneticField - b).transpose() * A;
-
- magneticField.normalize();
- kalman->correctMag(magneticField);
- }
+ nas->correctMag(magneticField);
+ nas->correctAcc(acceleration);
- auto kalmanState = kalman->getState();
+ // std::cout << acceleration.transpose() << angularVelocity.transpose()
+ // << magneticField.transpose() << std::endl;
- printf("w%fwa%fab%fbc%fc\n", kalmanState.qw, kalmanState.qx, kalmanState.qy,
- kalmanState.qz);
+ auto nasState = nas->getState();
+ printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+ nasState.qz);
}
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57fd167c839ec3bdef2d24c69417ddb7f118aab1
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -0,0 +1,183 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void init();
+void step();
+void print();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+Vector2f startPos = Vector2f(45.501141, 9.156281);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+MPU9250* imu = nullptr;
+UbloxGPS* gps = nullptr;
+
+int main()
+{
+ init();
+
+ nas = new NAS(getEKConfig());
+ setInitialOrientation();
+
+ TaskScheduler scheduler;
+ scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+ scheduler.addTask(print, 250);
+ scheduler.start();
+
+ while (true)
+ Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+ NASConfig config;
+
+ config.T = 0.02f;
+ config.SIGMA_BETA = 0.0001f;
+ config.SIGMA_W = 0.3f;
+ config.SIGMA_MAG = 0.1f;
+ config.SIGMA_GPS = 10.0f;
+ config.SIGMA_BAR = 4.3f;
+ config.SIGMA_POS = 10.0;
+ config.SIGMA_VEL = 10.0;
+ config.P_POS = 1.0f;
+ config.P_POS_VERTICAL = 10.0f;
+ config.P_VEL = 1.0f;
+ config.P_VEL_VERTICAL = 10.0f;
+ config.P_ATT = 0.01f;
+ config.P_BIAS = 0.01f;
+ config.SATS_NUM = 6.0f;
+ config.NED_MAG = nedMag;
+
+ return config;
+}
+
+void setInitialOrientation()
+{
+ Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+
+ // Set quaternions
+ Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+ x(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ nas->setX(x);
+}
+
+void init()
+{
+ imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+ imu->init();
+
+ gps = new UbloxGPS(38400, 10, 2, "gps", 38400);
+ gps->init();
+ gps->start();
+
+ Logger::getInstance().start();
+}
+
+void step()
+{
+ imu->sample();
+ gps->sample();
+ auto imuData = imu->getLastSample();
+ auto gpsData = gps->getLastSample();
+
+ Vector3f acceleration(imuData.accelerationX, imuData.accelerationY,
+ imuData.accelerationZ);
+ Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY,
+ imuData.angularVelocityZ);
+ Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY,
+ imuData.magneticFieldZ);
+
+ Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+ gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+ Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityNorth);
+ Vector4f gpsCorrection;
+ // cppcheck-suppress constStatement
+ gpsCorrection << gpsPos, gpsVel;
+
+ // Calibration
+ {
+ Vector3f biasAcc(-0.1255, 0.2053, -0.2073);
+ acceleration -= biasAcc;
+ Vector3f bias(-0.0291, 0.0149, 0.0202);
+ angularVelocity -= bias;
+ Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+ magneticField -= offset;
+ magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+ }
+
+ // Predict step
+ nas->predictGyro(angularVelocity);
+ if (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 &&
+ gpsPos[1] > -1e3)
+ nas->predictAcc(acceleration);
+
+ // Correct step
+ magneticField.normalize();
+ nas->correctMag(magneticField);
+ acceleration.normalize();
+ nas->correctAcc(acceleration);
+ if (gpsData.fix)
+ nas->correctGPS(gpsCorrection);
+ nas->correctBaro(100000, 110000, 20 + 273.5);
+
+ auto nasState = nas->getState();
+ Logger::getInstance().log(imuData);
+ Logger::getInstance().log(gpsData);
+ Logger::getInstance().log(nasState);
+}
+
+void print()
+{
+ auto gpsData = gps->getLastSample();
+ auto nasState = nas->getState();
+
+ Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+ gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+
+ printf("%d, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0], gpsPos[1],
+ nasState.n, nasState.e, nasState.vn, nasState.ve);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-stack.cpp b/src/tests/algorithms/NAS/test-nas-stack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e72a1c9830b63682cffff7968cbc27106b85a61f
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-stack.cpp
@@ -0,0 +1,216 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/MS5803/MS5803.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Constants.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void init();
+void step();
+void print();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+Vector2f startPos = Vector2f(45.501141, 9.156281);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+BMX160* imu = nullptr;
+UbloxGPS* gps = nullptr;
+MS5803* baro = nullptr;
+
+int main()
+{
+ init();
+
+ nas = new NAS(getEKConfig());
+ setInitialOrientation();
+
+ TaskScheduler scheduler;
+ scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+ // scheduler.addTask(print, 250);
+ scheduler.start();
+
+ while (true)
+ Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+ NASConfig config;
+
+ config.T = 0.02f;
+ config.SIGMA_BETA = 0.0001f;
+ config.SIGMA_W = 0.3f;
+ config.SIGMA_MAG = 0.1f;
+ config.SIGMA_GPS = 10.0f;
+ config.SIGMA_BAR = 4.3f;
+ config.SIGMA_POS = 10.0;
+ config.SIGMA_VEL = 10.0;
+ config.P_POS = 1.0f;
+ config.P_POS_VERTICAL = 10.0f;
+ config.P_VEL = 1.0f;
+ config.P_VEL_VERTICAL = 10.0f;
+ config.P_ATT = 0.01f;
+ config.P_BIAS = 0.01f;
+ config.SATS_NUM = 6.0f;
+ config.NED_MAG = nedMag;
+
+ return config;
+}
+
+void setInitialOrientation()
+{
+ Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+
+ // Set quaternions
+ Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+ x(6) = q(0);
+ x(7) = q(1);
+ x(8) = q(2);
+ x(9) = q(3);
+
+ nas->setX(x);
+}
+
+void init()
+{
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+ BMX160Config bmx_config;
+ bmx_config.fifoMode = BMX160Config::FifoMode::HEADER;
+ bmx_config.fifoWatermark = 80;
+ bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+ bmx_config.temperatureDivider = 1;
+
+ bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+
+ bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
+
+ bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+ bmx_config.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100;
+ bmx_config.magnetometerRate = BMX160Config::OutputDataRate::HZ_100;
+
+ bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+ imu =
+ new BMX160(spi1, sensors::bmx160::cs::getPin(), bmx_config, spiConfig);
+ imu->init();
+
+ gps = new UbloxGPS(921600, 10, 2, "gps", 38400);
+ gps->init();
+ gps->start();
+
+ baro = new MS5803(spi1, sensors::ms5803::cs::getPin());
+ baro->init();
+
+ Logger::getInstance().start();
+}
+
+void step()
+{
+ imu->sample();
+ gps->sample();
+ baro->sample();
+ auto imuData = imu->getLastSample();
+ auto gpsData = gps->getLastSample();
+ auto baroData = baro->getLastSample();
+
+ Vector3f acceleration(imuData.accelerationX, imuData.accelerationY,
+ imuData.accelerationZ);
+ Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY,
+ imuData.angularVelocityZ);
+ Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY,
+ imuData.magneticFieldZ);
+
+ Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+ gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+ Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityEast);
+ Vector4f gpsCorrection;
+ // cppcheck-suppress constStatement
+ gpsCorrection << gpsPos, gpsVel;
+
+ // Calibration
+ {
+ Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+ angularVelocity = angularVelocity - offset;
+ angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+ Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+ Matrix3f A{{0.688760050772712, 0, 0},
+ {0, 0.637715211784480, 0},
+ {0, 0, 2.27669720320908}};
+ magneticField = (magneticField - b).transpose() * A;
+ }
+
+ // Predict step
+ nas->predictGyro(angularVelocity);
+ nas->predictAcc(acceleration);
+
+ // Correct step
+ magneticField.normalize();
+ nas->correctMag(magneticField);
+ acceleration.normalize();
+ nas->correctAcc(acceleration);
+ if (gpsData.fix)
+ nas->correctGPS(gpsCorrection);
+ nas->correctBaro(100000, Constants::MSL_PRESSURE,
+ Constants::MSL_TEMPERATURE);
+
+ auto nasState = nas->getState();
+
+ Logger::getInstance().log(imuData);
+ Logger::getInstance().log(gpsData);
+ Logger::getInstance().log(baroData);
+ Logger::getInstance().log(nasState);
+}
+
+void print()
+{
+ auto gpsData = gps->getLastSample();
+ auto baroData = baro->getLastSample();
+ auto nasState = nas->getState();
+
+ Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+ gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+
+ printf("%d, %f, %f, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0],
+ gpsPos[1], nasState.n, nasState.e, nasState.d, nasState.vn,
+ nasState.ve, baroData.pressure);
+}
diff --git a/src/tests/algorithms/ExtendedKalman/test-ekf-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
similarity index 84%
rename from src/tests/algorithms/ExtendedKalman/test-ekf-with-triad.cpp
rename to src/tests/algorithms/NAS/test-nas-with-triad.cpp
index 929b9fefd9a7d37b3873b2160742eceef9822532..59baeb1d1e37a36035e2fe6208ec22617ea8ad2d 100644
--- a/src/tests/algorithms/ExtendedKalman/test-ekf-with-triad.cpp
+++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
@@ -20,11 +20,10 @@
* THE SOFTWARE.
*/
-#include <algorithms/ExtendedKalman/ExtendedKalman.h>
-#include <algorithms/ExtendedKalman/StateInitializer.h>
+#include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
#include <miosix.h>
#include <sensors/BMX160/BMX160.h>
-#include <sensors/LIS3MDL/LIS3MDL.h>
#include <sensors/SensorManager.h>
#include <utils/SkyQuaternion/SkyQuaternion.h>
@@ -34,7 +33,7 @@ using namespace miosix;
using namespace Boardcore;
using namespace Eigen;
-ExtendedKalmanConfig getEKConfig();
+NASConfig getEKConfig();
void bmxInit();
void bmxCallback();
@@ -43,7 +42,7 @@ constexpr uint64_t CALIBRATION_TIMEOUT = 5 * 1e6;
Vector3f nedMag = Vector3f(0.47472049, 0.02757190, 0.87970463);
StateInitializer* stateInitializer;
-ExtendedKalman* kalman;
+NAS* nas;
SPIBus spi1(SPI1);
BMX160* bmx160 = nullptr;
@@ -57,7 +56,7 @@ int main()
sensorManager = new SensorManager(sensorsMap);
stateInitializer = new StateInitializer();
- kalman = new ExtendedKalman(getEKConfig());
+ nas = new NAS(getEKConfig());
// Logger::getInstance().start();
TimestampTimer::getInstance().resetTimestamp();
@@ -67,9 +66,9 @@ int main()
Thread::sleep(1000);
}
-ExtendedKalmanConfig getEKConfig()
+NASConfig getEKConfig()
{
- ExtendedKalmanConfig config;
+ NASConfig config;
config.T = 0.02f;
config.SIGMA_BETA = 0.0001f;
@@ -155,15 +154,15 @@ void bmxCallback()
}
else
{
- // Now the calibration has ended, compute and log the kalman state
+ // Now the calibration has ended, compute and log the nas state
calibrating = false;
- // Compute the initial kalman state
+ // Compute the initial nas state
stateInitializer->triad(accMean, magMean, nedMag);
- kalman->setX(stateInitializer->getInitX());
+ nas->setX(stateInitializer->getInitX());
// Save the state and the IMU data
- // Logger::getInstance().log(kalman->getState());
+ // Logger::getInstance().log(nas->getState());
data.accelerationX = accMean[0];
data.accelerationY = accMean[1];
data.accelerationZ = accMean[2];
@@ -181,8 +180,8 @@ void bmxCallback()
{
// Predict step
{
- // kalman->predictAcc(acceleration);
- kalman->predictGyro(angularVelocity);
+ // nas->predictAcc(acceleration);
+ nas->predictGyro(angularVelocity);
data.angularVelocityX = angularVelocity[0];
data.angularVelocityY = angularVelocity[1];
@@ -192,23 +191,23 @@ void bmxCallback()
// Correct step
{
magneticField.normalize();
- kalman->correctMag(magneticField);
+ nas->correctMag(magneticField);
data.magneticFieldX = magneticField[0];
data.magneticFieldY = magneticField[1];
data.magneticFieldZ = magneticField[2];
}
- auto kalmanState = kalman->getState();
+ auto nasState = nas->getState();
- kalmanState.timestamp = TimestampTimer::getInstance().getTimestamp();
- data.accelerationTimestamp = kalmanState.timestamp;
- data.magneticFieldTimestamp = kalmanState.timestamp;
- data.angularVelocityTimestamp = kalmanState.timestamp;
+ nasState.timestamp = TimestampTimer::getInstance().getTimestamp();
+ data.accelerationTimestamp = nasState.timestamp;
+ data.magneticFieldTimestamp = nasState.timestamp;
+ data.angularVelocityTimestamp = nasState.timestamp;
- // Logger::getInstance().log(kalmanState);
+ // Logger::getInstance().log(nasState);
// Logger::getInstance().log(data);
- printf("w%fwa%fab%fbc%fc\n", kalmanState.qw, kalmanState.qx,
- kalmanState.qy, kalmanState.qz);
+ printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+ nasState.qz);
}
}
diff --git a/src/tests/algorithms/NAS/test-tmp.cpp b/src/tests/algorithms/NAS/test-tmp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39fcdbd511705c39361f2a926ef92c15ea662ff9
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-tmp.cpp
@@ -0,0 +1,89 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <drivers/spi/SPIDriver.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <utils/Debug.h>
+
+using namespace miosix;
+
+using namespace Boardcore;
+
+GpioPin mpuCs = GpioPin(GPIOG_BASE, 3);
+SPIBus spi1(SPI1);
+
+MPU9250* mpu = nullptr;
+BMX160* bmx = nullptr;
+
+void initBoard()
+{
+ // Chip select pin as output starting high
+ mpuCs.mode(miosix::Mode::OUTPUT);
+ mpuCs.high();
+
+ mpu = new MPU9250(spi1, mpuCs);
+
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+ BMX160Config bmxConfig;
+ bmxConfig.fifoMode = BMX160Config::FifoMode::HEADER;
+ bmxConfig.fifoWatermark = 80;
+ bmxConfig.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+ bmxConfig.temperatureDivider = 1;
+ bmxConfig.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+ bmxConfig.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
+ bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+ bmxConfig.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100;
+ bmxConfig.magnetometerRate = BMX160Config::OutputDataRate::HZ_100;
+ bmxConfig.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+ bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmxConfig,
+ spiConfig);
+
+ mpu->init();
+ bmx->init();
+}
+
+int main()
+{
+ initBoard();
+
+ auto lastTick = getTick();
+ while (true)
+ {
+ mpu->sample();
+ bmx->sample();
+ auto mpuData = mpu->getLastSample();
+ auto bmxData = bmx->getLastSample();
+
+ printf("%f,%f,%f,", mpuData.magneticFieldX, mpuData.magneticFieldY,
+ mpuData.magneticFieldZ);
+ printf("%f,%f,%f\n", bmxData.magneticFieldX, bmxData.magneticFieldY,
+ bmxData.magneticFieldZ);
+
+ Thread::sleepUntil(lastTick + 20);
+ lastTick = getTick();
+ }
+}
diff --git a/src/tests/algorithms/NAS/test-triad-parafoil.cpp b/src/tests/algorithms/NAS/test-triad-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed93635ffcb04004de8ce7810051b48a21bb08bb
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-triad-parafoil.cpp
@@ -0,0 +1,79 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <algorithms/NAS/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/calibration/SensorDataExtra.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+void mpuInit();
+void bmxCallback();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+SPIBus spi1(SPI1);
+MPU9250* mpu = nullptr;
+
+int main()
+{
+ mpu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+ mpu->init();
+
+ auto lastTick = getTick();
+ while (true)
+ {
+ mpu->sample();
+ auto data = mpu->getLastSample();
+
+ Vector3f acceleration(data.accelerationX, data.accelerationY,
+ data.accelerationZ);
+
+ Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+ Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+ data.magneticFieldZ);
+ magneticField -= offset;
+ magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+
+ acceleration.normalize();
+ magneticField.normalize();
+
+ StateInitializer state;
+ state.triad(acceleration, magneticField, nedMag);
+
+ auto kalmanState = state.getInitX();
+ if (!std::isnan(kalmanState(9)))
+ printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
+ kalmanState(7), kalmanState(8));
+
+ Thread::sleepUntil(lastTick + 20);
+ lastTick = getTick();
+ }
+}
diff --git a/src/tests/algorithms/ExtendedKalman/test-triad.cpp b/src/tests/algorithms/NAS/test-triad.cpp
similarity index 55%
rename from src/tests/algorithms/ExtendedKalman/test-triad.cpp
rename to src/tests/algorithms/NAS/test-triad.cpp
index 0a29cc466c32c871f34fdd9ba0088b1e6872e7cf..f5104a1e0f2cb838a1b14f7b58ec072fbabcdcab 100644
--- a/src/tests/algorithms/ExtendedKalman/test-triad.cpp
+++ b/src/tests/algorithms/NAS/test-triad.cpp
@@ -20,11 +20,12 @@
* THE SOFTWARE.
*/
-#include <algorithms/ExtendedKalman/StateInitializer.h>
+#include <algorithms/NAS/StateInitializer.h>
#include <miosix.h>
#include <sensors/BMX160/BMX160.h>
-#include <sensors/LIS3MDL/LIS3MDL.h>
#include <sensors/SensorManager.h>
+#include <sensors/calibration/SensorDataExtra.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
#include <cmath>
#include <iostream>
@@ -33,29 +34,56 @@ using namespace miosix;
using namespace Boardcore;
using namespace Eigen;
-void bmxInit();
-void bmxCallback();
+void imuInit();
Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
SPIBus spi1(SPI1);
-BMX160* bmx160 = nullptr;
-
-Boardcore::SensorManager::SensorMap_t sensorsMap;
-Boardcore::SensorManager* sensorManager = nullptr;
+BMX160* bmx = nullptr;
int main()
{
- bmxInit();
-
- sensorManager = new SensorManager(sensorsMap);
- sensorManager->start();
+ imuInit();
+ bmx->init();
+ auto lastTick = getTick();
while (true)
- Thread::sleep(1000);
+ {
+ bmx->sample();
+ auto data = bmx->getLastSample();
+
+ Vector3f acceleration(data.accelerationX, data.accelerationY,
+ data.accelerationZ);
+ Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+ data.angularVelocityZ);
+ Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+ angularVelocity = angularVelocity - offset;
+ angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+ Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+ data.magneticFieldZ);
+ Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+ Matrix3f A{{0.688760050772712, 0, 0},
+ {0, 0.637715211784480, 0},
+ {0, 0, 2.27669720320908}};
+ magneticField = (magneticField - b).transpose() * A;
+
+ acceleration.normalize();
+ magneticField.normalize();
+
+ StateInitializer state;
+ state.triad(acceleration, magneticField, nedMag);
+
+ auto kalmanState = state.getInitX();
+ if (!std::isnan(kalmanState(9)))
+ printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
+ kalmanState(7), kalmanState(8));
+
+ Thread::sleepUntil(lastTick + 20);
+ lastTick = getTick();
+ }
}
-void bmxInit()
+void imuInit()
{
SPIBusConfig spiConfig;
spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
@@ -77,40 +105,6 @@ void bmxInit()
bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
- bmx160 = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
- spiConfig);
-
- SensorInfo info("BMX160", 20, &bmxCallback);
-
- sensorsMap.emplace(std::make_pair(bmx160, info));
-}
-
-void bmxCallback()
-{
- auto data = bmx160->getLastSample();
-
- Vector3f acceleration(data.accelerationX, data.accelerationY,
- data.accelerationZ);
- Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
- data.magneticFieldZ);
-
- // Apply correction
- Vector3f acc_b(0.3763, -0.1445, -0.1010);
- acceleration -= acc_b;
- Vector3f mag_b{21.0730033648425, -24.3997259703105, -2.32621524742862};
- Matrix3f A{{0.659926504672263, 0, 0},
- {0, 0.662442130094073, 0},
- {0, 0, 2.28747567094359}};
- magneticField = (magneticField - mag_b).transpose() * A;
-
- acceleration.normalize();
- magneticField.normalize();
-
- StateInitializer state;
- state.triad(acceleration, magneticField, nedMag);
-
- auto kalmanState = state.getInitX();
- if (!std::isnan(kalmanState(9)))
- printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
- kalmanState(7), kalmanState(8));
+ bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+ spiConfig);
}
diff --git a/src/tests/sensors/calibration/test-calibration-benchmark.cpp b/src/tests/sensors/calibration/test-calibration-benchmark.cpp
index 066536cf73f042b53047cc615ae3ab5c67070f9d..1ed6b0f81de257186fd9c1c858b8648fce157998 100644
--- a/src/tests/sensors/calibration/test-calibration-benchmark.cpp
+++ b/src/tests/sensors/calibration/test-calibration-benchmark.cpp
@@ -21,9 +21,9 @@
*/
/* ACCELEROMETER calibration: please set the chosen one to 1 */
-#define BIAS_CALIBRATION_LOAD_TEST 1
-#define SIX_PARAMETER_CALIBRATION_LOAD_TEST 0
-#define TWELVE_PARAMETER_CALIBRATION_LOAD_TEST 0
+#define BIAS_CALIBRATION_LOAD_TEST
+// #define SIX_PARAMETER_CALIBRATION_LOAD_TEST
+// #define TWELVE_PARAMETER_CALIBRATION_LOAD_TEST
/* Expressed in Hertz: valid values: 1 <= frequency <= 1000 */
#define SAMPLE_FREQUENCY_LOAD_TEST 1000
@@ -35,7 +35,6 @@
#include <sensors/calibration/BiasCalibration.h>
#include <sensors/calibration/HardIronCalibration.h>
#include <sensors/calibration/SixParameterCalibration.h>
-#include <sensors/calibration/SoftIronCalibration.h>
#include <sensors/calibration/TwelveParameterCalibration.h>
#include <utils/Debug.h>
@@ -47,17 +46,17 @@ volatile AccelerometerData testData;
int main()
{
-#if BIAS_CALIBRATION_LOAD_TEST
+#ifdef BIAS_CALIBRATION_LOAD_TEST
BiasCorrector<AccelerometerData> corrector;
TRACE("Using BIAS calibration model.\n");
#endif
-#if SIX_PARAMETER_CALIBRATION_LOAD_TEST
+#ifdef SIX_PARAMETER_CALIBRATION_LOAD_TEST
SixParameterCorrector<AccelerometerData> corrector;
TRACE("Using SIX-PARAMETER calibration model.\n");
#endif
-#if TWELVE_PARAMETER_CALIBRATION_LOAD_TEST
+#ifdef TWELVE_PARAMETER_CALIBRATION_LOAD_TEST
TwelveParameterCorrector<AccelerometerData> corrector;
TRACE("Using TWELVE-PARAMETER calibration model.\n");
#endif
diff --git a/src/tests/sensors/calibration/test-calibration-stats.cpp b/src/tests/sensors/calibration/test-calibration-stats.cpp
index 6c3eccbc251e9fcf6415762d6cf75fde2e984067..40aa29ad31fb57621dc67c00d6c83193c4b6d30b 100644
--- a/src/tests/sensors/calibration/test-calibration-stats.cpp
+++ b/src/tests/sensors/calibration/test-calibration-stats.cpp
@@ -30,6 +30,7 @@
#include <sensors/LIS3DSH/LIS3DSH.h>
#endif
+#include <sensors/calibration/AxisOrientation.h>
#include <sensors/calibration/Calibration.h>
using namespace Boardcore;
diff --git a/src/tests/sensors/calibration/test-calibration.cpp b/src/tests/sensors/calibration/test-calibration.cpp
index 4172dec1c3bface25671de10f8f8a3f9479c511f..9e2429572ddd71e14b969bb8c10f77d473e9a619 100644
--- a/src/tests/sensors/calibration/test-calibration.cpp
+++ b/src/tests/sensors/calibration/test-calibration.cpp
@@ -39,7 +39,6 @@
#include <sensors/calibration/BiasCalibration.h>
#include <sensors/calibration/HardIronCalibration.h>
#include <sensors/calibration/SixParameterCalibration.h>
-#include <sensors/calibration/SoftIronCalibration.h>
#include <sensors/calibration/TwelveParameterCalibration.h>
#include <utils/Debug.h>
diff --git a/src/tests/sensors/test-mpu9250.cpp b/src/tests/sensors/test-mpu9250.cpp
index ba2ad4ea4f8d1687c18554f12cacf41fa892ff35..03e798fa2ac28a1d7c23b334f8c715b67d2db0ae 100644
--- a/src/tests/sensors/test-mpu9250.cpp
+++ b/src/tests/sensors/test-mpu9250.cpp
@@ -53,18 +53,11 @@ void initBoard()
int main()
{
// Enable SPI clock and set gpios
- initBoard();
-
- // SPI configuration setup
-
- SPIBusConfig spiConfig;
- spiConfig.clockDivider = SPI::ClockDivider::DIV_64;
- spiConfig.mode = SPI::Mode::MODE_3;
- SPIBus spiBus(SPI2);
- SPISlave spiSlave(spiBus, csPin, spiConfig);
+ // initBoard();
// Device initialization
- MPU9250 mpu9250(spiSlave);
+ SPIBus spiBus(SPI1);
+ MPU9250 mpu9250(spiBus, sensors::mpu9250::cs::getPin());
// Initialize the device
bool result = mpu9250.init();
@@ -85,7 +78,7 @@ int main()
printf("%lld,%f,%f,%f\n", data.magneticFieldTimestamp,
data.magneticFieldX, data.magneticFieldY, data.magneticFieldZ);
- // Serial communicaion at 115200 baud takes aprx. 10ms
+ // Serial communication at 115200 baud takes approximately 10ms
// miosix::delayMs(10);
}
diff --git a/src/tests/sensors/test-ms5803.cpp b/src/tests/sensors/test-ms5803.cpp
index f3a8bcc1539d5b4eb9a7255f9056a85d043f6fec..4c00318899a0fd596e002c0baa3e350203ecd876 100644
--- a/src/tests/sensors/test-ms5803.cpp
+++ b/src/tests/sensors/test-ms5803.cpp
@@ -35,12 +35,9 @@ using namespace miosix;
int main()
{
- SPIBusConfig spiConfig;
+ // Sample temperature every 10 pressure samples
SPIBus spiBus(SPI1);
- SPISlave spiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), spiConfig);
-
- // Sample temperature every 5 pressure samples
- MS5803 sensor(spiSlave, 10);
+ MS5803 sensor(spiBus, miosix::sensors::ms5803::cs::getPin(), {}, 10);
Thread::sleep(100);
diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp
index 0c611daf4e7ffc2ccabcc56da70d989496e1d45f..24b6cc092a73e531765fbbe4897a29133a827014 100644
--- a/src/tests/sensors/test-ubloxgps.cpp
+++ b/src/tests/sensors/test-ubloxgps.cpp
@@ -69,15 +69,17 @@ int main()
while (true)
{
+ printf("a\n");
// Give time to the thread
Thread::sleep(1000 / RATE);
// Sample
gps.sample();
+ printf("b\n");
dataGPS = gps.getLastSample();
// Print out the latest sample
- TRACE(
+ printf(
"[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
"height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
"track %3.1f\n",