eive-obsw/mission/controller/PowerController.cpp

#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) {}

ReturnValue_t PowerController::initialize() {
  ReturnValue_t result = parameterHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  sif::debug << "Rush B, no stop" << std::endl;
  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;
        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::READY;
      }
      return;
    }
    case InternalState::READY: {
      // if (mode != MODE_OFF) {
      sif::debug << "oh shit, now i gotta do something" << std::endl;
      calculateStateOfCharge();
      // do something
      //}
      break;
    }
    default:
      break;
  }
}

ReturnValue_t PowerController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                       LocalDataPoolManager &poolManager) {
  return returnvalue::OK;
}

LocalPoolDataSetBase *PowerController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case pwrctrl::STATE_OF_CHARGE_DATA:
      return &stateOfChargeData;
    default:
      return nullptr;
  }
  return nullptr;
}

ReturnValue_t PowerController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                uint32_t *msToReachTheMode) {
  if (mode == MODE_OFF) {
    if (submode == SUBMODE_NONE) {
      return returnvalue::OK;
    } else {
      return INVALID_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;
}