From a0025030f103c6998984c909cc9c2fa81a5b1ca5 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 28 Jul 2023 11:36:20 +0200 Subject: [PATCH] first bits of cleanup --- mission/controller/PowerController.cpp | 109 +++++++++++------- mission/controller/PowerController.h | 17 ++- .../PowerCtrlDefinitions.h | 14 ++- mission/power/defs.h | 7 ++ 4 files changed, 99 insertions(+), 48 deletions(-) diff --git a/mission/controller/PowerController.cpp b/mission/controller/PowerController.cpp index 38db8847..fa6ab403 100644 --- a/mission/controller/PowerController.cpp +++ b/mission/controller/PowerController.cpp @@ -1,11 +1,11 @@ -#include + #include PowerController::PowerController(object_id_t objectId, bool enableHkSets) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), parameterHelper(this), - stateOfChargeData(this) {} + pwrCtrlCoreHk(this) {} ReturnValue_t PowerController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -40,6 +40,9 @@ ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterI case 0x0: parameterWrapper->set(batteryInternalResistance); break; + case 0x1: + parameterWrapper->set(batteryMaximumCapacity); + break; default: return INVALID_IDENTIFIER_ID; } @@ -78,13 +81,18 @@ void PowerController::performControlOperation() { ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(pwrctrl::PoolIds::TOTAL_BATTERY_CURRENT, new PoolEntry({0})); + localDataPoolMap.emplace(pwrctrl::PoolIds::OPEN_CIRCUIT_VOLTAGE_CHARGE, + new PoolEntry({0.0})); + localDataPoolMap.emplace(pwrctrl::PoolIds::COULOMB_COUNTER_CHARGE, new PoolEntry({0.0})); + poolManager.subscribeForRegularPeriodicPacket({pwrCtrlCoreHk.getSid(), enableHkSets, 60.0}); return returnvalue::OK; } LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) { switch (sid.ownerSetId) { - case pwrctrl::STATE_OF_CHARGE_DATA: - return &stateOfChargeData; + case pwrctrl::CORE_HK: + return &pwrCtrlCoreHk; default: return nullptr; } @@ -105,53 +113,72 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode, void PowerController::calculateStateOfCharge() { // get time - timeval now; Clock::getClock_timeval(&now); - // update EPS hk values + // update EPS HK values ReturnValue_t result = updateEpsData(); - - float iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.value + - bpxBatteryHk.dischargeCurrent.value; - - // Open Circuit Voltage Charge - // ToDo: battery heater and battery discharge currents missing - float vBatCorrected = p60CoreHk.batteryVoltage.value - iBat * batteryInternalResistance; - uint8_t lookUpTableIdx; - for (lookUpTableIdx = 24; lookUpTableIdx > 0; lookUpTableIdx--) { - if (lookUpTableOcv[1][lookUpTableIdx] < vBatCorrected) { - break; - } - } - // ToDo: check indexing - float openCircuitVoltageCharge = - lookUpTableOcv[0][lookUpTableIdx] + - (vBatCorrected - lookUpTableOcv[1][lookUpTableIdx]) * - (lookUpTableOcv[1][lookUpTableIdx + 1] - lookUpTableOcv[1][lookUpTableIdx]) / - (lookUpTableOcv[0][lookUpTableIdx + 1] - lookUpTableOcv[0][lookUpTableIdx]); - - // Coulomb Counter - float coulombCounterCharge = 0; - if (stateOfChargeData.coulombCounterCharge.value == 0) - coulombCounterCharge = openCircuitVoltageCharge; - else { - double timeDiff = timevalOperations::toDouble(now - oldTime); - coulombCounterCharge = stateOfChargeData.coulombCounterCharge.value + iBat * timeDiff; + if (result != returnvalue::OK) { + triggerEvent(power::DATASET_READ_FAILED); + sif::info << "Power Controller::Reading of Datasets has failed" << std::endl; + return; } - // commit TM + // calculate total battery current + iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.heaterCurrent.value + + bpxBatteryHk.dischargeCurrent.value; + + calculateOpenCircuitVoltageCharge(); + calculateCoulombCounterCharge(); + + // commit to dataset { - PoolReadGuard pg(&stateOfChargeData); - stateOfChargeData.openCircuitVoltageCharge.value = - charge2stateOfCharge(openCircuitVoltageCharge); - stateOfChargeData.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge); - stateOfChargeData.setValidity(true, true); + PoolReadGuard pg(&pwrCtrlCoreHk); + if (pg.getReadResult() == returnvalue::OK) { + pwrCtrlCoreHk.totalBatteryCurrent.value = iBat; + pwrCtrlCoreHk.openCircuitVoltageCharge.value = charge2stateOfCharge(openCircuitVoltageCharge); + pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge); + pwrCtrlCoreHk.setValidity(true, true); + } } // store time for next run oldTime = now; } +void PowerController::calculateOpenCircuitVoltageCharge() { + float vBatCorrected = p60CoreHk.batteryVoltage.value - iBat * batteryInternalResistance; + if (vBatCorrected >= lookUpTableOcv[1][100]) { + triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0); + sif::error << "Power Controller::Voltage is too high" << std::endl; + return; + } else if (vBatCorrected <= lookUpTableOcv[1][0]) { + triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1); + sif::error << "Power Controller::Voltage is too low" << std::endl; + return; + } + uint8_t lookUpTableIdx = 99; + while (lookUpTableOcv[1][lookUpTableIdx] > vBatCorrected) { + lookUpTableIdx--; + } + + openCircuitVoltageCharge = linearInterpolation( + vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1], + lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]); +} + +void PowerController::calculateCoulombCounterCharge() { + if ((pwrCtrlCoreHk.coulombCounterCharge.value == 0) or + (p60CoreHk.batteryVoltage.value > coulombCounterVoltageUpperThreshold and + pwrCtrlCoreHk.coulombCounterCharge.value >= )) { + coulombCounterCharge = openCircuitVoltageCharge; + } + + else { + double timeDiff = timevalOperations::toDouble(now - oldTime); + coulombCounterCharge = pwrCtrlCoreHk.coulombCounterCharge.value + iBat * timeDiff; + } +} + ReturnValue_t PowerController::updateEpsData() { std::vector results; { @@ -185,3 +212,7 @@ ReturnValue_t PowerController::updateEpsData() { float PowerController::charge2stateOfCharge(float capacity) { return capacity / batteryMaximumCapacity; } + +float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) { + return y0 + (x - x0) * (y1 - y0) / (x1 - x0); +} diff --git a/mission/controller/PowerController.h b/mission/controller/PowerController.h index e49e6b71..d255af7b 100644 --- a/mission/controller/PowerController.h +++ b/mission/controller/PowerController.h @@ -43,15 +43,19 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM void performControlOperation() override; void calculateStateOfCharge(); + void calculateOpenCircuitVoltageCharge(); + void calculateCoulombCounterCharge(); ReturnValue_t updateEpsData(); float charge2stateOfCharge(float capacity); + float linearInterpolation(float x, float x0, float x1, float y0, float y1); // Parameters - float batteryInternalResistance = 70 / 2 / 1000; // [Ohm] - float batteryMaximumCapacity = 2.6 * 2; // [Ah] + float batteryInternalResistance = 70.0 / 2.0 / 1000.0; // [Ohm] + float batteryMaximumCapacity = 2.6 * 2; // [Ah] + float coulombCounterVoltageUpperThreshold = 16.2e3; // [mV] // OCV Look-up-Table - float lookUpTableOcv[2][25] = { + float lookUpTableOcv[2][100] = { {0.00000000e+00, 3.16227766e-04, 4.52809661e-04, 6.48382625e-04, 9.28425483e-04, 1.32942162e-03, 1.90361194e-03, 2.72580074e-03, 3.90310099e-03, 5.58888885e-03, 8.00278514e-03, 1.14592671e-02, 1.64086377e-02, 2.34956903e-02, 3.36437110e-02, @@ -89,7 +93,12 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM 16.67249918, 16.74926904}}; // Variables + timeval now; timeval oldTime; + int16_t iBat = 0.0; // [mA] + float openCircuitVoltageCharge = 0.0; // [mC] + float coulombCounterCharge = 0; // [mC] + float coulombCounterChargeUpperThreshold = 0.0; // [mC] // HK Datasets for Calculation BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER); @@ -98,7 +107,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM PDU2::Pdu2CoreHk pdu2CoreHk = PDU2::Pdu2CoreHk(objects::PDU2_HANDLER); ACU::CoreHk acuCoreHk = ACU::CoreHk(objects::ACU_HANDLER); // Output Dataset - pwrctrl::StateOfChargedData stateOfChargeData; + pwrctrl::CoreHk pwrCtrlCoreHk; }; #endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */ diff --git a/mission/controller/controllerdefinitions/PowerCtrlDefinitions.h b/mission/controller/controllerdefinitions/PowerCtrlDefinitions.h index 770889c1..f7cce708 100644 --- a/mission/controller/controllerdefinitions/PowerCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/PowerCtrlDefinitions.h @@ -1,27 +1,31 @@ #ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_ #define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_ +#include #include #include +#include #include namespace pwrctrl { -enum SetIds : uint32_t { STATE_OF_CHARGE_DATA }; +enum SetIds : uint32_t { CORE_HK }; enum PoolIds : lp_id_t { + TOTAL_BATTERY_CURRENT, OPEN_CIRCUIT_VOLTAGE_CHARGE, COULOMB_COUNTER_CHARGE, }; -static constexpr uint8_t STATE_OF_CHARGE_ENTRIES = 2; +static constexpr uint8_t CORE_HK_ENTRIES = 3; -class StateOfChargedData : public StaticLocalDataSet { +class CoreHk : public StaticLocalDataSet { public: - StateOfChargedData(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, STATE_OF_CHARGE_DATA) {} + CoreHk(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CORE_HK) {} + lp_var_t totalBatteryCurrent = + lp_var_t(sid.objectId, TOTAL_BATTERY_CURRENT, this); lp_var_t openCircuitVoltageCharge = lp_var_t(sid.objectId, OPEN_CIRCUIT_VOLTAGE_CHARGE, this); lp_var_t coulombCounterCharge = diff --git a/mission/power/defs.h b/mission/power/defs.h index 13e5e63f..682be614 100644 --- a/mission/power/defs.h +++ b/mission/power/defs.h @@ -47,6 +47,13 @@ static constexpr Event SWITCHING_Q7S_DENIED = event::makeEvent(SUBSYSTEM_ID, 2, static constexpr Event FDIR_REACTION_IGNORED = event::makeEvent(SUBSYSTEM_ID, 3, severity::MEDIUM); +//! [EXPORT] : [COMMENT] The dataset read for the inputs of the Power Controller has failed. +static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, severity::INFO); +//! [EXPORT] : [COMMENT] The battery voltage read is out of the bounds in which it is supposed to +//! be. +//! P1: 1 too high, 0 too low +static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH); + enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING }; enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };