diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 3f878454cccbfc2b3c134578294a896f6e53c46e..0f530eb0bb42c7e438451bfb5f7c8f48ac500d1f 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -131,7 +131,7 @@ "defines": [ "DEBUG", "_ARCH_CORTEXM4_STM32F4", - "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X", + "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X", "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x", "HSE_VALUE=8000000", "SYSCLK_FREQ_168MHz=168000000", @@ -195,7 +195,7 @@ "DEBUG", "HILSimulation", "_ARCH_CORTEXM4_STM32F4", - "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X_MAKER_FAIRE", + "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X_MAKER_FAIRE", "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire", "HSE_VALUE=8000000", "SYSCLK_FREQ_168MHz=168000000", diff --git a/.vscode/settings.json b/.vscode/settings.json index 743392d0cbad610129652fe8bcf299b7831f3e7f..85d459346d971d2af82abac3ea5ef87f7c240860 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -113,7 +113,8 @@ "xstring": "cpp", "xtr1common": "cpp", "xtree": "cpp", - "xutility": "cpp" + "xutility": "cpp", + "BiasCorrector.C": "cpp" }, "editor.formatOnSave": true, "[c]": { @@ -152,15 +153,18 @@ "GPIOF", "KALM", "kalman", + "magcal", "mavlink", "miosix", "MPXH", "Nidasio", "pitot", + "setb", "Setpoint", "stateinitializer", "telecommands", "TMTC", + "tparam", "UBXGPS", "usart", "VREF", @@ -171,4 +175,4 @@ "CMAKE_C_COMPILER_LAUNCHER": "ccache", "CMAKE_CXX_COMPILER_LAUNCHER": "ccache" } -} \ No newline at end of file +} diff --git a/CMakeLists.txt b/CMakeLists.txt index 859b26d6f03a33685ddb376ac00fb9c68709b112..73e684ae99af289299c879863a71a4a91ea4cf8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,7 @@ sbs_target(main-entry-roccaraso stm32f429zi_skyward_death_stack_v3) add_executable(main-entry-milano src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER}) target_include_directories(main-entry-milano PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(main-entry-milano PRIVATE MILANO) +target_compile_definitions(main-entry-milano PRIVATE MILANO BUZZER_LOW INTERP USE_SERIAL_TRANSCEIVER) sbs_target(main-entry-milano stm32f429zi_skyward_death_stack_v3) add_executable(main-entry-hil-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL}) diff --git a/skyward-boardcore b/skyward-boardcore index e60ca34f7557877779f3da84d33a76ca0e1d84d5..a757b8dc34e0b160ddaf3ed33408d5aa2a22a4fc 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit e60ca34f7557877779f3da84d33a76ca0e1d84d5 +Subproject commit a757b8dc34e0b160ddaf3ed33408d5aa2a22a4fc diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h index 0da418ccf9fdd0b1620e72bbb034244703d45576..445449e860808815818a0728857c88fda7cd5ca6 100644 --- a/src/boards/Main/Configs/NASConfig.h +++ b/src/boards/Main/Configs/NASConfig.h @@ -41,6 +41,7 @@ static const Boardcore::NASConfig config = { UPDATE_PERIOD / 1000.0, // T 0.0001f, // SIGMA_BETA 0.3f, // SIGMA_W + 0.1f, // SIGMA_ACC 0.1f, // SIGMA_MAG 10.0f, // SIGMA_GPS 4.3f, // SIGMA_BAR diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h index 3b5cf64b9518fe37d833d9d003feb607345036b1..1d886dbe0612a1ac3f55140e5ec869470acdebaf 100644 --- a/src/boards/Main/Configs/SensorsConfig.h +++ b/src/boards/Main/Configs/SensorsConfig.h @@ -29,7 +29,6 @@ #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/MPU9250/MPU9250.h> #include <sensors/calibration/AxisOrientation.h> -#include <sensors/calibration/Calibration.h> namespace Main { diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h index ac0227e53c2ce4dcc6a143fdc0b23850ce226bb8..81fd67b5873f38df541a5f00d0ddb226a6439a81 100644 --- a/src/boards/Main/Sensors/Sensors.h +++ b/src/boards/Main/Sensors/Sensors.h @@ -64,12 +64,9 @@ public: */ struct StateComplete { - HILAccelerometer *accelerometer; HILBarometer *barometer; HILPitot *pitot; HILGps *gps; - HILGyroscope *gyro; - HILMagnetometer *magnetometer; HILTemperature *temperature; HILImu *imu; HILKalman *kalman; diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp index a212199ae2ec6846e83acf3aed227fb95f853df9..4fa5114b71bf861447b5cc075072a464035d4122 100644 --- a/src/boards/Main/StateMachines/NASController/NASController.cpp +++ b/src/boards/Main/StateMachines/NASController/NASController.cpp @@ -72,6 +72,9 @@ void NASController::update() nas.correctGPS(gpsData); nas.correctBaro(pressureData.pressure); + // TODO: Add accelerometer correction until the acceleration goes out of + // specs + Logger::getInstance().log(nas.getState()); FlightStatsRecorder::getInstance().update(nas.getState()); } diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp index f0f5c087ea9cba36ee14b78f1c78df548a54c208..83e639981ba8f11429036ab1eb6c804d5ea54715 100644 --- a/src/boards/Main/TMRepository/TMRepository.cpp +++ b/src/boards/Main/TMRepository/TMRepository.cpp @@ -35,6 +35,8 @@ #include <Main/StateMachines/Deployment/Deployment.h> #include <Main/StateMachines/FlightModeManager/FlightModeManager.h> #include <Main/StateMachines/NASController/NASController.h> +#include <algorithms/NAS/StateInitializer.h> +#include <common/ReferenceConfig.h> #include <diagnostic/CpuMeter/CpuMeter.h> #include <drivers/timer/TimestampTimer.h> #include <events/EventBroker.h> @@ -276,14 +278,24 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId, tm.nas_vn = nasState.vn; tm.nas_ve = nasState.ve; tm.nas_vd = nasState.vd; - tm.nas_qx = nasState.qx; - tm.nas_qy = nasState.qy; - tm.nas_qz = nasState.qz; - tm.nas_qw = nasState.qw; tm.nas_bias_x = nasState.bx; tm.nas_bias_y = nasState.by; tm.nas_bias_z = nasState.bz; + // Test triad + StateInitializer state; + state.triad({imuData.accelerationX, imuData.accelerationY, + imuData.accelerationZ}, + {imuData.magneticFieldX, imuData.magneticFieldY, + imuData.magneticFieldZ}, + Common::ReferenceConfig::nedMag); + auto testState = state.getInitX(); + + tm.nas_qw = testState(9); + tm.nas_qx = testState(6); + tm.nas_qy = testState(7); + tm.nas_qz = testState(8); + // Sensing pins statuses tm.pin_launch = PinHandler::getInstance().getPinsData()[LAUNCH_PIN].lastState; diff --git a/src/boards/Parafoil/Configs/NASConfig.h b/src/boards/Parafoil/Configs/NASConfig.h index 66ffb9fd50b0d6c4ead4f2febd75c0b16b947dc4..1c075cd60edb10bdf4012759a1ab3006757a8f7d 100644 --- a/src/boards/Parafoil/Configs/NASConfig.h +++ b/src/boards/Parafoil/Configs/NASConfig.h @@ -39,6 +39,7 @@ static const Boardcore::NASConfig config = { 1.0f / UPDATE_PERIOD, // T 0.0001f, // SIGMA_BETA 0.3f, // SIGMA_W + 0.1f, // SIGMA_ACC 0.1f, // SIGMA_MAG 10.0f, // SIGMA_GPS 4.3f, // SIGMA_BAR diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h index c99dde138646802d2e55355e44f9e2fa946338c4..dda4d519f9dd5e44993ac32457e56d15f69033c2 100644 --- a/src/boards/Payload/Configs/NASConfig.h +++ b/src/boards/Payload/Configs/NASConfig.h @@ -41,6 +41,7 @@ static const Boardcore::NASConfig config = { UPDATE_PERIOD / 1000.0, // T 0.0001f, // SIGMA_BETA 0.3f, // SIGMA_W + 0.1f, // SIGMA_ACC 0.1f, // SIGMA_MAG 10.0f, // SIGMA_GPS 4.3f, // SIGMA_BAR diff --git a/src/boards/Payload/Configs/SensorsConfig.h b/src/boards/Payload/Configs/SensorsConfig.h index 57f31660a3a3f578463a6877978ee87423f418a3..2c96b7af450e46eed1c4541b9a393bb232a9233c 100644 --- a/src/boards/Payload/Configs/SensorsConfig.h +++ b/src/boards/Payload/Configs/SensorsConfig.h @@ -27,7 +27,6 @@ #include <sensors/BMX160/BMX160Config.h> #include <sensors/LIS3MDL/LIS3MDL.h> #include <sensors/calibration/AxisOrientation.h> -#include <sensors/calibration/Calibration.h> namespace Payload { diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp index fc476a5892740fe3c2e4ae61a2e11a94b7ff3d44..99617f16397dffcfe2ff6b37aedd2e27728a7689 100644 --- a/src/boards/Payload/Sensors/Sensors.cpp +++ b/src/boards/Payload/Sensors/Sensors.cpp @@ -384,13 +384,8 @@ void Sensors::bmx160Callback() void Sensors::bmx160WithCorrectionInit() { - // Read the correction parameters - BMX160CorrectionParameters correctionParameters = - BMX160WithCorrection::readCorrectionParametersFromFile( - BMX160_CORRECTION_PARAMETERS_FILE); - - bmx160WithCorrection = new BMX160WithCorrection( - bmx160, correctionParameters, BMX160_AXIS_ROTATION); + bmx160WithCorrection = + new BMX160WithCorrection(bmx160, BMX160_AXIS_ROTATION); SensorInfo info( "BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX, diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index 8ff21963b71aa4b429713de80c47f6dfd2358c9d..380bc2c0b51dd68c2d40d488abfffc986e26b73b 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -70,6 +70,9 @@ void NASController::update() nas.correctGPS(gpsData); nas.correctBaro(pressureData.pressure); + // TODO: Add accelerometer correction until the acceleration goes out of + // specs + Logger::getInstance().log(nas.getState()); FlightStatsRecorder::getInstance().update(nas.getState()); } diff --git a/src/tests/Main/calibration/test-bmx160-calibration.cpp b/src/tests/Main/calibration/test-bmx160-calibration.cpp index 1fc701e86ca8202509c47e616f74ef6c51aaaa30..07f5f0b323369418582b7a5a49374f04195fa5f3 100644 --- a/src/tests/Main/calibration/test-bmx160-calibration.cpp +++ b/src/tests/Main/calibration/test-bmx160-calibration.cpp @@ -26,6 +26,8 @@ #include <sensors/BMX160/BMX160.h> #include <sensors/BMX160/BMX160WithCorrection.h> #include <sensors/calibration/AxisOrientation.h> +#include <sensors/calibration/BiasCalibration/BiasCalibration.h> +#include <sensors/calibration/SixParameterCalibration/SixParameterCalibration.h> #include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h> #include <utils/Debug.h> @@ -34,18 +36,21 @@ using namespace std; using namespace miosix; +using namespace Eigen; using namespace Boardcore; constexpr const char* CORRECTION_PARAMETER_FILE = "/sd/bmx160_params.csv"; constexpr const char* MAG_CALIBRATION_DATA_FILE = "/sd/bmx160_mag_calibration_data.csv"; -constexpr int ACC_CALIBRATION_SAMPLES = 200; -constexpr int ACC_CALIBRATION_SLEEP_TIME = 25; // [us] + +constexpr int ACC_CALIBRATION_SLEEP_TIME = 10; // [s] constexpr int ACC_CALIBRATION_N_ORIENTATIONS = 6; -constexpr int MAG_CALIBRATION_DURATION = 10; // [s] +constexpr int MAG_CALIBRATION_DURATION = 30; // [s] constexpr uint32_t MAG_CALIBRATION_SAMPLE_PERIOD = 20; // [ms] +constexpr int GYRO_CALIBRATION_DURATION = 10; // [s] + BMX160* bmx160; /** @@ -62,20 +67,30 @@ BMX160* bmx160; */ AxisOrthoOrientation orientations[ACC_CALIBRATION_N_ORIENTATIONS]{ {Direction::POSITIVE_X, Direction::POSITIVE_Y}, // Z up - {Direction::POSITIVE_Y, Direction::POSITIVE_Z}, // X up - {Direction::POSITIVE_Z, Direction::POSITIVE_X}, // Y up - {Direction::POSITIVE_Y, Direction::POSITIVE_X}, // Z down - {Direction::POSITIVE_Z, Direction::POSITIVE_Y}, // X down - {Direction::POSITIVE_X, Direction::POSITIVE_Z}, // Y down + {Direction::POSITIVE_Z, Direction::POSITIVE_Y}, // X up + {Direction::POSITIVE_X, Direction::POSITIVE_Z}, // Y up + {Direction::POSITIVE_X, Direction::NEGATIVE_Y}, // Z down + {Direction::NEGATIVE_Z, Direction::POSITIVE_Y}, // X down + {Direction::POSITIVE_X, Direction::NEGATIVE_Z}, // Y down }; constexpr const char* testHumanFriendlyDirection[]{ "Z up", "X up", "Y up", "Z down", "X down", "Y down", }; -// BMX160 Watermark interrupt +#if defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X) +SPIBus bus(SPI1); + +void __attribute__((used)) EXTI5_IRQHandlerImpl() +{ +#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3) +SPIBus bus(SPI4); + void __attribute__((used)) EXTI3_IRQHandlerImpl() { +#else +#error "Board not supported" +#endif if (bmx160) bmx160->IRQupdateTimestamp(TimestampTimer::getTimestamp()); } @@ -83,47 +98,45 @@ void __attribute__((used)) EXTI3_IRQHandlerImpl() int menu(); void waitForInput(); -BMX160CorrectionParameters calibrateAccelerometer( - BMX160CorrectionParameters correctionParameters); -BMX160CorrectionParameters calibrateMagnetometer( - BMX160CorrectionParameters correctionParameters); +void calibrateAccelerometer(); +void calibrateMagnetometer(); +void calibrateGyroscope(); int main() { - // Enable interrupt from BMX pin - enableExternalInterrupt(GPIOE_BASE, 3, InterruptTrigger::FALLING_EDGE); +// Enable interrupt from BMX pin +#if defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X) + enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(), + miosix::sensors::bmx160::intr::getPin().getNumber(), + InterruptTrigger::FALLING_EDGE); +#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3) + enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(), + miosix::sensors::bmx160::intr::getPin().getNumber(), + InterruptTrigger::FALLING_EDGE); +#else +#error "Board not supported" +#endif // Greet the user printf("\nWelcome to the calibration procedure!\n"); - // Read the current correction parameters - BMX160CorrectionParameters correctionParameters; - correctionParameters = - BMX160WithCorrection::readCorrectionParametersFromFile( - CORRECTION_PARAMETER_FILE); - // Make the user choose what to do switch (menu()) { case 1: - correctionParameters = calibrateAccelerometer(correctionParameters); + calibrateAccelerometer(); break; case 2: - correctionParameters = calibrateMagnetometer(correctionParameters); + calibrateMagnetometer(); + break; + case 3: + calibrateGyroscope(); break; default: break; } - // Save the correction parameters in the file - { - std::ofstream correctionParametersFile(CORRECTION_PARAMETER_FILE); - correctionParametersFile << BMX160CorrectionParameters::header() - << std::endl; - correctionParameters.print(correctionParametersFile); - } - return 0; } @@ -134,6 +147,7 @@ int menu() printf("\nWhat do you want to do?\n"); printf("1. Calibrate accelerometer\n"); printf("2. Calibrate magnetometer\n"); + printf("3. Calibrate gyroscope\n"); printf("\n>> "); scanf("%d", &choice); @@ -150,25 +164,16 @@ void waitForInput() } while (temp != "c"); } -/** - * The accelerometer calibration consists in finding the bias and coefficient - * against target 9.8m/s^2 on the three axis positioning the sensor in all the 6 - * positions (+x, -x, +y, -y, +z, -z). - */ -BMX160CorrectionParameters calibrateAccelerometer( - BMX160CorrectionParameters correctionParameters) +void calibrateAccelerometer() { - Eigen::Matrix<float, 3, 2> m; - auto* calibrationModel = - new SixParameterCalibration<AccelerometerData, - ACC_CALIBRATION_SAMPLES * - ACC_CALIBRATION_N_ORIENTATIONS>; + SixParametersCorrector correction; + correction.fromFile("/sd/bmx160_accelerometer_correction.csv"); - SPIBus bus(SPI4); + SixParameterCalibration calibrationModel({0, 0, 9.8}); BMX160Config bmxConfig; bmxConfig.fifoMode = BMX160Config::FifoMode::HEADER; - bmxConfig.fifoWatermark = 100; + bmxConfig.fifoWatermark = 20; bmxConfig.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; bmxConfig.temperatureDivider = 0; @@ -176,58 +181,38 @@ BMX160CorrectionParameters calibrateAccelerometer( bmxConfig.accelerometerRange = BMX160Config::AccelerometerRange::G_16; bmxConfig.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_2000; - bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_1600; - bmxConfig.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_1600; - bmxConfig.magnetometerRate = BMX160Config::OutputDataRate::HZ_50; + bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; bmxConfig.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; bmx160 = new BMX160(bus, miosix::sensors::bmx160::cs::getPin(), bmxConfig); - TRACE("Initializing BMX160...\n"); + printf("Initializing BMX160...\n"); if (!bmx160->init()) - { - TRACE("Init failed! (code: %d)\n", bmx160->getLastError()); - return correctionParameters; - } + printf("Init failed! (code: %d)\n", bmx160->getLastError()); - TRACE("Performing self-test...\n"); + printf("Performing self-test...\n"); if (!bmx160->selfTest()) - { - TRACE("Self-test failed! (code: %d)\n", bmx160->getLastError()); - return correctionParameters; - } - - TRACE("Initialization and self-test completed!\n"); + printf("Self-test failed! (code: %d)\n", bmx160->getLastError()); - Eigen::Vector3f avgAccel, vec; - int fifoAccSampleCount; - uint64_t accelTimestamp = 0; - - // Set reference vector, 1g on third axis - calibrationModel->setReferenceVector({0, 0, 9.8}); + printf("Initialization and self-test completed!\n"); // Show the user the current correction values - printf("\nCurrent bias vector\n"); - printf("b = [ % 2.5f % 2.5f % 2.5f ]\n\n", - correctionParameters.accelParams(0, 1), - correctionParameters.accelParams(1, 1), - correctionParameters.accelParams(2, 1)); - printf("Matrix to be multiplied to the input vector\n"); - printf(" | % 2.5f % 2.5f % 2.5f |\n", - correctionParameters.accelParams(0, 0), 0.f, 0.f); - printf("M = | % 2.5f % 2.5f % 2.5f |\n", 0.f, - correctionParameters.accelParams(1, 0), 0.f); - printf(" | % 2.5f % 2.5f % 2.5f |\n", 0.f, 0.f, - correctionParameters.accelParams(2, 0)); + printf("Current correction parameters:\n"); + printf("A = |% 2.5f % 2.5f % 2.5f|\n\n", correction.getA()(0), + correction.getA()(1), correction.getA()(2)); + printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", correction.getb()(0), + correction.getb()(1), correction.getb()(2)); waitForInput(); printf( - "Please note that the BMX axis, viewed facing the death stack x, " - "are as follows:\n"); + "Please note that the BMX axis, viewed facing the ELC bay, are as " + "follows:\n"); printf(" z x\n"); printf(" ^ ^\n"); printf(" | /\n"); @@ -247,70 +232,52 @@ BMX160CorrectionParameters calibrateAccelerometer( printf("Reding data and feeding the model...\n"); - // Sample the sensor and feed the data to the calibration model - int count = 0; - while (count < ACC_CALIBRATION_SAMPLES) - { - bmx160->sample(); - avgAccel = {0, 0, 0}; - - // Read all the data contained in the fifo - uint8_t size = bmx160->getLastFifoSize(); - fifoAccSampleCount = 0; - for (int ii = 0; ii < size; ii++) + TaskScheduler scheduler; + scheduler.addTask( + [&]() { - BMX160Data fifoElement = bmx160->getFifoElement(ii); + bmx160->sample(); - // Read acceleration data - if (fifoElement.accelerationTimestamp > accelTimestamp) - { - static_cast<AccelerometerData>(fifoElement) >> vec; - avgAccel += vec; + uint8_t fifoSize = bmx160->getLastFifoSize(); + auto& fifo = bmx160->getLastFifo(); - accelTimestamp = fifoElement.accelerationTimestamp; - fifoAccSampleCount++; + for (uint8_t ii = 0; ii < fifoSize; ii++) + { + Logger::getInstance().log(fifo.at(ii)); + calibrationModel.feed( + static_cast<AccelerometerData>(fifo.at(ii)), + orientations[i]); } - } - avgAccel /= fifoAccSampleCount; - - AccelerometerData data; - data << avgAccel; + }, + 200); + Logger::getInstance().start(); + scheduler.start(); - miosix::Thread::sleep(ACC_CALIBRATION_SLEEP_TIME); + Thread::sleep(ACC_CALIBRATION_SLEEP_TIME * 1000); - calibrationModel->feed(data, orientations[i]); - count++; - } - - printf("Orientation calibration completed\n"); + scheduler.stop(); + Logger::getInstance().stop(); } - printf("Computing the result...."); - SixParameterCorrector<AccelerometerData> corrector = - calibrationModel->computeResult(); - - corrector >> m; - corrector >> correctionParameters.accelParams; + printf("Computing the result....\n"); + auto newCorrector = calibrationModel.computeResult(); - printf("Bias vector\n"); - printf("b = [ % 2.5f % 2.5f % 2.5f ]\n\n", m(0, 1), m(1, 1), - m(2, 1)); - printf("Matrix to be multiplied to the input vector\n"); - printf(" | % 2.5f % 2.5f % 2.5f |\n", m(0, 0), 0.f, 0.f); - printf("M = | % 2.5f % 2.5f % 2.5f |\n", 0.f, m(1, 0), 0.f); - printf(" | % 2.5f % 2.5f % 2.5f |\n", 0.f, 0.f, m(2, 0)); + printf("New correction parameters:\n"); + printf("A = |% 2.5f % 2.5f % 2.5f|\n\n", newCorrector.getA()(0), + newCorrector.getA()(1), newCorrector.getA()(2)); + printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", newCorrector.getb()(0), + newCorrector.getb()(1), newCorrector.getb()(2)); - return correctionParameters; + newCorrector.toFile("/sd/bmx160_accelerometer_correction.csv"); } -BMX160CorrectionParameters calibrateMagnetometer( - BMX160CorrectionParameters correctionParameters) +void calibrateMagnetometer() { - Eigen::Matrix<float, 3, 2> m; - auto* calibrationModel = new SoftAndHardIronCalibration; - Eigen::Vector3f avgMag{0, 0, 0}, vec; + SixParametersCorrector corrector; + corrector.fromFile("/sd/bmx160_magnetometer_correction.csv"); - SPIBus bus(SPI4); + SoftAndHardIronCalibration calibrationModel; + Vector3f avgMag{0, 0, 0}, vec; BMX160Config bmxConfig; bmxConfig.fifoMode = BMX160Config::FifoMode::HEADER; @@ -330,37 +297,24 @@ BMX160CorrectionParameters calibrateMagnetometer( bmx160 = new BMX160(bus, miosix::sensors::bmx160::cs::getPin(), bmxConfig); - TRACE("Initializing BMX160...\n"); + printf("Initializing BMX160...\n"); if (!bmx160->init()) - { - TRACE("Init failed! (code: %d)\n", bmx160->getLastError()); - return correctionParameters; - } + printf("Init failed! (code: %d)\n", bmx160->getLastError()); - TRACE("Performing self-test...\n"); + printf("Performing self-test...\n"); if (!bmx160->selfTest()) - { - TRACE("Self-test failed! (code: %d)\n", bmx160->getLastError()); - return correctionParameters; - } + printf("Self-test failed! (code: %d)\n", bmx160->getLastError()); - TRACE("Initialization and self-test completed!\n"); + printf("Initialization and self-test completed!\n"); // Show the user the current correction values - printf("Current bias vector\n"); - printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", - correctionParameters.magnetoParams(0, 1), - correctionParameters.magnetoParams(1, 1), - correctionParameters.magnetoParams(2, 1)); - printf("Matrix to be multiplied to the input vector\n"); - printf(" |% 2.5f % 2.5f % 2.5f|\n", - correctionParameters.magnetoParams(0, 0), 0.f, 0.f); - printf("M = |% 2.5f % 2.5f % 2.5f|\n", 0.f, - correctionParameters.magnetoParams(1, 0), 0.f); - printf(" |% 2.5f % 2.5f % 2.5f|\n", 0.f, 0.f, - correctionParameters.magnetoParams(2, 0)); + printf("Current correction parameters:\n"); + printf("A = |% 2.5f % 2.5f % 2.5f|\n\n", corrector.getA()(0), + corrector.getA()(1), corrector.getA()(2)); + printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", corrector.getb()(0), + corrector.getb()(1), corrector.getb()(2)); printf("Now I will calibrate the magnetometer\n"); printf( @@ -386,7 +340,7 @@ BMX160CorrectionParameters calibrateMagnetometer( for (uint8_t i = 0; i < fifoSize; i++) { Logger::getInstance().log(fifo.at(i)); - calibrationModel->feed(fifo.at(i)); + calibrationModel.feed(fifo.at(i)); } }, 200); @@ -399,17 +353,89 @@ BMX160CorrectionParameters calibrateMagnetometer( Logger::getInstance().stop(); printf("Computing the result....\n"); - auto corrector = calibrationModel->computeResult(); + auto newCorrector = calibrationModel.computeResult(); + + printf("New correction parameters:\n"); + printf("A = |% 2.5f % 2.5f % 2.5f|\n\n", newCorrector.getA()(0), + newCorrector.getA()(1), newCorrector.getA()(2)); + printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", newCorrector.getb()(0), + newCorrector.getb()(1), newCorrector.getb()(2)); + + newCorrector.toFile("/sd/bmx160_magnetometer_correction.csv"); +} + +void calibrateGyroscope() +{ + BiasCalibration calibrationModel; + int count = 0; + + BMX160Config bmxConfig; + bmxConfig.fifoMode = BMX160Config::FifoMode::HEADER; + bmxConfig.fifoWatermark = 20; + bmxConfig.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1; + + bmxConfig.temperatureDivider = 0; + + bmxConfig.accelerometerRange = BMX160Config::AccelerometerRange::G_16; + bmxConfig.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_2000; + + bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.gyroscopeDataRate = BMX160Config::OutputDataRate::HZ_100; + bmxConfig.magnetometerRate = BMX160Config::OutputDataRate::HZ_100; + + bmxConfig.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD; + + bmx160 = new BMX160(bus, miosix::sensors::bmx160::cs::getPin(), bmxConfig); + + printf("Initializing BMX160...\n"); + + if (!bmx160->init()) + printf("Init failed! (code: %d)\n", bmx160->getLastError()); + + printf("Performing self-test...\n"); + + if (!bmx160->selfTest()) + printf("Self-test failed! (code: %d)\n", bmx160->getLastError()); + + printf("Initialization and self-test completed!\n"); + + printf( + "Now starting the gyroscope calibration, leave the stack perfectly " + "still\n"); - corrector >> m; - corrector >> correctionParameters.magnetoParams; + TaskScheduler scheduler; + scheduler.addTask( + [&]() + { + bmx160->sample(); + + uint8_t fifoSize = bmx160->getLastFifoSize(); + auto& fifo = bmx160->getLastFifo(); + + for (uint8_t i = 0; i < fifoSize; i++) + { + auto data = fifo.at(i); + Logger::getInstance().log(data); + + calibrationModel.feed({data.angularVelocityX, + data.angularVelocityY, + data.angularVelocityZ}); + count++; + } + }, + 200); + + Logger::getInstance().start(); + scheduler.start(); + + Thread::sleep(GYRO_CALIBRATION_DURATION * 1000); + + scheduler.stop(); + Logger::getInstance().stop(); - printf("b: the bias vector\n"); - printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", m(0, 1), m(1, 1), m(2, 1)); - printf("M: the matrix to be multiplied to the input vector\n"); - printf(" |% 2.5f % 2.5f % 2.5f|\n", m(0, 0), 0.f, 0.f); - printf("M = |% 2.5f % 2.5f % 2.5f|\n", 0.f, m(1, 0), 0.f); - printf(" |% 2.5f % 2.5f % 2.5f|\n", 0.f, 0.f, m(2, 0)); + auto corrector = calibrationModel.computeResult(); - return correctionParameters; + printf("New correction parameters:\n"); + printf("b = |% 2.5f % 2.5f % 2.5f|\n\n", corrector.getb()(0), + corrector.getb()(1), corrector.getb()(2)); } diff --git a/src/tests/Main/test-nas.cpp b/src/tests/Main/test-nas.cpp index ab81cf0c51367e93acbd0f72caced349ce7dac71..d9849ad79ffe3cb3806a07ddea357b08acf63fb2 100644 --- a/src/tests/Main/test-nas.cpp +++ b/src/tests/Main/test-nas.cpp @@ -61,8 +61,7 @@ int main() { init(); - nas = new NAS(getEKConfig()); - nas->setReferenceValues({0, 0, 0, 110000, 20 + 273.5}); + nas = new NAS(getEKConfig()); stateInitializer = new StateInitializer(); setInitialOrientation(); @@ -82,6 +81,7 @@ NASConfig getEKConfig() config.T = 0.02f; config.SIGMA_BETA = 0.0001f; config.SIGMA_W = 0.3f; + config.SIGMA_ACC = 0.1f; config.SIGMA_MAG = 0.1f; config.SIGMA_GPS = 10.0f; config.SIGMA_BAR = 4.3f; @@ -166,24 +166,11 @@ void step() // Calibration { - Vector3f offset{-0.0082, 0.0036, 0.0131}; - angularVelocity = angularVelocity - offset; - Vector3f b{9.41150, -6.49408, 19.60433}; - Matrix3f A{{0.66017, 0, 0}, {0, 0.66299, 0}, {0, 0, 2.28477}}; - magneticField = (magneticField + b).transpose() * A; - } + angularVelocity -= Vector3f{-0.00863, 0.00337, 0.01284}; - // Rotate - { - acceleration(0) = -acceleration(0); - acceleration(1) = acceleration(1); - acceleration(2) = -acceleration(2); - angularVelocity(0) = -angularVelocity(0); - angularVelocity(1) = angularVelocity(1); - angularVelocity(2) = -angularVelocity(2); - magneticField(0) = -magneticField(0); - magneticField(1) = magneticField(1); - magneticField(2) = -magneticField(2); + Matrix3f A{{0.73726, 0, 0}, {0, 0.59599, 0}, {0, 0, 2.27584}}; + Vector3f b{39.22325, -17.47903, -13.81505}; + magneticField = (magneticField - b).transpose() * A; } if (calibrating) @@ -209,13 +196,15 @@ void step() // Predict step nas->predictGyro(angularVelocity); - // nas->predictAcc(acceleration); + nas->predictAcc(acceleration); // Correct step magneticField.normalize(); nas->correctMag(magneticField); acceleration.normalize(); nas->correctAcc(acceleration); + nas->correctGPS( + Vector4f{Constants::B21_LATITUDE, Constants::B21_LONGITUDE, 0, 0}); nas->correctBaro(100000); // auto nasState = nas->getState();