eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

first bits of cleanup
Some checks failed
EIVE/eive-obsw/pipeline/pr-main There was a failure building this commit
Details

Browse Source
This commit is contained in:
Marius Eggert
2023-07-28 11:36:20 +02:00
parent 88a8969142
commit a0025030f1
4 changed files with 99 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
mission/controller/PowerController.cpp
View File

@ -1,11 +1,11 @@
#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/controller/PowerController.h>
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<int16_t>({0}));
localDataPoolMap.emplace(pwrctrl::PoolIds::OPEN_CIRCUIT_VOLTAGE_CHARGE,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(pwrctrl::PoolIds::COULOMB_COUNTER_CHARGE, new PoolEntry<float>({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<ReturnValue_t> 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);
}