eive-obsw/mission/controller/PowerController.h

#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);
  ReturnValue_t lookUpTableOcvIdxFinder(float voltage, uint8_t& idx);
  float linearInterpolation(float x, float x0, float x1, float y0, float y1);
  ReturnValue_t calculateCoulombCounterChargeUpperThreshold();

  // Parameters
  float batteryInternalResistance = 70.0 / 2.0 / 1000.0;  // [Ohm]
  float batteryMaximumCapacity = 2.6 * 2;                 // [Ah]
  float coulombCounterVoltageUpperThreshold = 16.2e3;     // [mV]
  double maxAllowedTimeDiff = 0.5;                        // [s]

  // 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]

  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;

  // HK Datasets for Calculation
  BpxBatteryHk bpxBatteryHk = BpxBatteryHk(objects::BPX_BATT_HANDLER);
  P60Dock::CoreHkSet p60CoreHk = P60Dock::CoreHkSet(objects::P60DOCK_HANDLER);
  // Output Dataset
  pwrctrl::CoreHk pwrCtrlCoreHk;
};

#endif /* MISSION_CONTROLLER_POWERCONTROLLER_H_ */