Merge branch 'main' into persistent-tle-store
All checks were successful
EIVE/eive-obsw/pipeline/pr-dev-7.5.0 This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-dev-7.5.0 This commit looks good
This commit is contained in:
@ -1,6 +1,7 @@
|
||||
if(TGT_BSP MATCHES "arm/q7s" OR TGT_BSP MATCHES "")
|
||||
target_sources(${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp
|
||||
AcsController.cpp)
|
||||
target_sources(
|
||||
${LIB_EIVE_MISSION} PRIVATE ThermalController.cpp AcsController.cpp
|
||||
PowerController.cpp)
|
||||
endif()
|
||||
|
||||
add_subdirectory(acs)
|
||||
|
||||
373
mission/controller/PowerController.cpp
Normal file
373
mission/controller/PowerController.cpp
Normal file
@ -0,0 +1,373 @@
|
||||
#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::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 (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;
|
||||
}
|
||||
131
mission/controller/PowerController.h
Normal file
131
mission/controller/PowerController.h
Normal file
@ -0,0 +1,131 @@
|
||||
#ifndef MISSION_CONTROLLER_POWERCONTROLLER_H_
|
||||
#define MISSION_CONTROLLER_POWERCONTROLLER_H_
|
||||
|
||||
#include <eive/objects.h>
|
||||
#include <fsfw/controller/ExtendedControllerBase.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/parameters/ParameterHelper.h>
|
||||
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
|
||||
#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
|
||||
#include <mission/power/bpxBattDefs.h>
|
||||
#include <mission/power/gsDefs.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
class PowerController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
|
||||
public:
|
||||
static constexpr dur_millis_t INIT_DELAY = 500;
|
||||
|
||||
PowerController(object_id_t objectId, bool enableHkSets);
|
||||
|
||||
MessageQueueId_t getCommandQueue() const;
|
||||
ReturnValue_t getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) override;
|
||||
|
||||
private:
|
||||
bool enableHkSets = false;
|
||||
ParameterHelper parameterHelper;
|
||||
|
||||
enum class InternalState { STARTUP, INITIAL_DELAY, INIT, READY };
|
||||
InternalState internalState = InternalState::STARTUP;
|
||||
|
||||
// Initial delay to make sure all pool variables have been initialized their owners
|
||||
Countdown initialCountdown = Countdown(INIT_DELAY);
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
||||
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) override;
|
||||
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
|
||||
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t* msToReachTheMode) override;
|
||||
void performControlOperation() override;
|
||||
|
||||
void calculateStateOfCharge();
|
||||
void watchStateOfCharge();
|
||||
ReturnValue_t calculateOpenCircuitVoltageCharge();
|
||||
ReturnValue_t calculateCoulombCounterCharge();
|
||||
ReturnValue_t updateEpsData();
|
||||
float charge2stateOfCharge(float capacity, bool coulombCounter);
|
||||
ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx, bool paramCmd);
|
||||
float linearInterpolation(float x, float x0, float x1, float y0, float y1);
|
||||
ReturnValue_t calculateCoulombCounterChargeUpperThreshold();
|
||||
|
||||
// Parameters
|
||||
float batteryInternalResistance = 0.06798200367; // [Ohm]
|
||||
float batteryMaximumCapacity = 2.6 * 2; // [Ah]
|
||||
float coulombCounterVoltageUpperThreshold = 16.2; // [V]
|
||||
double maxAllowedTimeDiff = 1.5; // [s]
|
||||
float payloadOpLimitOn = 0.90; // [%]
|
||||
float payloadOpLimitLow = 0.75; // [%]
|
||||
float higherModesLimit = 0.6; // [%]
|
||||
|
||||
// OCV Look-up-Table {[Ah],[V]}
|
||||
static constexpr uint8_t LOOK_UP_TABLE_MAX_IDX = 99;
|
||||
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,
|
||||
4.81747620e-02, 6.89819174e-02, 9.87758887e-02, 1.41438170e-01, 2.02526713e-01,
|
||||
2.90000000e-01, 3.00000000e-01, 3.62820513e-01, 4.25641026e-01, 4.88461538e-01,
|
||||
5.51282051e-01, 6.14102564e-01, 6.76923077e-01, 7.39743590e-01, 8.02564103e-01,
|
||||
8.65384615e-01, 9.28205128e-01, 9.91025641e-01, 1.05384615e+00, 1.11666667e+00,
|
||||
1.17948718e+00, 1.24230769e+00, 1.30512821e+00, 1.36794872e+00, 1.43076923e+00,
|
||||
1.49358974e+00, 1.55641026e+00, 1.61923077e+00, 1.68205128e+00, 1.74487179e+00,
|
||||
1.80769231e+00, 1.87051282e+00, 1.93333333e+00, 1.99615385e+00, 2.05897436e+00,
|
||||
2.12179487e+00, 2.18461538e+00, 2.24743590e+00, 2.31025641e+00, 2.37307692e+00,
|
||||
2.43589744e+00, 2.49871795e+00, 2.56153846e+00, 2.62435897e+00, 2.68717949e+00,
|
||||
2.75000000e+00, 2.81282051e+00, 2.87564103e+00, 2.93846154e+00, 3.00128205e+00,
|
||||
3.06410256e+00, 3.12692308e+00, 3.18974359e+00, 3.25256410e+00, 3.31538462e+00,
|
||||
3.37820513e+00, 3.44102564e+00, 3.50384615e+00, 3.56666667e+00, 3.62948718e+00,
|
||||
3.69230769e+00, 3.75512821e+00, 3.81794872e+00, 3.88076923e+00, 3.94358974e+00,
|
||||
4.00641026e+00, 4.06923077e+00, 4.13205128e+00, 4.19487179e+00, 4.25769231e+00,
|
||||
4.32051282e+00, 4.38333333e+00, 4.44615385e+00, 4.50897436e+00, 4.57179487e+00,
|
||||
4.63461538e+00, 4.69743590e+00, 4.76025641e+00, 4.82307692e+00, 4.88589744e+00,
|
||||
4.94871795e+00, 5.01153846e+00, 5.07435897e+00, 5.13717949e+00, 5.20000000e+00},
|
||||
{12.52033533, 12.58720948, 12.61609309, 12.65612591, 12.67105282, 12.69242681, 12.72303245,
|
||||
12.76685696, 12.80313768, 12.83600741, 12.8830739, 12.94720576, 13.00112629, 13.07833563,
|
||||
13.17486308, 13.27128842, 13.37713879, 13.49275604, 13.60395193, 13.68708863, 13.75196335,
|
||||
13.7582376, 13.79298643, 13.82885799, 13.87028849, 13.91585718, 13.96701874, 14.02343574,
|
||||
14.07665641, 14.12626342, 14.1675095, 14.20582917, 14.23342159, 14.25724476, 14.27264301,
|
||||
14.28922389, 14.30898535, 14.32750837, 14.34358057, 14.35965277, 14.37698366, 14.3943261,
|
||||
14.41079196, 14.42679817, 14.44261008, 14.45771025, 14.47281042, 14.48751461, 14.50193089,
|
||||
14.5164887, 14.53193477, 14.54738084, 14.56341235, 14.58054578, 14.59799552, 14.61632769,
|
||||
14.63716465, 14.66935073, 14.70511347, 14.74315094, 14.77251031, 14.80005585, 14.8315427,
|
||||
14.86078285, 14.89444687, 14.93495892, 14.97114013, 15.01055751, 15.0538516, 15.09698825,
|
||||
15.14850029, 15.18947994, 15.24249483, 15.28521713, 15.335695, 15.37950723, 15.43241224,
|
||||
15.48082213, 15.53314287, 15.58907248, 15.64030253, 15.68385331, 15.74149122, 15.80051882,
|
||||
15.84959348, 15.90443241, 15.95743724, 16.01283068, 16.07629253, 16.13470801, 16.1890518,
|
||||
16.24200781, 16.30521118, 16.37368429, 16.43661267, 16.49604875, 16.56223813, 16.62741412,
|
||||
16.67249918, 16.74926904}};
|
||||
|
||||
// Variables
|
||||
timeval now;
|
||||
timeval oldTime;
|
||||
int16_t iBat = 0; // [mA]
|
||||
float openCircuitVoltageCharge = 0.0; // [Ah]
|
||||
float coulombCounterCharge = 0.0; // [Ah]
|
||||
float coulombCounterChargeUpperThreshold = 0.0; // [Ah]
|
||||
float oldCoulombCounterVoltageUpperThreshold = 0.0; // [V]
|
||||
|
||||
static constexpr float CONVERT_FROM_MILLI = 1e-3;
|
||||
static constexpr float SECONDS_TO_HOURS = 1. / (60. * 60.);
|
||||
|
||||
static constexpr int16_t INVALID_TOTAL_BATTERY_CURRENT = 0;
|
||||
static constexpr float INVALID_SOC = -1;
|
||||
|
||||
bool pwrLvlLowFlag = false;
|
||||
bool pwrLvlCriticalFlag = false;
|
||||
bool voltageOutOfBoundsFlag = false;
|
||||
|
||||
// HK Datasets for Calculation
|
||||
BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
|
||||
P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
|
||||
// Output Dataset
|
||||
pwrctrl::CoreHk pwrCtrlCoreHk;
|
||||
// Dataset for PL Flag
|
||||
pwrctrl::EnablePl enablePl;
|
||||
};
|
||||
|
||||
#endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */
|
||||
@ -0,0 +1,51 @@
|
||||
#ifndef MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_
|
||||
#define MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||
#include <fsfw/datapoollocal/localPoolDefinitions.h>
|
||||
#include <mission/power/defs.h>
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace pwrctrl {
|
||||
|
||||
enum SetIds : uint32_t { CORE_HK, ENABLE_PL };
|
||||
|
||||
enum PoolIds : lp_id_t {
|
||||
TOTAL_BATTERY_CURRENT,
|
||||
OPEN_CIRCUIT_VOLTAGE_CHARGE,
|
||||
COULOMB_COUNTER_CHARGE,
|
||||
PAYLOAD_FLAG
|
||||
};
|
||||
|
||||
static constexpr uint8_t CORE_HK_ENTRIES = 3;
|
||||
static constexpr uint8_t ENABLE_PL_ENTRIES = 1;
|
||||
|
||||
class CoreHk : public StaticLocalDataSet<CORE_HK_ENTRIES> {
|
||||
public:
|
||||
CoreHk(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CORE_HK) {}
|
||||
|
||||
lp_var_t<int16_t> totalBatteryCurrent =
|
||||
lp_var_t<int16_t>(sid.objectId, TOTAL_BATTERY_CURRENT, this);
|
||||
lp_var_t<float> openCircuitVoltageCharge =
|
||||
lp_var_t<float>(sid.objectId, OPEN_CIRCUIT_VOLTAGE_CHARGE, this);
|
||||
lp_var_t<float> coulombCounterCharge =
|
||||
lp_var_t<float>(sid.objectId, COULOMB_COUNTER_CHARGE, this);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
class EnablePl : public StaticLocalDataSet<ENABLE_PL_ENTRIES> {
|
||||
public:
|
||||
EnablePl(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, ENABLE_PL) {}
|
||||
EnablePl(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, ENABLE_PL)) {}
|
||||
|
||||
lp_var_t<uint8_t> plUseAllowed = lp_var_t<uint8_t>(sid.objectId, PAYLOAD_FLAG, this);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
} // namespace pwrctrl
|
||||
|
||||
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_POWERCTRLDEFINITIONS_H_ */
|
||||
Reference in New Issue
Block a user