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

SoC Calculator #754

Merged
muellerr merged 74 commits from soc-calculator into main 2023-10-11 10:50:48 +02:00
Conversation 1 Commits 74 Files Changed 52 +99 -48
4 changed files with 99 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit a0025030f1 - Show all commits

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);
}

17
mission/controller/PowerController.h
View File

@ -43,15 +43,19 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
void performControlOperation() override;
void calculateStateOfCharge();
void calculateOpenCircuitVoltageCharge();
void calculateCoulombCounterCharge();
ReturnValue_t updateEpsData();
float charge2stateOfCharge(float capacity);
float linearInterpolation(float x, float x0, float x1, float y0, float y1);
// Parameters
float batteryInternalResistance = 70 / 2 / 1000; // [Ohm]
float batteryMaximumCapacity = 2.6 * 2; // [Ah]
float batteryInternalResistance = 70.0 / 2.0 / 1000.0; // [Ohm]
float batteryMaximumCapacity = 2.6 * 2; // [Ah]
float coulombCounterVoltageUpperThreshold = 16.2e3; // [mV]
// OCV Look-up-Table
float lookUpTableOcv[2][25] = {
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,
@ -89,7 +93,12 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
16.67249918, 16.74926904}};
// Variables
timeval now;
timeval oldTime;
int16_t iBat = 0.0; // [mA]
float openCircuitVoltageCharge = 0.0; // [mC]
float coulombCounterCharge = 0; // [mC]
float coulombCounterChargeUpperThreshold = 0.0; // [mC]
// HK Datasets for Calculation
BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
@ -98,7 +107,7 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
PDU2::Pdu2CoreHk pdu2CoreHk = PDU2::Pdu2CoreHk(objects::PDU2_HANDLER);
ACU::CoreHk acuCoreHk = ACU::CoreHk(objects::ACU_HANDLER);
// Output Dataset
pwrctrl::StateOfChargedData stateOfChargeData;
pwrctrl::CoreHk pwrCtrlCoreHk;
};
#endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */

14
mission/controller/controllerdefinitions/PowerCtrlDefinitions.h
View File

@ -1,27 +1,31 @@
#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 { STATE_OF_CHARGE_DATA };
enum SetIds : uint32_t { CORE_HK };
enum PoolIds : lp_id_t {
TOTAL_BATTERY_CURRENT,
OPEN_CIRCUIT_VOLTAGE_CHARGE,
COULOMB_COUNTER_CHARGE,
};
static constexpr uint8_t STATE_OF_CHARGE_ENTRIES = 2;
static constexpr uint8_t CORE_HK_ENTRIES = 3;
class StateOfChargedData : public StaticLocalDataSet<STATE_OF_CHARGE_ENTRIES> {
class CoreHk : public StaticLocalDataSet<CORE_HK_ENTRIES> {
public:
StateOfChargedData(HasLocalDataPoolIF* hkOwner)
: StaticLocalDataSet(hkOwner, STATE_OF_CHARGE_DATA) {}
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 =

7
mission/power/defs.h
View File

@ -47,6 +47,13 @@ static constexpr Event SWITCHING_Q7S_DENIED = event::makeEvent(SUBSYSTEM_ID, 2,
static constexpr Event FDIR_REACTION_IGNORED = event::makeEvent(SUBSYSTEM_ID, 3, severity::MEDIUM);
//! [EXPORT] : [COMMENT] The dataset read for the inputs of the Power Controller has failed.
static constexpr Event DATASET_READ_FAILED = event::makeEvent(SUBSYSTEM_ID, 4, severity::INFO);
//! [EXPORT] : [COMMENT] The battery voltage read is out of the bounds in which it is supposed to
//! be.
//! P1: 1 too high, 0 too low
static constexpr Event VOLTAGE_OUT_OF_BOUNDS = event::makeEvent(SUBSYSTEM_ID, 5, severity::HIGH);
enum class States { IDLE, SWITCHING_POWER, CHECKING_POWER, MODE_COMMANDING };
enum class OpCodes { NONE, TO_OFF_DONE, TO_NOT_OFF_DONE, TIMEOUT_OCCURED };