diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4100936c084b7c836ee5e26555face62957c591f..433a59c0551d9149da2a86e27923e3a1c93fec23 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,17 +132,26 @@ 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-nas src/tests/algorithms/NAS/test-nas.cpp)
-sbs_target(test-nas 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-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp)
-sbs_target(test-nas-with-triad stm32f429zi_parafoil)
+add_executable(test-attitude src/tests/algorithms/NAS/test-attitude.cpp)
+sbs_target(test-attitude 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-nas-with-logs src/tests/algorithms/NAS/test-nas-with-logs.cpp)
-sbs_target(test-nas-with-logs 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-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_parafoil)
+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 #
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index efb132cd7d1c500d5150d2e96732e7460bc05c79..e119c361e8a72e7f898725c4b942a720b6b6076e 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -224,7 +224,7 @@ void NAS::correctMag(const Vector3f& mag)
P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
}
-void NAS::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 +249,8 @@ void NAS::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())};
diff --git a/src/shared/algorithms/NAS/NAS.h b/src/shared/algorithms/NAS/NAS.h
index e5752e3ac530ea15b3fafbcfdabb8e6e0a90f3b8..d232f4beb6f6951c1d8ecded27b3aea6c2c9a77f 100644
--- a/src/shared/algorithms/NAS/NAS.h
+++ b/src/shared/algorithms/NAS/NAS.h
@@ -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);
diff --git a/src/shared/utils/MovingAverage.h b/src/shared/utils/MovingAverage.h
index 94e8fda20d5577c21c9de585a6f3081547c2a0e3..a5f64e04f47bb0a93b1df5e7d071a2397fc10cc3 100644
--- a/src/shared/utils/MovingAverage.h
+++ b/src/shared/utils/MovingAverage.h
@@ -48,9 +48,9 @@ public:
private:
T value = 0;
- const int movingAverageN = 20;
- const float movingAverageCoeff = 1 / (float)movingAverageN;
- const float movingAverageCompCoeff = 1 - movingAverageCoeff;
+ int movingAverageN = 20;
+ float movingAverageCoeff = 1 / (float)movingAverageN;
+ float movingAverageCompCoeff = 1 - movingAverageCoeff;
};
} // namespace Boardcore
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/NAS/test-nas.cpp b/src/tests/algorithms/NAS/test-attitude.cpp
similarity index 65%
rename from src/tests/algorithms/NAS/test-nas.cpp
rename to src/tests/algorithms/NAS/test-attitude.cpp
index 1d6e5a5cfcfb155a8ed0f155abf0cb285fdc0a6e..beacab318c26c6005805ca9f7bc24aa7e1147e6b 100644
--- a/src/tests/algorithms/NAS/test-nas.cpp
+++ b/src/tests/algorithms/NAS/test-attitude.cpp
@@ -21,9 +21,9 @@
*/
#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>
@@ -35,26 +35,26 @@ using namespace Eigen;
NASConfig getEKConfig();
void setInitialOrientation();
-void bmxInit();
-void bmxCallback();
+void imuInit();
+void imuStep();
-NAS* 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 NAS(getEKConfig());
+ nas = new NAS(getEKConfig());
setInitialOrientation();
- sensorManager->start();
+ TaskScheduler scheduler;
+ scheduler.addTask(imuStep, 20);
+ scheduler.start();
while (true)
Thread::sleep(1000);
@@ -70,8 +70,8 @@ NASConfig 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 @@ NASConfig 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..eb969d4631f481d59968aa9d5f454da69316752e
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -0,0 +1,185 @@
+/* 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();
+
+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);
+ 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);
+ // std::cout << gpsPos.transpose() << " - ";
+ Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityNorth);
+ // std::cout << gpsVel.transpose() << " - ";
+ 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 (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 &&
+ gpsPos[1] > -1e3)
+ nas->correctGPS(gpsCorrection);
+ nas->correctBaro(100000, 110000, 20 + 273.5);
+
+ // 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",
+ // (float)gpsData.gpsTimestamp / 1000000, gpsData.fix, gpsData.latitude,
+ // gpsData.longitude, gpsData.height, gpsData.satellites, gpsData.speed,
+ // gpsData.velocityNorth, gpsData.velocityEast, gpsData.track);
+
+ auto nasState = nas->getState();
+ // printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+ // nasState.qz);
+ // printf("%f, %f, %f, %f, %f, %f, %f\n", nasState.n, nasState.e,
+ // nasState.d,
+ // nasState.qw, nasState.qx, nasState.qy, nasState.qz);
+ Logger::getInstance().log(imuData);
+ Logger::getInstance().log(gpsData);
+ Logger::getInstance().log(nasState);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-with-logs.cpp b/src/tests/algorithms/NAS/test-nas-with-logs.cpp
deleted file mode 100644
index d85bb459f5c86bb65a1b22635d08ae1b4431072b..0000000000000000000000000000000000000000
--- a/src/tests/algorithms/NAS/test-nas-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/NAS/NAS.h>
-#include <algorithms/NAS/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;
-
-NASConfig getEKConfig();
-NASState readInitialState();
-void updateKalman(BMX160Data data);
-void printState();
-std::istream& operator>>(std::istream& input, BMX160Data& data);
-std::istream& operator>>(std::istream& input, NASState& data);
-
-// Normalized NED magnetic field in Milan
-Vector3f nedMag = Vector3f(0.47338841, 0.02656764, 0.88045305);
-
-NAS* kalman;
-bool triad = false;
-
-int main()
-{
- // Prepare the Kalman
- kalman = new NAS(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();
- }
-}
-
-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;
-
- // Normalized magnetic field in Milan
- config.NED_MAG = nedMag;
-
- return config;
-}
-
-NASState readInitialState()
-{
- auto initialState =
- CSVParser<NASState>("/sd/initial_state.csv", true).collect();
-
- printf("Initial state count: %d\n", initialState.size());
-
- if (initialState.size() == 0)
- return NASState();
- 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>(NAS::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, NASState& 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-nas-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
index 942b7f9d9b248954f0eac29847f35a2eb233327f..59baeb1d1e37a36035e2fe6208ec22617ea8ad2d 100644
--- a/src/tests/algorithms/NAS/test-nas-with-triad.cpp
+++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
@@ -22,12 +22,9 @@
#include <algorithms/NAS/NAS.h>
#include <algorithms/NAS/StateInitializer.h>
-#include <drivers/timer/TimestampTimer.h>
#include <miosix.h>
-#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/BMX160/BMX160.h>
#include <sensors/SensorManager.h>
-#include <sensors/calibration/SensorDataExtra.h>
-#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
#include <utils/SkyQuaternion/SkyQuaternion.h>
#include <iostream>
@@ -45,10 +42,10 @@ constexpr uint64_t CALIBRATION_TIMEOUT = 5 * 1e6;
Vector3f nedMag = Vector3f(0.47472049, 0.02757190, 0.87970463);
StateInitializer* stateInitializer;
-NAS* kalman;
+NAS* nas;
SPIBus spi1(SPI1);
-MPU9250* mpu = nullptr;
+BMX160* bmx160 = nullptr;
Boardcore::SensorManager::SensorMap_t sensorsMap;
Boardcore::SensorManager* sensorManager = nullptr;
@@ -59,7 +56,7 @@ int main()
sensorManager = new SensorManager(sensorsMap);
stateInitializer = new StateInitializer();
- kalman = new NAS(getEKConfig());
+ nas = new NAS(getEKConfig());
// Logger::getInstance().start();
TimestampTimer::getInstance().resetTimestamp();
@@ -97,11 +94,27 @@ NASConfig getEKConfig()
void bmxInit()
{
- mpu = new MPU9250(spi1, miosix::sensors::mpu9250::cs::getPin());
-
- SensorInfo info("MPU9250", 20, &bmxCallback);
-
- sensorsMap.emplace(std::make_pair(mpu, info));
+ 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;
+
+ 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()
@@ -111,19 +124,25 @@ void bmxCallback()
static Vector3f accMean = Vector3f::Zero();
static Vector3f magMean = Vector3f::Zero();
- auto data = mpu->getLastSample();
-
+ auto data = bmx160->getLastSample();
Vector3f acceleration(data.accelerationX, data.accelerationY,
data.accelerationZ);
-
Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
data.angularVelocityZ);
- angularVelocity = angularVelocity * Constants::DEGREES_TO_RADIANS;
-
- SoftAndHardIronCorrector corrector({-5.15221, -56.5428, 38.2193},
- {0.962913, 0.987191, 1.05199});
- Vector3f magneticField;
- magneticField << corrector.correct(data);
+ Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+ data.magneticFieldZ);
+
+ // Apply correction
+ Vector3f acc_b(0.3763 + 0.094, -0.1445 - 0.0229, -0.1010 + 0.0150);
+ acceleration -= acc_b;
+ Vector3f gyro_b{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+ angularVelocity = angularVelocity - gyro_b;
+ angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+ 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;
if (calibrating)
{
@@ -135,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];
@@ -161,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];
@@ -172,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/NAS/test-triad.cpp b/src/tests/algorithms/NAS/test-triad.cpp
index a11b8f3272b8b3a741ae902ab535b1adba91ffc2..f5104a1e0f2cb838a1b14f7b58ec072fbabcdcab 100644
--- a/src/tests/algorithms/NAS/test-triad.cpp
+++ b/src/tests/algorithms/NAS/test-triad.cpp
@@ -22,7 +22,7 @@
#include <algorithms/NAS/StateInitializer.h>
#include <miosix.h>
-#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/BMX160/BMX160.h>
#include <sensors/SensorManager.h>
#include <sensors/calibration/SensorDataExtra.h>
#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
@@ -34,60 +34,77 @@ 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);
-MPU9250* mpu = 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()
{
- mpu = new MPU9250(spi1, miosix::sensors::mpu9250::cs::getPin());
-
- SensorInfo info("MPU9250", 20, &bmxCallback);
+ SPIBusConfig spiConfig;
+ spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
- sensorsMap.emplace(std::make_pair(mpu, info));
-}
-
-void bmxCallback()
-{
- auto data = mpu->getLastSample();
+ BMX160Config bmx_config;
+ bmx_config.fifoMode = BMX160Config::FifoMode::HEADER;
+ bmx_config.fifoWatermark = 80;
+ bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
- Vector3f acceleration(data.accelerationX, data.accelerationY,
- data.accelerationZ);
+ bmx_config.temperatureDivider = 1;
- SoftAndHardIronCorrector corrector({-5.15221, -56.5428, 38.2193},
- {0.962913, 0.987191, 1.05199});
- Vector3f magneticField;
- magneticField << (MagnetometerData)data;
+ bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
- magneticField << corrector.correct(data);
+ bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
- acceleration.normalize();
- magneticField.normalize();
+ bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+ bmx_config.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100;
+ bmx_config.magnetometerRate = BMX160Config::OutputDataRate::HZ_100;
- StateInitializer state;
- state.triad(acceleration, magneticField, nedMag);
+ bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
- std::cout << acceleration.transpose();
- 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);
}