From 6d2bfbcfe6448af279c5a8e9a837500659d9e7e1 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 14:20:11 +0100 Subject: [PATCH 1/5] this is cleaner --- mission/controller/PowerController.cpp | 31 ++++++++++---------------- mission/controller/PowerController.h | 2 +- 2 files changed, 13 insertions(+), 20 deletions(-) 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); From aab705ca040baf148d853ca99045be640935955b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 1 Dec 2023 14:22:10 +0100 Subject: [PATCH 2/5] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index e0926cc4..f763c678 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 From 064200e7300142fe096bf9daa719abf4be0f5c9f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Dec 2023 09:58:37 +0100 Subject: [PATCH 3/5] yesyes clangd shush now --- mission/controller/acs/AcsParameters.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index a9c9ab5d..130d9a91 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -75,9 +75,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; @@ -787,10 +787,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; From 4ea1e16880ed4c75c0846d0f29ee71926444f735 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 10:59:44 +0100 Subject: [PATCH 4/5] cpu goes brrrr --- mission/controller/acs/SensorProcessing.cpp | 6 ++---- mission/controller/acs/SensorProcessing.h | 2 ++ 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 56217a8f..975b38b1 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -1,5 +1,7 @@ #include "SensorProcessing.h" +#include + SensorProcessing::SensorProcessing() {} SensorProcessing::~SensorProcessing() {} @@ -16,12 +18,8 @@ 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::GpsSource::NONE) { - 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; diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index fea0fd01..7caa29f8 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -35,6 +35,8 @@ class SensorProcessing { static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ECCENTRICITY_WGS84 = 0.0818195; + Igrf13Model igrf13; + protected: void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid, From 41c8d7e0dd884f7a73983f4d4b6c3c2748e8137c Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 4 Dec 2023 11:15:08 +0100 Subject: [PATCH 5/5] replaced one ToDo with another one --- mission/controller/acs/SensorProcessing.cpp | 3 +++ mission/controller/acs/SensorProcessing.h | 2 -- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 975b38b1..23388e5f 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -18,6 +18,9 @@ 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::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); igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value, diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 7caa29f8..fea0fd01 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -35,8 +35,6 @@ class SensorProcessing { static constexpr double ZERO_VEC_D[3] = {0, 0, 0}; static constexpr double ECCENTRICITY_WGS84 = 0.0818195; - Igrf13Model igrf13; - protected: void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid, const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,