diff --git a/CMakeLists.txt b/CMakeLists.txt index 433a59c0551d9149da2a86e27923e3a1c93fec23..1c07b46355e37135c849ff6faa1c81f512fb09d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -135,15 +135,18 @@ sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery) add_executable(test-tmp src/tests/algorithms/NAS/test-tmp.cpp) sbs_target(test-tmp stm32f429zi_skyward_death_stack_x) -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-attitude-stack src/tests/algorithms/NAS/test-attitude-stack.cpp) +sbs_target(test-attitude-stack 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-stack src/tests/algorithms/NAS/test-nas-stack.cpp) +sbs_target(test-nas-stack 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_skyward_death_stack_x) @@ -347,7 +350,7 @@ 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/src/tests/algorithms/NAS/test-attitude.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp similarity index 100% rename from src/tests/algorithms/NAS/test-attitude.cpp rename to src/tests/algorithms/NAS/test-attitude-stack.cpp diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp index eb969d4631f481d59968aa9d5f454da69316752e..57fd167c839ec3bdef2d24c69417ddb7f118aab1 100644 --- a/src/tests/algorithms/NAS/test-nas-parafoil.cpp +++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp @@ -40,6 +40,7 @@ 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); @@ -58,7 +59,8 @@ int main() setInitialOrientation(); TaskScheduler scheduler; - scheduler.addTask(step, 20); + scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER); + scheduler.addTask(print, 250); scheduler.start(); while (true) @@ -131,9 +133,7 @@ void step() 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; @@ -158,28 +158,26 @@ void step() // 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) + acceleration.normalize(); + nas->correctAcc(acceleration); + if (gpsData.fix) 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); } + +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/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",