From 09495c5168b800ae6cba6c9baf079293bf8f11d9 Mon Sep 17 00:00:00 2001 From: Alberto Nidasio <alberto.nidasio@skywarder.eu> Date: Sun, 9 Oct 2022 16:21:51 +0200 Subject: [PATCH] [NAS] Implemented acceleration correction with threshold --- src/boards/Main/Configs/NASConfig.h | 2 ++ .../StateMachines/NASController/NASController.cpp | 12 +++++++++--- .../StateMachines/NASController/NASController.h | 1 + src/boards/Payload/Configs/NASConfig.h | 2 ++ .../StateMachines/NASController/NASController.cpp | 14 +++++++++++--- .../StateMachines/NASController/NASController.h | 1 + 6 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h index 445449e86..115838e0c 100644 --- a/src/boards/Main/Configs/NASConfig.h +++ b/src/boards/Main/Configs/NASConfig.h @@ -37,6 +37,8 @@ constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz constexpr int CALIBRATION_SAMPLES_COUNT = 20; constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms] +constexpr float ACCELERATION_THRESHOLD = 0.5; // [m/s^2] + static const Boardcore::NASConfig config = { UPDATE_PERIOD / 1000.0, // T 0.0001f, // SIGMA_BETA diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp index 166ec0f67..056adbc1b 100644 --- a/src/boards/Main/StateMachines/NASController/NASController.cpp +++ b/src/boards/Main/StateMachines/NASController/NASController.cpp @@ -72,8 +72,13 @@ void NASController::update() nas.correctGPS(gpsData); nas.correctBaro(pressureData.pressure); - // TODO: Add accelerometer correction until the acceleration goes out of - // specs + // Correct with accelerometer if the acceleration is in specs + Vector3f acceleration = static_cast<AccelerometerData>(imuData); + if (acceleration.norm() > (9.8 + ACCELERATION_THRESHOLD) || + acceleration.norm() < (9.8 - ACCELERATION_THRESHOLD)) + accelerationValid = false; + if (accelerationValid) + nas.correctAcc(imuData); Logger::getInstance().log(nas.getState()); FlightStatsRecorder::getInstance().update(nas.getState()); @@ -86,7 +91,8 @@ void NASController::update() void NASController::calibrate() { - Vector3f acceleration, magneticField; + Vector3f acceleration = Vector3f::Zero(); + Vector3f magneticField = Vector3f::Zero(); Stats pressure; for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++) diff --git a/src/boards/Main/StateMachines/NASController/NASController.h b/src/boards/Main/StateMachines/NASController/NASController.h index 0f64d5efc..6ae8b821f 100644 --- a/src/boards/Main/StateMachines/NASController/NASController.h +++ b/src/boards/Main/StateMachines/NASController/NASController.h @@ -74,6 +74,7 @@ private: Boardcore::NAS nas; Eigen::Vector3f initialOrientation; + bool accelerationValid = true; Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("nas"); diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h index dda4d519f..450f4ea8a 100644 --- a/src/boards/Payload/Configs/NASConfig.h +++ b/src/boards/Payload/Configs/NASConfig.h @@ -37,6 +37,8 @@ constexpr uint32_t UPDATE_PERIOD = 20; // 50 hz constexpr int CALIBRATION_SAMPLES_COUNT = 20; constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms] +constexpr float ACCELERATION_THRESHOLD = 0.5; // [m/s^2] + static const Boardcore::NASConfig config = { UPDATE_PERIOD / 1000.0, // T 0.0001f, // SIGMA_BETA diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index 380bc2c0b..8f9147750 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -70,8 +70,13 @@ void NASController::update() nas.correctGPS(gpsData); nas.correctBaro(pressureData.pressure); - // TODO: Add accelerometer correction until the acceleration goes out of - // specs + // Correct with accelerometer if the acceleration is in specs + Vector3f acceleration = static_cast<AccelerometerData>(imuData); + if (acceleration.norm() > (9.8 + ACCELERATION_THRESHOLD) || + acceleration.norm() < (9.8 - ACCELERATION_THRESHOLD)) + accelerationValid = false; + if (accelerationValid) + nas.correctAcc(imuData); Logger::getInstance().log(nas.getState()); FlightStatsRecorder::getInstance().update(nas.getState()); @@ -80,7 +85,8 @@ void NASController::update() void NASController::calibrate() { - Vector3f acceleration, magneticField; + Vector3f acceleration = Vector3f::Zero(); + Vector3f magneticField = Vector3f::Zero(); Stats pressure; for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++) @@ -248,6 +254,8 @@ void NASController::state_ready(const Event& event) { case EV_ENTRY: { + accelerationValid = true; + return logStatus(NASControllerState::READY); } case NAS_CALIBRATE: diff --git a/src/boards/Payload/StateMachines/NASController/NASController.h b/src/boards/Payload/StateMachines/NASController/NASController.h index db0eb9270..476c5a84d 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.h +++ b/src/boards/Payload/StateMachines/NASController/NASController.h @@ -69,6 +69,7 @@ private: Boardcore::NAS nas; Eigen::Vector3f initialOrientation; + bool accelerationValid = true; Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("nas"); }; -- GitLab