basics

mission/controller/PowerController.cpp

@@ -34,7 +34,20 @@ ReturnValue_t PowerController::getParameter(uint8_t domainId, uint8_t parameterI
                                             ParameterWrapper *parameterWrapper,
                                             const ParameterWrapper *newValues,
                                             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() {
@@ -53,6 +66,7 @@ void PowerController::performControlOperation() {
     case InternalState::READY: {
       // if (mode != MODE_OFF) {
       sif::debug << "oh shit, now i gotta do something" << std::endl;
+      calculateStateOfCharge();
       // do something
       //}
       break;
@@ -69,6 +83,8 @@ ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &loca
 
 LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
   switch (sid.ownerSetId) {
+    case pwrctrl::STATE_OF_CHARGE_DATA:
+      return &stateOfChargeData;
     default:
       return nullptr;
   }
@@ -86,3 +102,86 @@ ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
   }
   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;
+}