diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp index 03321da8e87f384283cac4ccb9354354a9982293..cf800147d9ef7acb260e13af196f9765fca17d35 100644 --- a/src/Groundstation/Automated/SMA/SMA.cpp +++ b/src/Groundstation/Automated/SMA/SMA.cpp @@ -332,7 +332,10 @@ void SMA::update() case SMAState::CALIBRATE: { if (!sensors->isCalibrating()) + { EventBroker::getInstance().post(ARP_CAL_DONE, TOPIC_ARP); + LOG_DEBUG(logger, "Exit from calibration\n"); + } } break; default: diff --git a/src/Groundstation/Automated/Sensors/Sensors.cpp b/src/Groundstation/Automated/Sensors/Sensors.cpp index 315756771bab1724555c609337ea430715c4b63a..0fe17cf13e091b16f95e3e3c06a71893bd637508 100644 --- a/src/Groundstation/Automated/Sensors/Sensors.cpp +++ b/src/Groundstation/Automated/Sensors/Sensors.cpp @@ -80,9 +80,13 @@ void Sensors::vn300Callback() { if (calibrating) { - if (calibrationStart - std::chrono::nanoseconds(miosix::getTime()) > + LOG_DEBUG(logger, "Sensor Manager VN300 calibrating\n"); + if (std::chrono::nanoseconds(miosix::getTime()) - calibrationStart > VN300_CAL_TIME) + { vn300->stopHSIEstimator(); + calibrating = false; + } } else Logger::getInstance().log(vn300->getLastSample());