diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index 289e70b8..21e3ad3d 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -158,7 +158,26 @@ void PowerController::calculateStateOfCharge() { } return; } - calculateCoulombCounterCharge(); + + result = calculateCoulombCounterCharge(); + // store time for next run + oldTime = now; + if (result != returnvalue::OK) { + // notifying events have already been triggered + { + PoolReadGuard pg(&pwrCtrlCoreHk); + if (pg.getReadResult() == returnvalue::OK) { + pwrCtrlCoreHk.totalBatteryCurrent.value = iBat; + pwrCtrlCoreHk.totalBatteryCurrent.setValid(true); + pwrCtrlCoreHk.openCircuitVoltageCharge.value = + charge2stateOfCharge(openCircuitVoltageCharge); + pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true); + pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC; + pwrCtrlCoreHk.coulombCounterCharge.setValid(false); + } + } + return; + } // commit to dataset { @@ -170,9 +189,6 @@ void PowerController::calculateStateOfCharge() { pwrCtrlCoreHk.setValidity(true, true); } } - - // store time for next run - oldTime = now; } ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { @@ -189,21 +205,20 @@ ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() { } ReturnValue_t PowerController::calculateCoulombCounterCharge() { - if ((pwrCtrlCoreHk.coulombCounterCharge.value == 0) or + double timeDiff = timevalOperations::toDouble(now - oldTime); + if (timeDiff > maxAllowedTimeDiff) { + triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS); + sif::error << "Power Controller::Time delta too large for Coulomb Counter" << std::endl; + return returnvalue::FAILED; + } + if ((not pwrCtrlCoreHk.coulombCounterCharge.isValid()) or (p60CoreHk.batteryVoltage.value > coulombCounterVoltageUpperThreshold and pwrCtrlCoreHk.coulombCounterCharge.value >= coulombCounterChargeUpperThreshold)) { coulombCounterCharge = openCircuitVoltageCharge; - return returnvalue::OK; + } else { + coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff; } - - else { - double timeDiff = timevalOperations::toDouble(now - oldTime); - if (timeDiff < maxAllowedTimeDiff) { - coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff; - return returnvalue::OK; - } - } - return returnvalue::FAILED; + return returnvalue::OK; } ReturnValue_t PowerController::updateEpsData() { @@ -259,3 +274,17 @@ ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &i } return returnvalue::OK; } + +ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() { + uint8_t lookUpTableIdx = 99; + ReturnValue_t result = + lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx); + if (result != returnvalue::OK) { + return result; + } + coulombCounterChargeUpperThreshold = + linearInterpolation(coulombCounterVoltageUpperThreshold, lookUpTableOcv[1][lookUpTableIdx], + lookUpTableOcv[1][lookUpTableIdx + 1], lookUpTableOcv[0][lookUpTableIdx], + lookUpTableOcv[0][lookUpTableIdx + 1]); + return returnvalue::OK; +} diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h index 971bc0af..0f15363b 100644 --- a/mission/controller/PowerController.h +++ b/mission/controller/PowerController.h @@ -49,6 +49,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM float charge2stateOfCharge(float capacity); ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx); float linearInterpolation(float x, float x0, float x1, float y0, float y1); + ReturnValue_t calculateCoulombCounterChargeUpperThreshold(); // Parameters float batteryInternalResistance = 70.0 / 2.0 / 1000.0; // [Ohm] diff --git a/mission/power/defs.h b/mission/power/defs.h index 682be614..87fc3f08 100644 --- a/mission/power/defs.h +++ b/mission/power/defs.h @@ -53,6 +53,9 @@ static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, s //! be. //! P1: 1 too high, 0 too low static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH); +//! [EXPORT] : [COMMENT] Time difference for Coulomb Counter was too large. +static constexpr Event TIMEDELTA_OUT_OF_BOUNDS = + event::makeEvent(SUBSYSTEM_ID, 6, severity::MEDIUM); enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING }; enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };