source: trunk/fw_g473rct/SES/src/wh_counter.c @ 28

/*!
 *      \file           wh_counter.c
 *      \author         ECS, Zhambolat Kazharov
 *      \brief
 *
 */

#include <math.h>
#include <stdint.h>

#include "stm32g4xx_hal.h"
#include "sysdata.h"
#include "wh_counter.h"

//static int64_t mWs_AutoMode;

void WH_COUNTER_CalcSoH(void);


void WH_COUNTER_Init(void)
{
  sys_data.s.values.mWs_AutoMode =   (int32_t)-sys_data.s.parameter.battEnergy * 3600;;
}


//------------------------------------------------------------------------------

/*!
 *      \brief  Calculates Wh periodically (1s)
 *
 *                      Must be called every second
 *
 */

void WH_COUNTER_Exec(void) {

  static uint64_t totalDischarge = 0;
  static uint64_t totalCharge = 0;

  if (totalDischarge == 0) totalDischarge = sys_data.s.values.dischargeTotalWh * 3600000;
  if (totalCharge == 0) totalCharge = sys_data.s.values.chargeTotalWh * 3600000;

  double cefwh;
  if (sys_data.s.values.calculatedCEFWh < 0)
  {
        cefwh =  sys_data.s.parameter.cef / 100.0;
  }
  else
  {
        cefwh =  sys_data.s.values.calculatedCEFWh / 1000.0;
  }
  
  double realStrom;
  realStrom = (int32_t) sys_data.s.values.batteryCurrent - sys_data.s.parameter.extraDischargeStrom_mA;

  //------------ separater CEF -----------
  // bei Strom größer 0 -> Ladestrom CEF rechnen
  if (realStrom >= 0) {// 99 --> 99% --> 0.99
    sys_data.s.values.correctedStromForChargeWhCnt = (realStrom * cefwh) ;    
  } 
  else 
  {
    sys_data.s.values.correctedStromForChargeWhCnt = sys_data.s.values.batteryCurrentCorrected;
  }

  // mW = (mA * mV) / 1000
  int64_t i_mA = sys_data.s.values.correctedStromForChargeWhCnt;
  int64_t v_mV = sys_data.s.values.batteryVoltage;
  int64_t p_mW = (i_mA * v_mV) / 1000LL;

  int64_t E_mWh = (int64_t)sys_data.s.parameter.battEnergy;
  int64_t battEnergy_mWs = E_mWh * 3600LL; // Umrechnung mWh zu mWs

  // Aufsummieren
  sys_data.s.values.mWsCounter += p_mW; // Energy value for both positive and negative currents
  sys_data.s.values.mWs_AutoMode += p_mW;


  // Begrenzen, Batterie darf nicht über 100% gehen
  if (sys_data.s.values.mWsCounter > battEnergy_mWs) {
    sys_data.s.values.mWsCounter = battEnergy_mWs;
  }

  // Autmode Zähler, zählen von 0 Rückwärts und sollen nicht über 0 steigen
  if (sys_data.s.values.mWs_AutoMode > 0) {
    sys_data.s.values.mWs_AutoMode = 0;
  }

  sys_data.s.values.mWh_AutoMode = sys_data.s.values.mWs_AutoMode / 3600LL;
  sys_data.s.values.mWhCounter = sys_data.s.values.mWsCounter / 3600LL;

  // Counting Total Power
  if (sys_data.s.values.batteryPower < 0)
  {
          totalDischarge += -sys_data.s.values.batteryPower;
          sys_data.s.values.dischargeTotalWh = totalDischarge / 3600000; //Umrechnung von mWs auf Wh    
  }
  else
  {
          totalCharge += sys_data.s.values.batteryPower;
          sys_data.s.values.chargeTotalWh = totalCharge / 3600000; //Umrechnung von mWs auf Wh
  }
}

//------------------------------------------------------------------------------

void WH_COUNTER_SetDetectedEnergy(void) {
  sys_data.s.values.detectedEnergy = sys_data.s.values.mWh_AutoMode >= 0 ? sys_data.s.values.mWh_AutoMode : -sys_data.s.values.mWh_AutoMode;
  WH_COUNTER_CalcSoH();
}

//------------------------------------------------------------------------------

/*!
 *      \brief  Returns Soc in m%
 *
 *      \return SoC value in m%
 */

