eive-obsw/mission/controller/PowerController.cpp

373 lines
13 KiB
C++
Raw Normal View History

2023-06-07 11:33:09 +02:00
#include <mission/controller/PowerController.h>
PowerController::PowerController(object_id_t objectId, bool enableHkSets)
2023-07-26 11:18:15 +02:00
: ExtendedControllerBase(objectId),
enableHkSets(enableHkSets),
parameterHelper(this),
2023-09-29 10:16:23 +02:00
pwrCtrlCoreHk(this),
enablePl(this) {}
2023-06-07 11:33:09 +02:00
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;
}
2023-06-15 17:51:32 +02:00
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) {
2023-07-26 17:01:48 +02:00
switch (domainId) {
case 0x0: // direct members
switch (parameterId) {
case 0x0:
parameterWrapper->set(batteryInternalResistance);
break;
2023-07-28 11:36:20 +02:00
case 0x1:
parameterWrapper->set(batteryMaximumCapacity);
break;
2023-07-31 13:32:40 +02:00
case 0x2: {
2023-07-31 13:27:14 +02:00
float oldCoulombCounterVoltageUpperThreshold = coulombCounterVoltageUpperThreshold;
2023-10-10 11:53:54 +02:00
ReturnValue_t result = newValues->getElement(&coulombCounterVoltageUpperThreshold);
2023-07-31 13:27:14 +02:00
if (result != returnvalue::OK) {
coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
2023-10-10 11:53:54 +02:00
return result;
2023-07-31 13:27:14 +02:00
}
2023-10-10 11:53:54 +02:00
result = calculateCoulombCounterChargeUpperThreshold();
if (result != returnvalue::OK) {
coulombCounterVoltageUpperThreshold = oldCoulombCounterVoltageUpperThreshold;
return result;
}
2023-10-11 09:24:36 +02:00
parameterWrapper->set(coulombCounterVoltageUpperThreshold);
2023-07-31 11:51:03 +02:00
break;
2023-07-31 13:32:40 +02:00
}
2023-07-31 11:51:03 +02:00
case 0x3:
parameterWrapper->set(maxAllowedTimeDiff);
break;
2023-09-27 14:46:45 +02:00
case 0x4:
2023-09-29 10:16:23 +02:00
parameterWrapper->set(payloadOpLimitOn);
2023-09-27 14:46:45 +02:00
break;
case 0x5:
2023-09-29 10:16:23 +02:00
parameterWrapper->set(payloadOpLimitLow);
break;
case 0x6:
2023-09-27 14:46:45 +02:00
parameterWrapper->set(higherModesLimit);
break;
2023-07-26 17:01:48 +02:00
default:
return INVALID_IDENTIFIER_ID;
}
break;
default:
return INVALID_DOMAIN_ID;
};
return returnvalue::OK;
2023-06-15 17:51:32 +02:00
}
2023-06-07 11:33:09 +02:00
void PowerController::performControlOperation() {
switch (internalState) {
case InternalState::STARTUP: {
initialCountdown.resetTimer();
internalState = InternalState::INITIAL_DELAY;
return;
}
case InternalState::INITIAL_DELAY: {
if (initialCountdown.hasTimedOut()) {
2023-07-31 13:27:14 +02:00
internalState = InternalState::INIT;
}
return;
}
case InternalState::INIT: {
ReturnValue_t result = calculateCoulombCounterChargeUpperThreshold();
if (result == returnvalue::OK) {
2023-06-07 11:33:09 +02:00
internalState = InternalState::READY;
}
return;
}
case InternalState::READY: {
2023-10-16 14:10:51 +02:00
if (mode != MODE_NORMAL) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.setValidity(false, true);
}
}
2023-09-28 12:00:48 +02:00
if (mode != MODE_OFF) {
calculateStateOfCharge();
if (mode == MODE_NORMAL) {
watchStateOfCharge();
}
2023-09-13 16:29:54 +02:00
}
2023-06-07 11:33:09 +02:00
break;
}
default:
break;
}
}
ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
2023-07-28 11:36:20 +02:00
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});
2023-09-29 10:16:23 +02:00
localDataPoolMap.emplace(pwrctrl::PoolIds::PAYLOAD_FLAG, new PoolEntry<uint8_t>({false}));
poolManager.subscribeForRegularPeriodicPacket({enablePl.getSid(), false, 60.0});
2023-06-07 11:33:09 +02:00
return returnvalue::OK;
}
2023-06-10 15:14:20 +02:00
LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
2023-07-28 11:36:20 +02:00
case pwrctrl::CORE_HK:
return &pwrCtrlCoreHk;
2023-09-29 10:16:23 +02:00
case pwrctrl::ENABLE_PL:
return &enablePl;
2023-06-10 15:14:20 +02:00
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) {
2023-06-10 15:14:20 +02:00
if (submode == SUBMODE_NONE) {
return returnvalue::OK;
} else {
return INVALID_SUBMODE;
}
}
return INVALID_MODE;
}
2023-07-26 17:01:48 +02:00
void PowerController::calculateStateOfCharge() {
// get time
2023-10-11 14:30:57 +02:00
Clock::getClockMonotonic(&now);
2023-12-01 14:20:11 +01:00
double timeDelta = 0.0;
if (now.tv_sec != 0 and oldTime.tv_sec != 0) {
timeDelta = timevalOperations::toDouble(now - oldTime);
}
oldTime = now;
2023-07-26 17:01:48 +02:00
2023-07-28 11:36:20 +02:00
// update EPS HK values
2023-07-26 17:01:48 +02:00
ReturnValue_t result = updateEpsData();
2023-07-28 11:36:20 +02:00
if (result != returnvalue::OK) {
triggerEvent(power::DATASET_READ_FAILED);
2023-07-31 11:51:03 +02:00
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);
}
}
2023-07-28 11:36:20 +02:00
return;
}
2023-07-26 17:01:48 +02:00
2023-07-28 11:36:20 +02:00
// calculate total battery current
2023-09-27 16:36:09 +02:00
iBat = p60CoreHk.batteryCurrent.value + bpxBatteryHk.dischargeCurrent.value;
2023-07-26 17:01:48 +02:00
2023-07-31 11:51:03 +02:00
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;
}
2023-07-31 13:14:04 +02:00
2023-12-01 14:20:11 +01:00
result = calculateCoulombCounterCharge(timeDelta);
2023-07-31 13:14:04 +02:00
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 =
2023-09-27 16:36:09 +02:00
charge2stateOfCharge(openCircuitVoltageCharge, false);
2023-07-31 13:14:04 +02:00
pwrCtrlCoreHk.openCircuitVoltageCharge.setValid(true);
pwrCtrlCoreHk.coulombCounterCharge.value = INVALID_SOC;
pwrCtrlCoreHk.coulombCounterCharge.setValid(false);
}
}
return;
2023-07-31 13:14:04 +02:00
}
2023-07-28 11:36:20 +02:00
// commit to dataset
{
PoolReadGuard pg(&pwrCtrlCoreHk);
if (pg.getReadResult() == returnvalue::OK) {
pwrCtrlCoreHk.totalBatteryCurrent.value = iBat;
2023-09-27 16:36:09 +02:00
pwrCtrlCoreHk.openCircuitVoltageCharge.value =
charge2stateOfCharge(openCircuitVoltageCharge, false);
pwrCtrlCoreHk.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge, true);
2023-07-28 11:36:20 +02:00
pwrCtrlCoreHk.setValidity(true, true);
2023-07-26 17:01:48 +02:00
}
}
2023-07-28 11:36:20 +02:00
}
2023-09-13 16:29:54 +02:00
void PowerController::watchStateOfCharge() {
2023-09-27 14:46:45 +02:00
if (pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
2023-09-29 10:16:23 +02:00
if (pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitOn) {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = false;
enablePl.setValidity(true, true);
2023-09-29 10:16:23 +02:00
}
} else {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = true;
enablePl.setValidity(true, true);
2023-09-29 10:16:23 +02:00
}
}
if (not pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value < payloadOpLimitLow) {
2023-09-27 14:46:45 +02:00
triggerEvent(power::POWER_LEVEL_LOW);
pwrLvlLowFlag = true;
2023-10-09 14:03:57 +02:00
} else if (pwrLvlLowFlag and pwrCtrlCoreHk.coulombCounterCharge.value > payloadOpLimitLow) {
2023-09-27 14:46:45 +02:00
pwrLvlLowFlag = false;
}
if (not pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value < higherModesLimit) {
triggerEvent(power::POWER_LEVEL_CRITICAL);
pwrLvlCriticalFlag = true;
2023-10-09 14:03:57 +02:00
} else if (pwrLvlCriticalFlag and pwrCtrlCoreHk.coulombCounterCharge.value > higherModesLimit) {
2023-09-27 14:46:45 +02:00
pwrLvlCriticalFlag = false;
}
2023-09-29 10:16:23 +02:00
} else {
PoolReadGuard pg(&enablePl);
if (pg.getReadResult() == returnvalue::OK) {
enablePl.plUseAllowed.value = false;
enablePl.setValidity(true, true);
2023-09-29 10:16:23 +02:00
}
2023-09-27 14:46:45 +02:00
}
2023-09-13 16:29:54 +02:00
}
2023-07-31 11:51:03 +02:00
ReturnValue_t PowerController::calculateOpenCircuitVoltageCharge() {
2023-09-22 10:08:04 +02:00
float vBatCorrected =
2023-09-22 16:07:26 +02:00
(bpxBatteryHk.battVoltage.value - iBat * batteryInternalResistance) * CONVERT_FROM_MILLI;
2023-09-13 16:29:54 +02:00
uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
2023-10-09 15:36:49 +02:00
ReturnValue_t result = lookUpTableOcvIdxFinder(vBatCorrected, lookUpTableIdx, false);
2023-07-31 11:51:03 +02:00
if (result != returnvalue::OK) {
return result;
2023-07-26 17:01:48 +02:00
}
2023-07-28 11:36:20 +02:00
openCircuitVoltageCharge = linearInterpolation(
vBatCorrected, lookUpTableOcv[1][lookUpTableIdx], lookUpTableOcv[1][lookUpTableIdx + 1],
lookUpTableOcv[0][lookUpTableIdx], lookUpTableOcv[0][lookUpTableIdx + 1]);
2023-07-31 11:51:03 +02:00
return returnvalue::OK;
2023-07-28 11:36:20 +02:00
}
2023-12-01 14:20:11 +01:00
ReturnValue_t PowerController::calculateCoulombCounterCharge(double timeDelta) {
if (timeDelta == 0.0) {
2023-10-11 14:30:57 +02:00
return returnvalue::FAILED;
}
2023-12-01 14:20:11 +01:00
if (timeDelta > maxAllowedTimeDiff) {
2023-09-29 15:56:07 +02:00
// should not be a permanent state so no spam protection required
2023-12-01 14:20:11 +01:00
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;
2023-07-31 13:14:04 +02:00
return returnvalue::FAILED;
}
2023-09-26 11:56:04 +02:00
if (not pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
2023-07-28 11:36:20 +02:00
coulombCounterCharge = openCircuitVoltageCharge;
2023-07-31 13:14:04 +02:00
} else {
2023-09-22 16:07:26 +02:00
coulombCounterCharge =
2023-12-01 14:20:11 +01:00
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDelta * SECONDS_TO_HOURS;
2023-09-26 11:56:04 +02:00
if (coulombCounterCharge >= coulombCounterChargeUpperThreshold) {
coulombCounterCharge = coulombCounterChargeUpperThreshold;
}
2023-07-28 11:36:20 +02:00
}
2023-07-31 13:14:04 +02:00
return returnvalue::OK;
2023-07-26 17:01:48 +02:00
}
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;
}
2023-09-27 16:36:09 +02:00
float PowerController::charge2stateOfCharge(float capacity, bool coulombCounter) {
if (coulombCounter) {
return capacity / coulombCounterChargeUpperThreshold;
}
return capacity / batteryMaximumCapacity;
2023-07-26 17:01:48 +02:00
}
2023-07-28 11:36:20 +02:00
float PowerController::linearInterpolation(float x, float x0, float x1, float y0, float y1) {
return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
}
2023-07-31 11:51:03 +02:00
2023-10-09 15:36:49 +02:00
ReturnValue_t PowerController::lookUpTableOcvIdxFinder(float voltage, uint8_t &idx, bool paramCmd) {
2023-09-22 10:08:04 +02:00
if (voltage >= lookUpTableOcv[1][99]) {
2023-10-09 15:36:49 +02:00
if (not voltageOutOfBoundsFlag and not paramCmd) {
2023-09-29 15:56:07 +02:00
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;
2023-07-31 11:51:03 +02:00
return returnvalue::FAILED;
} else if (voltage <= lookUpTableOcv[1][0]) {
2023-10-09 15:36:49 +02:00
if (not voltageOutOfBoundsFlag and not paramCmd) {
2023-09-29 15:56:07 +02:00
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;
2023-07-31 11:51:03 +02:00
return returnvalue::FAILED;
}
2023-09-29 15:56:07 +02:00
voltageOutOfBoundsFlag = false;
2023-07-31 11:51:03 +02:00
while (lookUpTableOcv[1][idx] > voltage) {
idx--;
}
return returnvalue::OK;
}
2023-07-31 13:14:04 +02:00
ReturnValue_t PowerController::calculateCoulombCounterChargeUpperThreshold() {
2023-09-13 16:29:54 +02:00
uint8_t lookUpTableIdx = LOOK_UP_TABLE_MAX_IDX;
2023-07-31 13:14:04 +02:00
ReturnValue_t result =
2023-10-09 15:36:49 +02:00
lookUpTableOcvIdxFinder(coulombCounterVoltageUpperThreshold, lookUpTableIdx, true);
2023-07-31 13:14:04 +02:00
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;
}