From 2cd8d657cf6c8302489863d8f086d6e7ea370b0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu> Date: Thu, 5 Dec 2024 19:12:14 +0100 Subject: [PATCH] [ARP] SMA calibration with vn300 soft-hard iron calibration Now the CALIBRATION state performs a soft-hard iron calibration of the VN300. --- src/Groundstation/Automated/SMA/SMA.cpp | 9 ++++++++ src/Groundstation/Automated/SMA/SMA.h | 1 + .../Automated/Sensors/Sensors.cpp | 22 ++++++++++++++++++- src/Groundstation/Automated/Sensors/Sensors.h | 22 +++++++++++++++++++ 4 files changed, 53 insertions(+), 1 deletion(-) diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp index 57b170743..03321da8e 100644 --- a/src/Groundstation/Automated/SMA/SMA.cpp +++ b/src/Groundstation/Automated/SMA/SMA.cpp @@ -329,6 +329,12 @@ void SMA::update() break; } + case SMAState::CALIBRATE: + { + if (!sensors->isCalibrating()) + EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP); + } + break; default: { break; @@ -702,6 +708,9 @@ State SMA::state_calibrate(const Event& event) { case EV_ENTRY: { + auto* sensors = getModule<Sensors>(); + if (!sensors->calibrate() && sensors->isCalibrating()) + transition(&SMA::state_test); logStatus(SMAState::CALIBRATE); return HANDLED; } diff --git a/src/Groundstation/Automated/SMA/SMA.h b/src/Groundstation/Automated/SMA/SMA.h index 2bc290d9a..d86934ac0 100644 --- a/src/Groundstation/Automated/SMA/SMA.h +++ b/src/Groundstation/Automated/SMA/SMA.h @@ -97,6 +97,7 @@ public: /** * @brief move the stepper `stepperId` of `angle` degrees + * */ ActuationStatus moveStepperDeg(StepperList stepperId, float angle); diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp index 866231a74..315756771 100644 --- a/src/Groundstation/Automated/Sensors/Sensors.cpp +++ b/src/Groundstation/Automated/Sensors/Sensors.cpp @@ -63,9 +63,29 @@ bool Sensors::vn300Init() return true; } +bool Sensors::calibrate() +{ + // Already in calibration mode. + if (calibrating) + return false; + calibrationStart = std::chrono::nanoseconds(miosix::getTime()); + calibrating = true; + vn300->startHSIEstimator(VN300_CAL_CONVERGENCE); + return true; +} + +bool Sensors::isCalibrating() { return calibrating; } + void Sensors::vn300Callback() { - Logger::getInstance().log(vn300->getLastSample()); + if (calibrating) + { + if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) > + VN300_CAL_TIME) + vn300->stopHSIEstimator(); + } + else + Logger::getInstance().log(vn300->getLastSample()); } VN300Data Sensors::getVN300LastSample() { return vn300->getLastSample(); } diff --git a/src/Groundstation/Automated/Sensors/Sensors.h b/src/Groundstation/Automated/Sensors/Sensors.h index 87ce9a237..43d06d76b 100644 --- a/src/Groundstation/Automated/Sensors/Sensors.h +++ b/src/Groundstation/Automated/Sensors/Sensors.h @@ -21,14 +21,23 @@ */ #pragma once +#include <drivers/timer/TimestampTimer.h> #include <utils/DependencyManager/DependencyManager.h> +#include <chrono> + #include "Groundstation/LyraGS/Buses.h" #include "sensors/SensorManager.h" #include "sensors/Vectornav/VN300/VN300.h" namespace Antennas { + +static constexpr uint8_t VN300_CAL_CONVERGENCE = + 4; ///< Calibration convergence parameter for VN300 soft and hard iron + ///< calibration. 5: converge in 60-90sec, 1: converge in 15-20sec +static constexpr std::chrono::seconds VN300_CAL_TIME = std::chrono::seconds(30); + class Sensors : public Boardcore::InjectableWithDeps<LyraGS::Buses> { public: @@ -44,14 +53,27 @@ public: */ Boardcore::VN300Data getVN300LastSample(); + /** + * @brief Trigger the calibration process for soft-hard iron in the VN300 + */ + bool calibrate(); + + /** + * @brief Returns the status of the calibration + */ + bool isCalibrating(); + private: bool vn300Init(); void vn300Callback(); + std::atomic<bool> calibrating{false}; + Boardcore::VN300* vn300 = nullptr; Boardcore::SensorManager* sm = nullptr; Boardcore::SensorManager::SensorMap_t sensorsMap; Boardcore::PrintLogger logger = Boardcore::Logging::getLogger("sensors"); + std::chrono::nanoseconds calibrationStart; }; } // namespace Antennas -- GitLab