diff --git a/CHANGELOG.md b/CHANGELOG.md index 09514e52..3c1d0bd3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,6 +24,8 @@ will consitute of a breaking change warranting a new major release: A filesystem change via `prefSD` on bootup, can lead to the TLE not being read, even though it is there. - Added action cmd to read the currently stored TLE. +- Both the `AcsController` and the `PwrController` now use the monotonic clock to calculate + the time difference. # [v7.4.0] 2023-11-30 diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index f8a02c09..719ef507 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -158,6 +158,11 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode, void PowerController::calculateStateOfCharge() { // get time Clock::getClockMonotonic(&now); + double timeDelta = 0.0; + if (now.tv_sec != 0 and oldTime.tv_sec != 0) { + timeDelta = timevalOperations::toDouble(now - oldTime); + } + oldTime = now; // update EPS HK values ReturnValue_t result = updateEpsData(); @@ -173,8 +178,6 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.setValidity(false, true); } } - // store time for next run - oldTime = now; return; } @@ -195,12 +198,10 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.coulombCounterCharge.setValid(false); } } - // store time for next run - oldTime = now; return; } - result = calculateCoulombCounterCharge(); + result = calculateCoulombCounterCharge(timeDelta); if (result != returnvalue::OK) { // notifying events have already been triggered { @@ -215,9 +216,6 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.coulombCounterCharge.setValid(false); } } - // store time for next run - oldTime = now; - return; } // commit to dataset @@ -231,8 +229,6 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.setValidity(true, true); } } - // store time for next run - oldTime = now; } void PowerController::watchStateOfCharge() { @@ -285,17 +281,14 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { return returnvalue::OK; } -ReturnValue_t PowerController::calculateCoulombCounterCharge() { - double timeDiff = 0.0; - if (oldTime.tv_sec != 0) { - timeDiff = timevalOperations::toDouble(now - oldTime); - } else { +ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) { + if (timeDelta == 0.0) { return returnvalue::FAILED; } - if (timeDiff > maxAllowedTimeDiff) { + if (timeDelta > maxAllowedTimeDiff) { // should not be a permanent state so no spam protection required - triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast(timeDiff * 10)); - sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff + triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast(timeDelta * 10)); + sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta << std::endl; return returnvalue::FAILED; } @@ -303,7 +296,7 @@ ReturnValue_t PowerController::calculateCoulombCounterCharge() { coulombCounterCharge = openCircuitVoltageCharge; } else { coulombCounterCharge = - coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * SECONDS_TO_HOURS; + coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS; if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) { coulombCounterCharge = coulombCounterChargeUpperThreshold; } diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h index b7c3d1a0..6df60732 100644 --- a/mission/controller/PowerController.h +++ b/mission/controller/PowerController.h @@ -45,7 +45,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM void calculateStateOfCharge(); void watchStateOfCharge(); ReturnValue_t calculateOpenCircuitVoltageCharge(); - ReturnValue_t calculateCoulombCounterCharge(); + ReturnValue_t calculateCoulombCounterCharge(double timeDelta); ReturnValue_t updateEpsData(); float charge2stateOfCharge(float capacity, bool coulombCounter); ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 598fecb3..80269d55 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -78,9 +78,9 @@ class AcsParameters : public HasParametersIF { {-0.007534, 1.253879, 0.006812}, {-0.037072, 0.006812, 1.313158}}; - float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; - float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; - float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; + double mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; + double mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; + double mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; float mgmVectorFilterWeight = 0.85; float mgmDerivativeFilterWeight = 0.99; uint8_t useMgm4 = false; @@ -790,10 +790,10 @@ class AcsParameters : public HasParametersIF { /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is * assumed to be equal for the same class of sensors */ - float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms + double gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(6.1e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms - float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; + double gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; uint8_t preferAdis = false; float gyrFilterWeight = 0.6; } gyrHandlingParameters; diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0390d7f1..c454440a 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -15,13 +15,12 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; bool gpsValid = false; - if (gpsDataProcessed->source.value != acs::gps::Source::NONE) { + if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { + // There seems to be a bug here, which causes the model vector to drift until infinity, if the + // model class is not initialized new every time. Works for now, but should be investigated. Igrf13Model igrf13; igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeAbsolute); - // ToDo - // maybe put a condition here, to only update after a full day, this - // class function has around 700 steps to perform igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, gpsDataProcessed->altitude.value, timeAbsolute, magIgrfModel); gpsValid = true;