This commit is contained in:
parent
8c97ad0213
commit
88a8969142
@ -34,7 +34,20 @@ ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterI
|
|||||||
ParameterWrapper *parameterWrapper,
|
ParameterWrapper *parameterWrapper,
|
||||||
const ParameterWrapper *newValues,
|
const ParameterWrapper *newValues,
|
||||||
uint16_t startAtIndex) {
|
uint16_t startAtIndex) {
|
||||||
return returnvalue::FAILED;
|
switch (domainId) {
|
||||||
|
case 0x0: // direct members
|
||||||
|
switch (parameterId) {
|
||||||
|
case 0x0:
|
||||||
|
parameterWrapper->set(batteryInternalResistance);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return INVALID_IDENTIFIER_ID;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return INVALID_DOMAIN_ID;
|
||||||
|
};
|
||||||
|
return returnvalue::OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
void PowerController::performControlOperation() {
|
void PowerController::performControlOperation() {
|
||||||
@ -53,6 +66,7 @@ void PowerController::performControlOperation() {
|
|||||||
case InternalState::READY: {
|
case InternalState::READY: {
|
||||||
// if (mode != MODE_OFF) {
|
// if (mode != MODE_OFF) {
|
||||||
sif::debug << "oh shit, now i gotta do something" << std::endl;
|
sif::debug << "oh shit, now i gotta do something" << std::endl;
|
||||||
|
calculateStateOfCharge();
|
||||||
// do something
|
// do something
|
||||||
//}
|
//}
|
||||||
break;
|
break;
|
||||||
@ -69,6 +83,8 @@ ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &loca
|
|||||||
|
|
||||||
LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
|
LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
|
||||||
switch (sid.ownerSetId) {
|
switch (sid.ownerSetId) {
|
||||||
|
case pwrctrl::STATE_OF_CHARGE_DATA:
|
||||||
|
return &stateOfChargeData;
|
||||||
default:
|
default:
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
@ -86,3 +102,86 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
|
|||||||
}
|
}
|
||||||
return INVALID_MODE;
|
return INVALID_MODE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void PowerController::calculateStateOfCharge() {
|
||||||
|
// get time
|
||||||
|
timeval now;
|
||||||
|
Clock::getClock_timeval(&now);
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
// commit TM
|
||||||
|
{
|
||||||
|
PoolReadGuard pg(&stateOfChargeData);
|
||||||
|
stateOfChargeData.openCircuitVoltageCharge.value =
|
||||||
|
charge2stateOfCharge(openCircuitVoltageCharge);
|
||||||
|
stateOfChargeData.coulombCounterCharge.value = charge2stateOfCharge(coulombCounterCharge);
|
||||||
|
stateOfChargeData.setValidity(true, true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// store time for next run
|
||||||
|
oldTime = now;
|
||||||
|
}
|
||||||
|
|
||||||
|
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());
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PoolReadGuard pgPdu1(&pdu1CoreHk);
|
||||||
|
results.push_back(pgPdu1.getReadResult());
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PoolReadGuard pgPdu2(&pdu2CoreHk);
|
||||||
|
results.push_back(pgPdu2.getReadResult());
|
||||||
|
}
|
||||||
|
{
|
||||||
|
PoolReadGuard pgAcu(&acuCoreHk);
|
||||||
|
results.push_back(pgAcu.getReadResult());
|
||||||
|
}
|
||||||
|
for (const auto &result : results) {
|
||||||
|
if (result != returnvalue::OK) {
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return returnvalue::OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
float PowerController::charge2stateOfCharge(float capacity) {
|
||||||
|
return capacity / batteryMaximumCapacity;
|
||||||
|
}
|
||||||
|
@ -3,12 +3,15 @@
|
|||||||
|
|
||||||
#include <eive/objects.h>
|
#include <eive/objects.h>
|
||||||
#include <fsfw/controller/ExtendedControllerBase.h>
|
#include <fsfw/controller/ExtendedControllerBase.h>
|
||||||
|
#include <fsfw/datapool/PoolReadGuard.h>
|
||||||
#include <fsfw/parameters/ParameterHelper.h>
|
#include <fsfw/parameters/ParameterHelper.h>
|
||||||
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
|
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
|
||||||
#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
|
#include <mission/controller/controllerdefinitions/PowerCtrlDefinitions.h>
|
||||||
#include <mission/power/bpxBattDefs.h>
|
#include <mission/power/bpxBattDefs.h>
|
||||||
#include <mission/power/gsDefs.h>
|
#include <mission/power/gsDefs.h>
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
class PowerController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
|
class PowerController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
|
||||||
public:
|
public:
|
||||||
static constexpr dur_millis_t INIT_DELAY = 500;
|
static constexpr dur_millis_t INIT_DELAY = 500;
|
||||||
@ -39,6 +42,55 @@ class PowerController : public ExtendedControllerBase, public ReceivesParameterM
|
|||||||
uint32_t* msToReachTheMode) override;
|
uint32_t* msToReachTheMode) override;
|
||||||
void performControlOperation() override;
|
void performControlOperation() override;
|
||||||
|
|
||||||
|
void calculateStateOfCharge();
|
||||||
|
ReturnValue_t updateEpsData();
|
||||||
|
float charge2stateOfCharge(float capacity);
|
||||||
|
|
||||||
|
// Parameters
|
||||||
|
float batteryInternalResistance = 70 / 2 / 1000; // [Ohm]
|
||||||
|
float batteryMaximumCapacity = 2.6 * 2; // [Ah]
|
||||||
|
|
||||||
|
// OCV Look-up-Table
|
||||||
|
float lookUpTableOcv[2][25] = {
|
||||||
|
{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 oldTime;
|
||||||
|
|
||||||
// HK Datasets for Calculation
|
// HK Datasets for Calculation
|
||||||
BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
|
BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
|
||||||
P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
|
P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
|
||||||
|
@ -11,16 +11,22 @@ namespace pwrctrl {
|
|||||||
enum SetIds : uint32_t { STATE_OF_CHARGE_DATA };
|
enum SetIds : uint32_t { STATE_OF_CHARGE_DATA };
|
||||||
|
|
||||||
enum PoolIds : lp_id_t {
|
enum PoolIds : lp_id_t {
|
||||||
|
OPEN_CIRCUIT_VOLTAGE_CHARGE,
|
||||||
|
COULOMB_COUNTER_CHARGE,
|
||||||
};
|
};
|
||||||
|
|
||||||
static constexpr uint8_t STATE_OF_CHARGE_ENTRIES = 6;
|
static constexpr uint8_t STATE_OF_CHARGE_ENTRIES = 2;
|
||||||
|
|
||||||
class StateOfChargedData : public StaticLocalDataSet<STATE_OF_CHARGE_ENTRIES> {
|
class StateOfChargedData : public StaticLocalDataSet<STATE_OF_CHARGE_ENTRIES> {
|
||||||
public:
|
public:
|
||||||
StateOfChargedData(HasLocalDataPoolIF* hkOwner)
|
StateOfChargedData(HasLocalDataPoolIF* hkOwner)
|
||||||
: StaticLocalDataSet(hkOwner, STATE_OF_CHARGE_DATA) {}
|
: StaticLocalDataSet(hkOwner, STATE_OF_CHARGE_DATA) {}
|
||||||
|
|
||||||
|
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:
|
private:
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -133,6 +133,9 @@ ReturnValue_t pst::pstGompaceCan(FixedTimeslotTaskIF *thisSequence) {
|
|||||||
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
|
thisSequence->addSlot(objects::PDU1_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
|
||||||
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
|
thisSequence->addSlot(objects::PDU2_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
|
||||||
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
|
thisSequence->addSlot(objects::ACU_HANDLER, length * 0.5, DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
|
thisSequence->addSlot(objects::POWER_CONTROLLER, length * 0.6, 0);
|
||||||
|
|
||||||
if (thisSequence->checkSequence() != returnvalue::OK) {
|
if (thisSequence->checkSequence() != returnvalue::OK) {
|
||||||
sif::error << "GomSpace PST initialization failed" << std::endl;
|
sif::error << "GomSpace PST initialization failed" << std::endl;
|
||||||
return returnvalue::FAILED;
|
return returnvalue::FAILED;
|
||||||
@ -591,9 +594,6 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
|
|||||||
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
|
thisSequence->addSlot(objects::BPX_BATT_HANDLER, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
|
||||||
DeviceHandlerIF::GET_READ);
|
DeviceHandlerIF::GET_READ);
|
||||||
|
|
||||||
thisSequence->addSlot(objects::POWER_CONTROLLER, length * config::spiSched::SCHED_BLOCK_10_PERIOD,
|
|
||||||
0);
|
|
||||||
|
|
||||||
/* Radiation sensor */
|
/* Radiation sensor */
|
||||||
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
|
thisSequence->addSlot(objects::RAD_SENSOR, length * config::spiSched::SCHED_BLOCK_9_PERIOD,
|
||||||
DeviceHandlerIF::PERFORM_OPERATION);
|
DeviceHandlerIF::PERFORM_OPERATION);
|
||||||
|
Loading…
Reference in New Issue
Block a user