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); }