378 lines
13 KiB
C++
378 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_OFF) {
|
|
calculateStateOfCharge();
|
|
if (mode == MODE_NORMAL) {
|
|
watchStateOfCharge();
|
|
} else {
|
|
PoolReadGuard pg(&enablePl);
|
|
if (pg.getReadResult() == returnvalue::OK) {
|
|
enablePl.setValidity(false, true);
|
|
}
|
|
}
|
|
}
|
|
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::getClock_timeval(&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);
|
|
}
|
|
}
|
|
// store time for next run
|
|
oldTime = now;
|
|
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);
|
|
}
|
|
}
|
|
// store time for next run
|
|
oldTime = now;
|
|
return;
|
|
}
|
|
|
|
result = calculateCoulombCounterCharge();
|
|
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);
|
|
}
|
|
}
|
|
// store time for next run
|
|
oldTime = now;
|
|
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);
|
|
}
|
|
}
|
|
// store time for next run
|
|
oldTime = now;
|
|
}
|
|
|
|
void PowerController::watchStateOfCharge() {
|
|
if (test < 100) {
|
|
test++;
|
|
} else if (test == 100) {
|
|
// pwrCtrlCoreHk.coulombCounterCharge.value = 0.7;
|
|
}
|
|
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 timeDiff = timevalOperations::toDouble(now - oldTime);
|
|
if (timeDiff > maxAllowedTimeDiff) {
|
|
// should not be a permanent state so no spam protection required
|
|
triggerEvent(power::TIMEDELTA_OUT_OF_BOUNDS, static_cast<uint32_t>(timeDiff * 10));
|
|
sif::error << "Power Controller::Time delta too large for Coulomb Counter: " << timeDiff
|
|
<< std::endl;
|
|
return returnvalue::FAILED;
|
|
}
|
|
if (not pwrCtrlCoreHk.coulombCounterCharge.isValid()) {
|
|
coulombCounterCharge = openCircuitVoltageCharge;
|
|
} else {
|
|
coulombCounterCharge =
|
|
coulombCounterCharge + iBat * CONVERT_FROM_MILLI * timeDiff * 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;
|
|
}
|