int32_t WH_COUNTER_GetSoCManual(void) {
  int64_t E_mWh = sys_data.s.parameter.battEnergy;
  int64_t battEnergy_mWs = E_mWh * 3600LL;

  int64_t SoC = 0LL;
  if (battEnergy_mWs != 0LL)
    SoC = (100000LL * sys_data.s.values.mWsCounter) / battEnergy_mWs;
  else
    SoC = 0LL;

  return (int32_t)SoC;
}

//------------------------------------------------------------------------------

/*!
 *      \brief  Returns Soc in m%
 *
 *      \return SoC value in m%
 */

int32_t WH_COUNTER_GetSoCAuto(void) {
  // int64_t E_mWh = sys_data.s.parameter.cellEnergy;
  // int64_t cellEnergy_mWs = E_mWh * 3600LL;
  const int64_t _100mPercent = 100000LL;

  int64_t mWh_AutoMode = sys_data.s.values.mWh_AutoMode < 0 ? -sys_data.s.values.mWh_AutoMode : 0;
  int64_t SoC = 0LL;
  if (sys_data.s.values.detectedEnergy <= 0)
  {
    SoC = _100mPercent - (_100mPercent * mWh_AutoMode) / (int64_t)sys_data.s.parameter.battEnergy;
  }
  else
  {
    SoC = _100mPercent - (_100mPercent * mWh_AutoMode) / (int64_t)sys_data.s.values.detectedEnergy;
  }

  if (SoC > _100mPercent)
    SoC = _100mPercent;
  else if (SoC <= 0LL)
    SoC = 0LL;

  return (int32_t)SoC;
}


int32_t WH_COUNTER_GetSoCAutoTemp(void) {
  // int64_t E_mWh = sys_data.s.parameter.cellEnergy;
  // int64_t cellEnergy_mWs = E_mWh * 3600LL;
  const int64_t _100mPercent = 100000LL;
  int32_t SoC = 0LL;

  // Verbleibene mAh
  int64_t rmAh;
  if (sys_data.s.values.detectedEnergy <= 0)
  {
    rmAh = sys_data.s.parameter.cellCapacity - (-sys_data.s.values.mAh_AutoMode); // 40000
  }
  else {
    rmAh = sys_data.s.values.detectedCapacity - (-sys_data.s.values.mAh_AutoMode); // 40000
  }


  // verbleibene Energie
  // dazu zunächst den Mittelwert der noch verbleibenden Spannung vom aktuellen Zeitpunkt bis zur Abschaltung ermittelndazu
  int64_t avgVoltage = (sys_data.s.values.batteryVoltage + sys_data.s.values.uBatEmptyTempComp) / 2;


  //Jetzt mit der verbleibene Kapazität die verbleibene Energie unter den aktuellen Bedingungen ermitteln (Spannung bei akt. Temp)
  int64_t rP = (rmAh * avgVoltage) / 1000LL;


  if (sys_data.s.values.detectedEnergy > 0)
  {
    SoC = (_100mPercent * rP) / sys_data.s.values.detectedEnergy;
  }
  else {
    SoC = (_100mPercent * rP) / sys_data.s.parameter.battEnergy;
  }


  if (SoC > _100mPercent)
    SoC = _100mPercent;
  else if (SoC <= 0LL)
    SoC = 0LL;

  return SoC;
}

//------------------------------------------------------------------------------

void WH_COUNTER_SetToMax(void) {
  int64_t E_mWh = sys_data.s.parameter.battEnergy;
  int64_t battEnergy_mWs = E_mWh * 3600LL;

  sys_data.s.values.mWsCounter = battEnergy_mWs;

  sys_data.s.values.mWs_AutoMode = 0LL;
  sys_data.s.values.mWh_AutoMode = 0;

  sys_data.s.values.lastTimeVbatFull = 0U;
}

//------------------------------------------------------------------------------

void WH_COUNTER_CalcSoH(void)
{
        const int64_t _promille = 1000LL;

        if (sys_data.s.values.detectedCapacity < 0) sys_data.s.values.SoH = -1; // SoH was not yet calculated
        else
        {
                uint32_t detectedCapacity_mAh = sys_data.s.values.detectedCapacity;

                if (detectedCapacity_mAh >= sys_data.s.parameter.cellCapacity) sys_data.s.values.SoH = (int32_t)_promille;
                else
                {
                        if (sys_data.s.parameter.cellCapacity == 0U)
                                sys_data.s.values.SoH = -1;
                        else
                                sys_data.s.values.SoH = (int32_t)((_promille * (int64_t)detectedCapacity_mAh) / (int64_t)sys_data.s.parameter.cellCapacity);
                }
        }
}