eive-obsw/mission/controller/PowerController.cpp
2024-03-20 15:53:51 +01:00

373 lines
13 KiB
C++

#include <mission/controller/PowerController.h>
PowerController::PowerController(object_id_t objectId, bool enableHkSets)
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
parameterHelper(this),
pwrCtrlCoreHk(this),
enablePl(this) {}
ReturnValue_t PowerController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
if (result != returnvalue::OK) {
return result;
}
return ExtendedControllerBase::initialize();
}
ReturnValue_t PowerController::handleCommandMessage(CommandMessage *message) {
ReturnValue_t result = actionHelper.handleActionMessage(message);
if (result == returnvalue::OK) {
return result;
}
result = parameterHelper.handleParameterMessage(message);
if (result == returnvalue::OK) {
return result;
}
return result;
}
MessageQueueId_t PowerController::getCommandQueue() const { return commandQueue->getId(); }
ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterId,
ParameterWrapper *parameterWrapper,
const ParameterWrapper *newValues,
uint16_t startAtIndex) {
switch (domainId) {
case 0x0: // direct members
switch (parameterId) {
case 0x0:
parameterWrapper->set(batteryInternalResistance);
break;
case 0x1:
parameterWrapper->set(batteryMaximumCapacity);
break;
case 0x2: {
float oldCoulombCounterVoltageUpperThreshold = coulombCounterVoltageUpperThreshold;
ReturnValue_t result = newValues->getElement(&coulombCounterVoltageUpperThreshold);
if (result != returnvalue::OK) {
coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
return result;
}
result = calculateCoulombCounterChargeUpperThreshold();
if (result != returnvalue::OK) {
coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
return result;
}
parameterWrapper->set(coulombCounterVoltageUpperThreshold);
break;
}
case 0x3:
parameterWrapper->set(maxAllowedTimeDiff);
break;
case 0x4:
parameterWrapper->set(payloadOpLimitOn);
break;
case 0x5:
parameterWrapper->set(payloadOpLimitLow);
break;
case 0x6:
parameterWrapper->set(higherModesLimit);
break;
default:
return INVALID_IDENTIFIER_ID;
}
break;
default:
return INVALID_DOMAIN_ID;
};
return returnvalue::OK;
}
void PowerController::performControlOperation() {
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
internalState = InternalState::INITIAL_DELAY;
return;
}
case InternalState::INITIAL_DELAY: {
if (initialCountdown.hasTimedOut()) {
internalState = InternalState::INIT;
}
return;
}
case InternalState::INIT: {
ReturnValue_t result = calculateCoulombCounterChargeUpperThreshold();
if (result == returnvalue::OK) {
internalState = InternalState::READY;
}
return;
}
case InternalState::READY: {
if (mode != MODE_NORMAL) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.setValidity(false, true);
}
}
if (mode != MODE_OFF) {
calculateStateOfCharge();
if (mode == MODE_NORMAL) {
watchStateOfCharge();
}
}
break;
}
default:
break;
}
}
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});
localDataPoolMap.emplace(pwrctrl::PoolIds::PAYLOAD_FLAG, new PoolEntry<uint8_t>({false}));
poolManager.subscribeForRegularPeriodicPacket({enablePl.getSid(), false, 60.0});
return returnvalue::OK;
}
LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case pwrctrl::CORE_HK:
return &pwrCtrlCoreHk;
case pwrctrl::ENABLE_PL:
return &enablePl;
default:
return nullptr;
}
return nullptr;
}
ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t *msToReachTheMode) {
if (mode == MODE_OFF or mode == MODE_ON or mode == MODE_NORMAL) {
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
} else {
return INVALID_SUBMODE;
}
}
return INVALID_MODE;
}
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();
if (result != returnvalue::OK) {
triggerEvent(power::DATASET_READ_FAILED);
sif::error << "Power Controller::Reading of Datasets has failed" << std::endl;
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = INVALID_TOTAL_BATTERY_CURRENT;
pwrCtrlCoreHk.openCircuitVoltageCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.setValidity(false, true);
}
}
return;
}
// calculate total battery current
iBat = p60CoreHk.batteryCurrent.value - bpxBatteryHk.dischargeCurrent.value;
result = calculateOpenCircuitVoltageCharge();
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 = INVALID_SOC;
pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(false);
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
return;
}
result = calculateCoulombCounterCharge(timeDelta);
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, false);
pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
return;
}
// commit to dataset
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
pwrCtrlCoreHk.openCircuitVoltageCharge.value =
charge2stateOfCharge(openCircuitVoltageCharge, false);
pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge, true);
pwrCtrlCoreHk.setValidity(true, true);
}
}
}
void PowerController::watchStateOfCharge() {
if (pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
if (pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitOn) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = false;
enablePl.setValidity(true, true);
}
} else {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = true;
enablePl.setValidity(true, true);
}
}
if (not pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitLow) {
triggerEvent(power::POWER_LEVEL_LOW);
pwrLvlLowFlag = true;
} else if (pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value > payloadOpLimitLow) {
pwrLvlLowFlag = false;
}
if (not pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value < higherModesLimit) {
triggerEvent(power::POWER_LEVEL_CRITICAL);
pwrLvlCriticalFlag = true;
} else if (pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value > higherModesLimit) {
pwrLvlCriticalFlag = false;
}
} else {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = false;
enablePl.setValidity(true, true);
}
}
}
ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
float vBatCorrected =
(bpxBatteryHk.battVoltage.value - iBat * batteryInternalResistance) * CONVERT_FROM_MILLI;
uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
ReturnValue_t result = lookUpTableOcvIdxFinder(vBatCorrected, lookUpTableIdx, false);
if (result != returnvalue::OK) {
return result;
}
openCircuitVoltageCharge = linearInterpolation(
vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1],
lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]);
return returnvalue::OK;
}
ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
if (timeDelta == 0.0) {
return returnvalue::FAILED;
}
if (timeDelta > maxAllowedTimeDiff) {
// should not be a permanent state so no spam protection required
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDelta * 10));
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDelta
<< std::endl;
return returnvalue::FAILED;
}
if (not pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
coulombCounterCharge = openCircuitVoltageCharge;
} else {
coulombCounterCharge =
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
coulombCounterCharge = coulombCounterChargeUpperThreshold;
}
}
return returnvalue::OK;
}
ReturnValue_t PowerController::updateEpsData() {
std::vector<ReturnValue_t> results;
{
PoolReadGuard pgBat(&bpxBatteryHk);
results.push_back(pgBat.getReadResult());
}
{
PoolReadGuard pgP60(&p60CoreHk);
results.push_back(pgP60.getReadResult());
}
for (const auto &result : results) {
if (result != returnvalue::OK) {
return result;
}
}
return returnvalue::OK;
}
float PowerController::charge2stateOfCharge(float capacity, bool coulombCounter) {
if (coulombCounter) {
return capacity / coulombCounterChargeUpperThreshold;
}
return capacity / batteryMaximumCapacity;
}
float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) {
return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
}
ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &idx, bool paramCmd) {
if (voltage >= lookUpTableOcv[1][99]) {
if (not voltageOutOfBoundsFlag and not paramCmd) {
triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 0, static_cast<uint32_t>(voltage * 10));
voltageOutOfBoundsFlag = true;
}
sif::error << "Power Controller::Voltage is too high: " << voltage << std::endl;
return returnvalue::FAILED;
} else if (voltage <= lookUpTableOcv[1][0]) {
if (not voltageOutOfBoundsFlag and not paramCmd) {
triggerEvent(power::VOLTAGE_OUT_OF_BOUNDS, 1, static_cast<uint32_t>(voltage * 10));
voltageOutOfBoundsFlag = true;
}
sif::error << "Power Controller::Voltage is too low: " << voltage << std::endl;
return returnvalue::FAILED;
}
voltageOutOfBoundsFlag = false;
while (lookUpTableOcv[1][idx] > voltage) {
idx--;
}
return returnvalue::OK;
}
ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
ReturnValue_t result =
lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx, true);
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;
}