KlausBMS.ino

/**
 * ==========================================================================
 * Klaus' Twizy Lithium BMS
 * ==========================================================================
 * 
 * Hardware setup:
 *  - basically like Twizy-Battery-Part-List.md#example-arduino-wiring-scheme
 *  - supports packs with up to 16 cells (see CELL_COUNT in _config.h)
 * 
 * Author:
 *  - Michael Balzer        <dexter@dexters-web.de>
 * 
 * Ideas, support & testing:
 *  - Błażej Błaszczyk      <blazej.blaszczyk@pascal-engineering.com>
 *  - Klaus Zinser          <klauszinser@posteo.eu>
 * 
 * Libraries used:
 *  - TwizyVirtualBMS   https://github.com/dexterbg/Twizy-Virtual-BMS
 *  - MCP_CAN           https://github.com/coryjfowler/MCP_CAN_lib
 *  - FlexiTimer2       https://github.com/PaulStoffregen/FlexiTimer2
 *  - AltSoftSerial     https://github.com/PaulStoffregen/AltSoftSerial
 * 
 * License:
 *  This is free software under GNU Lesser General Public License (LGPL)
 *  https://www.gnu.org/licenses/lgpl.html
 *  
 */

#define KLAUS_BMS_VERSION    "V0.9.2 (2017-11-08)"

#include <EEPROM.h>
#include <util/crc16.h>

#include "TwizyVirtualBMS_config.h"
#include "TwizyVirtualBMS.h"
#include "AltSoftSerial.h"
#include "KlausBMS_config.h"

// VirtualBMS:
TwizyVirtualBMS twizy;

// Bluetooth software serial port:
// Note: AltSoftSerial uses fixed pins!
// i.e. Arduino Nano: RX = pin 8, TX = pin 9
//      Arduino Mega: RX = pin 48, TX = pin 46
AltSoftSerial bt;

Stream *com_channels[] = { &Serial, &bt };

char inputbuf[30] = "";     // shared command input buffer
byte inputpos = 0;

byte quiet = 0;             // status output inhibitor (seconds)
char temp_chg;              // charger temperature [°C]


// Current, capacity & charge (coulomb):
//  QA = quarter amps (1/4 A) (Twizy current resolution)
//  CS = centi seconds (1/100 s) (Twizy time resolution)
#define SCALE_CURR_QA       ((SCALE_CURR) * 4L)
#define BASE_CURR_QA        ((BASE_CURR) * 4L)
#define CAP_NOMINAL_QACS    ((CAP_NOMINAL_AH) * 4L * 3600L * 100L)
#define AMPS(qa)            ((float) (qa) / 4L)
#define AMPHOURS(qacs)      ((float) (qacs) / 4L / 3600L / 100L)


// --------------------------------------------------------------------------
// KlausBMS main object
//

class KlausBMS {

public:
  
  // --------------------------------------------------------------------------
  // State variables
  // 
  
  float
    temp_f,                       // pack front temperature [°C]
    temp_r,                       // pack rear temperature [°C]
    tdif;                         // difference front/rear temperature [°C]

  float
    vpack,                        // pack voltage [V]
    vstack[CELL_COUNT],           // stacked voltages [V]
    vcell[CELL_COUNT];            // cell voltages [V]

  float
    cmin,                         // minimum cell voltage [V]
    cmax,                         // maximum cell voltage [V]
    cdif;                         // difference min/max cell voltage [V]
  byte
    cmin_i,                       // cell number with min voltage
    cmax_i;                       // cell number with max voltage
  float
    cmin_soc,                     // minimum cell voltage based SOC [%]
    cmax_soc;                     // maximum cell voltage based SOC [%]

  float
    soc,                          // effective SOC [%] combining …
    soc_volt,                     // … voltage based SOC estimation [%]
    soc_coulomb;                  // … charge based SOC estimation [%]

  float
    soh;                          // state of health [%]

  unsigned int
    drvpwr,                       // drive power level [W]
    recpwr;                       // recuperation power level [W]
  byte
    chgcur;                       // charge current level [A]

  unsigned long
    error;                        // display error status (0 = OK, see setError())
  byte
    bms_error;                    // informational BMS error code (see TwizyBmsError / setInfoError())

  unsigned long
    cap_qacs,                     // battery charge capacity (self-adjusting) [1/400 As]
    avail_qacs;                   // available charge [1/400 As]
  
  signed long
    curr_qa;                      // momentary current (10 ms measurement) [1/4 A]

  float
    soc_chgstart;                 // charge start SOC [%]
  unsigned long
    avail_qacs_chgstart;          // charge start coulomb count [1/400 As]


  // --------------------------------------------------------------------------
  // Configuration variables
  // 
  
  // Maximum charge current to use [A]
  // …at 20 °C & higher:
  byte max_charge_current;
  // …at zero °C:
  byte max_charge_current_0c;

  // Maximum driving & recuperation power limits to use [W]
  // …at 20 °C & higher:
  unsigned int max_drive_power;
  unsigned int max_recup_power;
  // …at zero °C:
  unsigned int max_drive_power_0c;
  unsigned int max_recup_power_0c;

  // Drive power cutback [%]:
  // (100% at FULL → 100% at <SOC1>% → <LVL2>% at <SOC2>% → 0% at EMPTY)
  byte drv_cutback_soc1;
  byte drv_cutback_soc2;
  byte drv_cutback_lvl2;

  // Voltage range for discharging [V]:
  float vmin_drv;
  float vmax_drv;

  // Voltage range for charging [V]:
  float vmin_chg;
  float vmax_chg;

  // Prioritize voltage based SOC [%]:
  byte soc_volt_prio_above;
  byte soc_volt_prio_below;

  // Degrade coulomb based SOC [%]:
  byte soc_coul_degr_above;
  byte soc_coul_degr_below;

  // Charge stop at SOC [%]:
  byte chg_stop_soc;

public:
  
  // --------------------------------------------------------------------------
  // Init
  // 
  
  KlausBMS() {
    init();
  }

  void initSOC(float newsoc) {
    
    soc = newsoc;
    soc_volt = newsoc;
    
    #ifdef PORT_CURR
      soc_coulomb = newsoc;
      avail_qacs = cap_qacs / 100 * soc_coulomb;
    #endif

    float cv;
    
    if (twizy.inState(Charging)) {
      cv = VMIN_CHG + (VMAX_CHG - VMIN_CHG) * newsoc / 100.0;
    } else {
      cv = VMIN_DRV + (VMAX_DRV - VMIN_DRV) * newsoc / 100.0;
    }

    for (int i=0; i < CELL_COUNT; i++) {
      vcell[i] = cv;
      vstack[i] = cv * (i+1);
    }
    
    vpack = vstack[CELL_COUNT-1];
    
    cmin_soc = cmax_soc = newsoc;
    cmin = cmax = cv;
  }

  void initSOH(float newsoh) {
    
    soh = newsoh;
    
    #ifdef PORT_CURR
      cap_qacs = CAP_NOMINAL_QACS / 100 * soh;
      avail_qacs = cap_qacs / 100 * soc_coulomb;
    #endif
  }
  
  void init() {
    
    // State variables:
    
    temp_f = 20.0;
    temp_r = 20.0;
    tdif = 0;

    initSOC(99.0);
    initSOH(100.0);

    drvpwr = MAX_DRIVE_POWER;
    recpwr = MAX_RECUP_POWER;
    chgcur = MAX_CHARGE_CURRENT;

    error = TWIZY_OK;
    bms_error = bmsError_None;

    cap_qacs = CAP_NOMINAL_QACS;
    avail_qacs = 0.99 * CAP_NOMINAL_QACS;
  
    curr_qa = 0;

    soc_chgstart = 100;
    avail_qacs_chgstart = 0;
  

    // Configuration variables:
    
    max_charge_current = MAX_CHARGE_CURRENT;
    
    max_charge_current_0c = MAX_CHARGE_CURRENT_0C;
    
    max_drive_power = MAX_DRIVE_POWER;
    max_recup_power = MAX_RECUP_POWER;

    max_drive_power_0c = MAX_DRIVE_POWER_0C;
    max_recup_power_0c = MAX_RECUP_POWER_0C;

    drv_cutback_soc1 = DRV_CUTBACK_SOC1;
    drv_cutback_soc2 = DRV_CUTBACK_SOC2;
    drv_cutback_lvl2 = DRV_CUTBACK_LVL2;

    vmin_drv = VMIN_DRV;
    vmax_drv = VMAX_DRV;

    vmin_chg = VMIN_CHG;
    vmax_chg = VMAX_CHG;

    soc_volt_prio_above = SOC_VOLT_PRIO_ABOVE;
    soc_volt_prio_below = SOC_VOLT_PRIO_BELOW;

    soc_coul_degr_above = SOC_COUL_DEGR_ABOVE;
    soc_coul_degr_below = SOC_COUL_DEGR_BELOW;
    
    chg_stop_soc = 100;
  }
  
  void begin() {
    
    // Init I/O ports:
    
    pinMode(PORT_VOLT, INPUT);
    pinMode(PORT_TEMP_F, INPUT);
    pinMode(PORT_TEMP_R, INPUT);
    #ifdef PORT_CURR
      pinMode(PORT_CURR, INPUT);
    #endif
    
    pinMode(PORT_MUX_S0, OUTPUT);
    pinMode(PORT_MUX_S1, OUTPUT);
    pinMode(PORT_MUX_S2, OUTPUT);
    pinMode(PORT_MUX_S3, OUTPUT);
    
    // load state from EEPROM:
    
    loadState();
    
    // init VirtualBMS state:
    
    twizy.setInfoBmsType(bmsType_VirtualBMS);
    twizy.setInfoState1(twizy.state());
    twizy.setInfoState2(0);
    twizy.setInfoError(bms_error);
    twizy.setInfoBalancing(0);
    
    twizy.setPowerLimits(drvpwr, recpwr);
    twizy.setChargeCurrent(chgcur);
    twizy.setSOC(soc);
    twizy.setTemperature(temp_r, temp_f, true);
    twizy.setVoltage(vpack, true);
    twizy.setError(error);
    twizy.setSOH(soh);
    twizy.setCurrent(curr_qa);

    Serial.println(F("bms.begin: done"));
  }


  // --------------------------------------------------------------------------
  // EEPROM utility: save state
  // 
  void saveState() {
    uint16_t crc, i;
    uint8_t *data;
    
    // calculate CRC:
    crc = 0xffff;
    data = (uint8_t *) this;
    for (i = 0; i < sizeof(*this); i++) {
      crc = _crc16_update(crc, data[i]);
    }
    
    // write:
    EEPROM.put(0, crc);
    EEPROM.put(sizeof(crc), *this);
    
    Serial.println(F("bms.saveState: state saved"));
  }


  // --------------------------------------------------------------------------
  // EEPROM utility: load state
  // 
  void loadState() {
    uint16_t crc, savedcrc, i;
    
    // check CRC:
    EEPROM.get(0, savedcrc);
    crc = 0xffff;
    for (i = 0; i < sizeof(*this); i++) {
      crc = _crc16_update(crc, EEPROM[sizeof(crc) + i]);
    }
    
    if (crc != savedcrc) {
      Serial.println(F("bms.loadState: CRC mismatch, state not loaded!"));
    }
    else {
      // read:
      EEPROM.get(sizeof(crc), *this);
      Serial.println(F("bms.loadState: state loaded"));
    }
  }


  // --------------------------------------------------------------------------
  // Utility: output voltage alert/state details
  // 
  void printVoltAlert(FLASHSTRING *intro) {
    if (quiet) {
      return;
    }
    for (Stream *s : com_channels) {
      s->print(F("!!! ")); s->print(intro);
      s->print(F(": dif="));  s->print(cdif, 2);
      s->print(F(", min="));  s->print(cmin, 2);
      s->print(F(" #"));      s->print(cmin_i);
      s->print(F(", max="));  s->print(cmax, 2);
      s->print(F(" #"));      s->print(cmax_i);
      s->print(F(", avg="));  s->print(vpack/CELL_COUNT, 2);
      s->println();
    }
  }


  // --------------------------------------------------------------------------
  // Utility: output temperature alert/state details
  // 
  void printTempAlert(FLASHSTRING *intro) {
    if (quiet) {
      return;
    }
    for (Stream *s : com_channels) {
      s->print(F("!!! ")); s->print(intro);
      s->print(F(": dif="));     s->print(tdif, 1);
      s->print(F(", temp_f="));  s->print(temp_f, 1);
      s->print(F(", temp_r="));  s->print(temp_r, 1);
      s->println();
    }
  }


  // --------------------------------------------------------------------------
  // Utility: output space padded numbers
  // 
  
  // …integer:
  void print(Stream *s, char len, long ival) {
    unsigned long p = 10;
    
    // get length of ival:
    if (ival < 0) {
      len--;
      while (-ival >= p) {
        p *= 10;
        len--;
      }
    } else {
      while (ival >= p) {
        p *= 10;
        len--;
      }
    }
    
    // pad & print:
    while (--len > 0) {
      s->print(' ');
    }
    s->print(ival);
  }
  
  // …float:
  void print(Stream *s, char len, float fval, char prec) {
    long p;
    
    // get rounded integer part:
    p = 1;
    for (char i=0; i < prec; i++) {
      p *= 10;
    }
    long ival = ((long) round(fval * p)) / p;
    
    // get length of rounded integer part:
    p = 10;
    if (signbit(fval)) {
      len--;
      ival = -ival;
    }
    while (ival >= p) {
      p *= 10;
      len--;
    }
    
    // pad & print:
    while (--len > 0) {
      s->print(' ');
    }
    s->print(fval, prec);
  }
  
  
  // --------------------------------------------------------------------------
  // Utility: output BMS status
  // 
  
  void printStatus(Stream *s) {

    /*
      | 100.0 %SOC |   55.6 V  | 18000 Wd |  35 Ac | StartTrickle
      | 100.0 %Sv  | -500.0 A  |  8500 Wr | -10 Cc | [error]
      | 100.0 %Sc  |  120.0 Ah |   100 Cf |< 88 %V | 0.12 V
      | 100.0 %SOH |  120.0 Ah |   100 Cr |>100 %V |
      | 3.12 | 3.12 | 3.12 |<3.12 | 3.12 | 3.12 | 3.12 | 3.12 |
      | 3.12 | 3.12 | 3.12 | 3.12 |>3.12 | 3.12 | 3.12 | 3.12 |
    */
    
    s->println();
    
    s->print('|'); print(s, 4, soc, 1);                   s->print(F(" %SOC")); s->print(' ');
    s->print('|'); print(s, 5, vpack, 1);                 s->print(F(" V "));   s->print(' ');
    s->print('|'); print(s, 6, drvpwr);                   s->print(F(" Wd"));  s->print(' ');
    s->print('|'); print(s, 4, chgcur);                   s->print(F(" Ac"));  s->print(' ');
    s->print('|'); s->print(' '); s->print(twizy.stateName());
    s->println();

    s->print('|'); print(s, 4, soc_volt, 1);              s->print(F(" %Sv ")); s->print(' ');
    s->print('|'); print(s, 5, AMPS(curr_qa), 1);         s->print(F(" A "));   s->print(' ');
    s->print('|'); print(s, 6, recpwr);                   s->print(F(" Wr"));  s->print(' ');
    s->print('|'); print(s, 4, temp_chg);                 s->print(F(" Cc"));  s->print(' ');
    s->print('|');
    if (error) {
      s->print(' ');
      s->print(error, HEX);
    }
    s->println();
    
    s->print('|'); print(s, 4, soc_coulomb, 1);               s->print(F(" %Sc ")); s->print(' ');
    s->print('|'); print(s, 5, AMPHOURS(avail_qacs), 1);      s->print(F(" Ah"));   s->print(' ');
    s->print('|'); print(s, 6, temp_f);                       s->print(F(" Cf"));  s->print(' ');
    s->print('|'); s->print('<'); print(s, 3, (int)cmin_soc); s->print(F(" %V"));  s->print(' ');
    s->print('|'); print(s, 2, cdif, 2); s->print(F(" V"));
    s->println();
    
    s->print('|'); print(s, 4, soh, 1);                       s->print(F(" %SOH")); s->print(' ');
    s->print('|'); print(s, 5, AMPHOURS(cap_qacs), 1);        s->print(F(" Ah"));   s->print(' ');
    s->print('|'); print(s, 6, temp_r);                       s->print(F(" Cr"));  s->print(' ');
    s->print('|'); s->print('>'); print(s, 3, (int)cmax_soc); s->print(F(" %V"));  s->print(' ');
    s->print('|');
    s->println();
    
    // cell voltages:
    
    for (byte i=0; i < CELL_COUNT; i++) {
      if (i == CELL_COUNT/2) {
        s->println('|');
      }
      s->print('|');
      s->print((i == cmin_i) ? '<' : (i == cmax_i) ? '>' : ' ');
      s->print(vcell[i], 2);
      s->print(' ');
    }
    s->println('|');
    
  }  
  
  
  // --------------------------------------------------------------------------
  // Callback: handle state transition for BMS
  //  - called by twizy.enterState() after Twizy handling
  // Note: avoid complex operations, this needs to be fast.
  // 
  void enterState(TwizyState currentState, TwizyState newState) {
    
    static uint8_t tricklecnt = 30;
    
    #if TWIZY_DEBUG_LEVEL == 0
      Serial.print(F("bms.enterState: newState="));
      Serial.println(FS(twizyStateName[newState]));
    #endif
    
    
    #if CALIBRATION_MODE == 0
      
      // ----------------------------------------------------------------------
      // bms.enterState: lower SOC at switch-on to prevent immediate charge stop:
      //
      
      if (currentState == Init && newState == Ready) {
        if (soc > 99) {
          soc -= 0.01;
          chgcur = 5;
          twizy.setSOC(soc);
          twizy.setChargeCurrent(chgcur);
          Serial.print(F("bms.enterState: SOC lowered to "));
          Serial.println(soc, 1);
        }
      }
      
      
      // ----------------------------------------------------------------------
      // bms.enterState: battery capacity adjustment by charging:
      //
      // Principle of operation:
      //  SOC estimation combines pack voltage _and_ coulomb counting.
      //  Voltage limits are hard limits overriding coulomb count, so
      //  100% SOC means 100% voltage. So we can check if the charge
      //  sum matches the SOC difference and adjust the capacity by the
      //  difference.
      //

      if (newState == StartCharge) {
        
        #ifdef PORT_CURR
          // remember start SOC & charge:
          soc_chgstart = soc;
          avail_qacs_chgstart = avail_qacs;
        #endif // #ifdef PORT_CURR
        
      }
      
      else if (newState == StopCharge) {
    
        #ifdef PORT_CURR
          
          // adjust capacity & SOH when charged at least 50% SOC difference:
          float soc_charged = soc - soc_chgstart;
          if (soc_charged >= 50) {

            // the charged SOC difference should account for...
            unsigned long expected_chgd_qacs = cap_qacs / 100 * soc_charged;
            // we actually got...
            unsigned long real_chgd_qacs = avail_qacs - avail_qacs_chgstart;
            
            // adjust capacity:
            unsigned long oldcap = cap_qacs;
            unsigned long newcap = real_chgd_qacs / soc_charged * 100;
            // ...smoothed, weighted by soc_charged:
            cap_qacs = (oldcap / SMOOTH_CAP) * (SMOOTH_CAP - soc_charged)
                    + (newcap / SMOOTH_CAP) * soc_charged;
            
            // calculate SOH:
            soh = constrain((float) cap_qacs / CAP_NOMINAL_QACS * 100, 0, 100);
            twizy.setSOH(soh);
            
            // output adjustment info:
            for (Stream *s : com_channels) {
              s->println();
              s->println(F("bms.enterState: capacity/SOH adjustment:"));
              s->print(F("- SOC charged = ")); s->println(soc_charged, 1);
              s->print(F("- expected Ah = ")); s->println(AMPHOURS(expected_chgd_qacs), 1);
              s->print(F("-  charged Ah = ")); s->println(AMPHOURS(real_chgd_qacs), 1);
              s->print(F("-  old cap Ah = ")); s->println(AMPHOURS(oldcap), 1);
              s->print(F("-  new cap Ah = ")); s->println(AMPHOURS(cap_qacs), 1);
              s->print(F("-   new SOH % = ")); s->println(soh, 1);
              s->println();
            }
          }
          
          // adjust available charge based on SOC:
          avail_qacs = cap_qacs / 100 * soc;
          
          // …or just limit to cap?
          //if (soc > 99.99)
          //  avail_qacs = cap_qacs;
          //else if (avail_qacs > cap_qacs)
          //  avail_qacs = cap_qacs;
        
        #endif // PORT_CURR
        
        // save state to EEPROM:
        saveState();
        tricklecnt = 30;
        
      } // if (newState == StopCharge)
      
      else if (newState == StopDrive) {
        
        // save state to EEPROM:
        saveState();
        tricklecnt = 30;
        
      }

      else if (newState == StopTrickle) {
        
        // to avoid high wear on the EEPROM, only save state to EEPROM
        // after 30 successive trickle charges:
        if (--tricklecnt == 0) {
          saveState();
          tricklecnt = 30;
        }
        
      }
      
    #endif // CALIBRATION_MODE == 0
    
    twizy.setInfoState1(newState);

  } // bms.enterState()


  // --------------------------------------------------------------------------
  // Callback: timer ticker
  //  - called every 10 ms by twizy.ticker() after twizy handling
  //  - clockCnt cyclic range: 0 .. 2999 = 30 seconds (reset to 0 on Off/Init)
  // Note: avoid complex operations, this needs to be fast.
  // 
  void ticker(unsigned int clockCnt) {
    
    int i;
    
    
    // ----------------------------------------------------------------------
    // bms.ticker: read current & count coulomb every 10 ms while on:
    //
    
    #ifdef PORT_CURR
      
      if (!twizy.inState(Off)) {
        
        // read current level:
        curr_qa = BASE_CURR_QA + analogRead(PORT_CURR) * VPORT * SCALE_CURR_QA;
        curr_qa = constrain(curr_qa, -2000, 2000);

        if (twizy.inState(Charging, StopCharge)) {
          curr_qa *= CURR_POLARITY_CHG;
        } else {
          curr_qa *= CURR_POLARITY_DRV;
        }
        
        #if CALIBRATION_MODE == 0
          // update VirtualBMS model:
          twizy.setCurrentQA(curr_qa);
        #endif
        
        // account for discharge/charge:
        avail_qacs += curr_qa;
        
      }
      
    #endif // PORT_CURR
    
    
    // ----------------------------------------------------------------------
    // bms.ticker: Read stacked cell voltages
    //
    
    if (!twizy.inState(Off) && clockCnt % 10 == 0) {
      
      for (i=0; i < CELL_COUNT; i++) {
        
        // select MUX input for PORT_VOLT:
        digitalWrite(PORT_MUX_S0, (i & 1) ? HIGH : LOW);
        digitalWrite(PORT_MUX_S1, (i & 2) ? HIGH : LOW);
        digitalWrite(PORT_MUX_S2, (i & 4) ? HIGH : LOW);
        digitalWrite(PORT_MUX_S3, (i & 8) ? HIGH : LOW);
        
        // read stacked voltage:
        float vc = analogRead(PORT_VOLT) * SCALE_VOLT[i];
        
        // …smooth:
        vstack[i] = ((vstack[i] * (SMOOTH_VOLT-1)) + vc) / SMOOTH_VOLT;
      }
      
      // derive single cell voltages from stacked voltages:
      vpack = vstack[CELL_COUNT-1];
      for (i=CELL_COUNT-1; i>0; i--) {
        vcell[i] = max(vstack[i] - vstack[i-1], 0);
      }
      vcell[0] = vstack[0];
      
      #if CALIBRATION_MODE == 0
        
        // update VirtualBMS model:
        #if CELL_COUNT >= 14
          twizy.setVoltage(vpack, false);
          for (i=1; i<=CELL_COUNT; i++) {
            twizy.setCellVoltage(i, vcell[i-1]);
          }
        #else
          twizy.setVoltage(vpack, true);
        #endif
        
      #endif // CALIBRATION_MODE == 0
    }
    
    
    // ----------------------------------------------------------------------
    // bms.ticker: more processing / status output?
    //
    
    #if CALIBRATION_MODE == 1
      
      // 10 seconds interval:
      if (clockCnt % 1000 != 0) {
        return;
      }
      
      Serial.println(F("\r\n*** CALIBRATION INFO [10s interval] ***"));
      
      for (i=0; i < CELL_COUNT; i++) {
        Serial.print(F("<    c"));
        if (i<10) Serial.print('0');
        Serial.print(i);
        Serial.print(F(" = "));
        print(&Serial, 3, vstack[i], 3);
        Serial.print(F("  [ "));
        print(&Serial, 3, vcell[i], 3);
        Serial.println(F(" ]"));
      }
      
    #else // CALIBRATION_MODE == 0
      
      // 1 second interval:
      if (twizy.inState(Off) || clockCnt % 100 != 0) {
        return;
      }

      #if TWIZY_DEBUG_LEVEL >= 1
        if (!quiet) {
          Serial.println(F("\r\nbms.ticker:"));
        }
      #endif
      
    #endif // CALIBRATION_MODE
    
    
    error = TWIZY_OK;
    bms_error = bmsError_None;
    
    
    // ------------------------------------------------------------
    // bms.ticker: find min/max cell voltages
    //
    
    cmin = 5.0;
    cmax = 0.0;
    
    for (i=0; i < CELL_COUNT; i++) {
      if (vcell[i] < cmin) {
        cmin = vcell[i];
        cmin_i = i;
      }
      if (vcell[i] > cmax) {
        cmax = vcell[i];
        cmax_i = i;
      }
    }
    
    cdif = cmax - cmin;
    
    
    // ----------------------------------------------------------------------
    // bms.ticker: calculate voltage based SOC
    // 
    // - newsoc_volt = direct SOC in operation mode voltage range
    // - soc_volt = smoothed operation mode SOC
    //    (used to derive drive & recup power & charge current)
    // … accordingly for cmin_soc & cmax_soc
    //
    
    float newsoc_volt, newsoc_cmin, newsoc_cmax;
    
    // voltage range depends on operation mode:
    if (twizy.inState(Charging)) {
      newsoc_volt = (vpack - (vmin_chg * CELL_COUNT)) / ((vmax_chg - vmin_chg) * CELL_COUNT) * 100.0;
      newsoc_cmin = (cmin - vmin_chg) / (vmax_chg - vmin_chg) * 100.0;
      newsoc_cmax = (cmax - vmin_chg) / (vmax_chg - vmin_chg) * 100.0;
    }
    else {
      newsoc_volt = (vpack - (vmin_drv * CELL_COUNT)) / ((vmax_drv - vmin_drv) * CELL_COUNT) * 100.0;
      newsoc_cmin = (cmin - vmin_drv) / (vmax_drv - vmin_drv) * 100.0;
      newsoc_cmax = (cmax - vmin_drv) / (vmax_drv - vmin_drv) * 100.0;
    }
    
    // smooth...
    if (newsoc_volt < soc_volt) {
      // slow adaption to lower voltages:
      soc_volt = ((soc_volt * (SMOOTH_SOC_DOWN-1)) + newsoc_volt) / SMOOTH_SOC_DOWN;
      cmin_soc = ((cmin_soc * (SMOOTH_SOC_DOWN-1)) + newsoc_cmin) / SMOOTH_SOC_DOWN;
      cmax_soc = ((cmax_soc * (SMOOTH_SOC_DOWN-1)) + newsoc_cmax) / SMOOTH_SOC_DOWN;
    }
    else {
      if (twizy.inState(Charging)) {
        // fast adaption while charging:
        soc_volt = ((soc_volt * (SMOOTH_SOC_UP_CHG-1)) + newsoc_volt) / SMOOTH_SOC_UP_CHG;
        cmin_soc = ((cmin_soc * (SMOOTH_SOC_UP_CHG-1)) + newsoc_cmin) / SMOOTH_SOC_UP_CHG;
        cmax_soc = ((cmax_soc * (SMOOTH_SOC_UP_CHG-1)) + newsoc_cmax) / SMOOTH_SOC_UP_CHG;
      }
      else {
        // slow adaption while driving:
        soc_volt = ((soc_volt * (SMOOTH_SOC_UP_DRV-1)) + newsoc_volt) / SMOOTH_SOC_UP_DRV;
        cmin_soc = ((cmin_soc * (SMOOTH_SOC_UP_DRV-1)) + newsoc_cmin) / SMOOTH_SOC_UP_DRV;
        cmax_soc = ((cmax_soc * (SMOOTH_SOC_UP_DRV-1)) + newsoc_cmax) / SMOOTH_SOC_UP_DRV;
      }
    }
    
    // sanitize...
    soc_volt = constrain(soc_volt, 0.0, 100.0);
    cmin_soc = constrain(cmin_soc, 0.0, 100.0);
    cmax_soc = constrain(cmax_soc, 0.0, 100.0);


    #ifdef PORT_CURR
      
      // ----------------------------------------------------------------------
      // bms.ticker: calculate coulomb based SOC
      //
      
      #if CALIBRATION_MODE == 1
        Serial.print(F("<   curr = ")); Serial.println((float) curr_qa/4, 1);
      #endif
      
      soc_coulomb = (float) avail_qacs / cap_qacs * 100;
      soc_coulomb = constrain(soc_coulomb, 0, 100);
      
      
      // ----------------------------------------------------------------------
      // bms.ticker: combine coulomb & voltage based SOC (hybrid SOC):
      // - prioritize soc_volt over soc_coulomb …
      //    a) …when soc_volt approaches 0/100%
      //    b) …when soc_coulomb approaches 0/100%
      //
      
      float soc_volt_prio = 0, soc_coul_degr = 0;

      if (twizy.inState(Charging) || curr_qa > 0) {
        // Charging: prioritize voltage when approaching 100%
        if (soc_volt > soc_volt_prio_above)
          soc_volt_prio = (soc_volt - soc_volt_prio_above) / (100 - soc_volt_prio_above);
        if (soc_coulomb > soc_coul_degr_above)
          soc_coul_degr = (soc_coulomb - soc_coul_degr_above) / (100 - soc_coul_degr_above);
      } else {
        // Discharging: prioritize voltage when approaching 0%
        if (soc_volt < soc_volt_prio_below)
          soc_volt_prio = (soc_volt_prio_below - soc_volt) / soc_volt_prio_below;
        if (soc_coulomb < soc_coul_degr_below)
          soc_coul_degr = (soc_coul_degr_below - soc_coulomb) / soc_coul_degr_below;
      }
      
      soc_volt_prio = max(soc_volt_prio, soc_coul_degr);
      
      soc = (soc_volt * soc_volt_prio) + (soc_coulomb * (1 - soc_volt_prio));
      soc = constrain(soc, 0, 100);
      
    #else // PORT_CURR undefined
      
      soc = soc_volt;
      
    #endif // PORT_CURR
    
    
    // ----------------------------------------------------------------------
    // bms.ticker: Derive power limits & charge current from SOC
    //

    // scale down drive power for low SOC:
    //  100% at FULL → 100% at <SOC1> → <LVL2> at <SOC2> → 0% at EMPTY
    
    // Note: minimum cell voltage SOC has priority if below <SOC2>,
    //  so if there is a bad cell in the pack, it will be protected
    //  from over discharge.
    
    #define soc2_drive_power ((drv_cutback_lvl2 / 100.0f) * max_drive_power)
    
    if (cmin_soc <= drv_cutback_soc2) {
      float factor = cmin_soc / drv_cutback_soc2;
      drvpwr = factor * soc2_drive_power;
    }
    else if (soc <= drv_cutback_soc2) {
      float factor = soc / drv_cutback_soc2;
      drvpwr = factor * soc2_drive_power;
    }
    else if (soc <= drv_cutback_soc1) {
      float factor = ((cmin_soc - drv_cutback_soc2) / (drv_cutback_soc1 - drv_cutback_soc2));
      drvpwr = soc2_drive_power + (factor * (max_drive_power - soc2_drive_power));
    }
    else {
      drvpwr = max_drive_power;
    }
    
    // scale down recuperation power & charge current for high SOC:
    //  0% at FULL → 100% at <CHG_CUTBACK_SOC> → 100% at EMPTY
    
    // Note: stop is controlled by overall pack SOC,
    //  current reduction is controlled first by maximum cell voltage.
    //  So the charger will enter the balancing phase when the first cell
    //  is getting full, but won't stop until the pack is full.
    
    if (soc > chg_stop_soc - 0.01) {
      // stop charge & reduce recuperation at 100% pack SOC / charge stop:
      recpwr = 500; // TODO: should(?) be 0 when driving, but affects D/R change
      chgcur = 0;
    }
    else if (cmax_soc >= CHG_CUTBACK_SOC) {
      // keep min 500W / 5A below 100% SOC:
      float factor = ((100 - cmax_soc) / (100 - CHG_CUTBACK_SOC));
      recpwr = 500 + (factor * (max_recup_power - 500));
      chgcur = 5 + (factor * (max_charge_current - 5));
    }
    else if (soc >= CHG_CUTBACK_SOC) {
      // keep min 500W / 5A below 100% SOC:
      float factor = ((100 - soc) / (100 - CHG_CUTBACK_SOC));
      recpwr = 500 + (factor * (max_recup_power - 500));
      chgcur = 5 + (factor * (max_charge_current - 5));
    }
    else {
      recpwr = max_recup_power;
      chgcur = max_charge_current;
    }
    
    
    // ------------------------------------------------------------
    // bms.ticker: Check cell voltage difference (min - max)
    //

    #if CALIBRATION_MODE == 0
    
      // check voltages:
      if (cdif >= VOLT_DIFF_SHUTDOWN) {
        // cell difference is critical: emergency shutdown
        printVoltAlert(F("VOLT_SHUTDOWN"));
        error |= TWIZY_SERV_BATT | TWIZY_SERV_STOP;
        bms_error = bmsError_VoltageDiff;
        twizy.enterState(Error);
      }
      else if (cdif >= VOLT_DIFF_ERROR) {
        // cell difference is high: set STOP signal, reduce drive power, stop recuperation & charge:
        printVoltAlert(F("VOLT_ERROR"));
        error |= TWIZY_SERV_BATT | TWIZY_SERV_STOP;
        bms_error = bmsError_VoltageDiff;
        drvpwr /= 4;
        recpwr /= 4;
        chgcur = 0;
      }
      else if (cdif >= VOLT_DIFF_WARN) {
        // cell difference detected: reduce power & charge levels:
        printVoltAlert(F("VOLT_WARN"));
        error |= TWIZY_SERV_BATT;
        bms_error = bmsError_VoltageDiff;
        drvpwr /= 2;
        recpwr /= 2;
        chgcur = min(chgcur, 5);
      }
      
    #endif // CALIBRATION_MODE
    
    
    // ----------------------------------------------------------------------
    // bms.ticker: Read & check battery temperature
    //
    
    float newtemp_f, newtemp_r;

    // dual read _seems_ to yield better results (LM35D issue?)
    newtemp_f = analogRead(PORT_TEMP_F);
    newtemp_f = BASE_TEMP + analogRead(PORT_TEMP_F) * VPORT * SCALE_TEMP;
    newtemp_r = analogRead(PORT_TEMP_R);
    newtemp_r = BASE_TEMP + analogRead(PORT_TEMP_R) * VPORT * SCALE_TEMP;

    // smooth...
    temp_f = (temp_f * (SMOOTH_TEMP-1) + newtemp_f) / SMOOTH_TEMP;
    temp_r = (temp_r * (SMOOTH_TEMP-1) + newtemp_r) / SMOOTH_TEMP;
    
    tdif = abs(temp_f - temp_r);
    
    #if CALIBRATION_MODE == 1
      
      Serial.print(F("< temp_f = ")); Serial.println(newtemp_f, 1);
      Serial.print(F("< temp_r = ")); Serial.println(newtemp_r, 1);
    
    #else // CALIBRATION_MODE == 0
    
      // check temperatures:
      if (max(temp_f, temp_r) > TEMP_SHUTDOWN || tdif > TEMP_DIFF_SHUTDOWN) {
        // battery is burning: emergency shutdown
        printTempAlert(F("TEMP_SHUTDOWN"));
        error |= TWIZY_SERV_TEMP | TWIZY_SERV_STOP;
        bms_error = bmsError_TemperatureHigh;
        twizy.enterState(Error);
      }
      else if (max(temp_f, temp_r) > TEMP_ERROR || tdif > TEMP_DIFF_ERROR) {
        // battery very hot: set STOP signal, stop recuperation, stop charge:
        printTempAlert(F("TEMP_ERROR"));
        error |= TWIZY_SERV_TEMP | TWIZY_SERV_STOP;
        bms_error = bmsError_TemperatureHigh;
        drvpwr /= 4;
        recpwr = 0;
        chgcur = 0;
      }
      else if (max(temp_f, temp_r) > TEMP_WARN || tdif > TEMP_DIFF_WARN) {
        // battery hot, show warning, reduce recuperation, reduce charge current:
        printTempAlert(F("TEMP_WARN"));
        error |= TWIZY_SERV_TEMP;
        bms_error = bmsError_TemperatureHigh;
        drvpwr /= 2;
        recpwr /= 2;
        chgcur = min(chgcur, 5);
      }
      else if (min(temp_f, temp_r) < 20) {
        // reduce power levels linear by temperature:
        float dt = 20 - min(temp_f, temp_r);
        float dy;
        dy = (max_drive_power - max_drive_power_0c) / 20;
        drvpwr = max(drvpwr - dt * dy, 0);
        dy = (max_recup_power - max_recup_power_0c) / 20;
        recpwr = max(recpwr - dt * dy, 0);
        if (chgcur != 0) {
          dy = (max_charge_current - max_charge_current_0c) / 20;
          chgcur = max(chgcur - dt * dy, 5);
        }
      }
      
    #endif // CALIBRATION_MODE


    // ----------------------------------------------------------------------
    // bms.ticker: Read & check charger temperature
    //

    temp_chg = twizy.getChargerTemperature();
    
    if (temp_chg > CHG_CUTBACK_TEMP) {
      float cutback = (float) (CHG_CUTBACK_TEMPMAX - temp_chg) / (CHG_CUTBACK_TEMPMAX - CHG_CUTBACK_TEMP);
      chgcur = max(chgcur * cutback, 5.0);
      bms_error = bmsError_ChargerTemperatureHigh;
	}

    
    // ----------------------------------------------------------------------
    // bms.ticker: Update VirtualBMS model
    //

    #if CALIBRATION_MODE == 0
    
      twizy.setTemperature(min(temp_f, temp_r), max(temp_f, temp_r), false);
      for (i=1; i<=7; i++) {
        // virtual module temperature = linear interpolation front→rear:
        twizy.setModuleTemperature(i, temp_f + ((float) (i-1) / 6 * (temp_r - temp_f)));
      }
      
      twizy.setSOC(soc);
      
      twizy.setPowerLimits(drvpwr, recpwr);
      twizy.setChargeCurrent(chgcur);
      
      twizy.setError(error);
      twizy.setInfoError(bms_error);

    #endif // CALIBRATION_MODE == 0

    
    // ----------------------------------------------------------------------
    // bms.ticker: Output state to serial ports
    //

    #if CALIBRATION_MODE == 0
      
      if (!quiet && clockCnt % 300 == 0) {
        printStatus(&Serial);
      }
      else if (!quiet && clockCnt % 300 == 100) {
        printStatus(&bt);
      }
      
      // Countdown quiet mode:
      if (quiet)
        quiet--;
      
    #endif // CALIBRATION_MODE == 0


  } // bms.ticker()
  
  
  // --------------------------------------------------------------------------
  // Command interpreter:
  // 
  void executeCommand(char *cmd) {

    if (!*cmd) {
      // no command: return to log mode
      quiet = 0;
      return;
    }

    for (Stream *s : com_channels) {
      s->print(F("\r\nbms.executeCommand: "));
      s->println(cmd);
      s->println();
    }
    
    char *arg = strtok(cmd, " ");
    
    if (strcasecmp(cmd, "soh") == 0) {
      // soh <prc>
      if (arg = strtok(NULL, " ")) {
        initSOH(constrain(atoi(arg), 0, 100));
      }
    }
    
    else if (strcasecmp(cmd, "soc") == 0) {
      // soc <prc>
      if (arg = strtok(NULL, " ")) {
        initSOC(constrain(atoi(arg), 0, 100));
      }
    }
    
    else if (strcasecmp(cmd, "mcc") == 0) {
      // mcc <cur>
      if (arg = strtok(NULL, " ")) {
        max_charge_current = constrain(atoi(arg), 5, 35);
      }
    }
    else if (strcasecmp(cmd, "mcc0") == 0) {
      // mcc0 <cur>
      if (arg = strtok(NULL, " ")) {
        max_charge_current_0c = constrain(atoi(arg), 5, 35);
      }
    }
    
    else if (strcasecmp(cmd, "sc") == 0) {
      // sc [<prc>]
      if (arg = strtok(NULL, " ")) {
        chg_stop_soc = constrain(atoi(arg), 1, 100);
      }
      else {
        twizy.setChargeCurrent(0); // stop charge
      }
    }
    
    else if (strcasecmp(cmd, "mpw") == 0) {
      // mpw <drv> <rec> <drv0c> <rec0c>
      if (arg = strtok(NULL, " ")) {
        max_drive_power = constrain(atoi(arg), 500, 30000);
      }
      if (arg = strtok(NULL, " ")) {
        max_recup_power = constrain(atoi(arg), 500, 30000);
      }
    }
    else if (strcasecmp(cmd, "mpw0") == 0) {
      // mpw0 <drv> <rec>
      if (arg = strtok(NULL, " ")) {
        max_drive_power_0c = constrain(atoi(arg), 500, 30000);
      }
      if (arg = strtok(NULL, " ")) {
        max_recup_power_0c = constrain(atoi(arg), 500, 30000);
      }
    }
    
    else if (strcasecmp(cmd, "dcb") == 0) {
      // dcb <soc1> <soc2> <lvl2>
      if (arg = strtok(NULL, " ")) {
        drv_cutback_soc1 = constrain(atoi(arg), 0, 100);
      }
      if (arg = strtok(NULL, " ")) {
        drv_cutback_soc2 = constrain(atoi(arg), 0, 100);
      }
      if (arg = strtok(NULL, " ")) {
        drv_cutback_lvl2 = constrain(atoi(arg), 0, 100);
      }
    }
    
    else if (strcasecmp(cmd, "vrd") == 0) {
      // vrd <min> <max>
      if (arg = strtok(NULL, " ")) {
        vmin_drv = constrain(atof(arg), 0.0, 5.0);
      }
      if (arg = strtok(NULL, " ")) {
        vmax_drv = constrain(atof(arg), 0.0, 5.0);
      }
    }
    
    else if (strcasecmp(cmd, "vrc") == 0) {
      // vrc <min> <max>
      if (arg = strtok(NULL, " ")) {
        vmin_chg = constrain(atof(arg), 0.0, 5.0);
      }
      if (arg = strtok(NULL, " ")) {
        vmax_chg = constrain(atof(arg), 0.0, 5.0);
      }
    }
    
    else if (strcasecmp(cmd, "svp") == 0) {
      // svp <top> <bot>
      if (arg = strtok(NULL, " ")) {
        soc_volt_prio_above = constrain(atoi(arg), 0, 100);
      }
      if (arg = strtok(NULL, " ")) {
        soc_volt_prio_below = constrain(atoi(arg), 0, 100);
      }
    }
    
    else if (strcasecmp(cmd, "scd") == 0) {
      // scd <top> <bot>
      if (arg = strtok(NULL, " ")) {
        soc_coul_degr_above = constrain(atoi(arg), 0, 100);
      }
      if (arg = strtok(NULL, " ")) {
        soc_coul_degr_below = constrain(atoi(arg), 0, 100);
      }
    }
    
    else if (strcasecmp(cmd, "init") == 0) {
      init();
    }
    else if (strcasecmp(cmd, "load") == 0) {
      loadState();
    }
    else if (strcasecmp(cmd, "save") == 0) {
      saveState();
    }
    
    #if FEATURE_CMD_ES == 1
      else if (strcasecmp(cmd, "es") == 0) {
        // es <statenr>
        if (arg = strtok(NULL, " ")) {
          twizy.enterState((TwizyState) constrain(atoi(arg), Off, StopTrickle));
        }
      }
    #endif // FEATURE_CMD_ES
    
    else {
      // HELP:
      for (Stream *s : com_channels) {
        s->println(F("HELP:\r\n"
          " soh <prc> -- set SOH%\r\n"
          " soc <prc> -- set SOC%\r\n"
          " mcc <cur> -- set max charge current @ 20°C\r\n"
          " mcc0 <cur> -- set max charge current @ 0°C\r\n"
          " sc [<prc>] -- stop charge / set stop SOC\r\n"
          " mpw <drv> <rec> -- set max power @ 20°C\r\n"
          " mpw0 <drv> <rec> -- set max power @ 0°C\r\n"
          " dcb <soc1> <soc2> <lvl2> -- set drive cutback\r\n"
          " vrd <min> <max> -- set voltage range discharging\r\n"
          " vrc <min> <max> -- set voltage range charging\r\n"
          " svp <top> <bot> -- set SOC voltage prioritize\r\n"
          " scd <top> <bot> -- set SOC coulomb degrade\r\n"
          " init      -- reset to default config\r\n"
          " load      -- load state from EEPROM\r\n"
          " save      -- save state to EEPROM\r\n"
          #if FEATURE_CMD_ES == 1
            " es <nr>   -- enter a VBMS state (0..12)\r\n"
          #endif
        ));
      }
    }
    
    // output status info:
    for (Stream *s : com_channels) {
      
      s->print(F("- SOH%   = ")); s->println(soh, 1);
      s->print(F("- SOC%   = ")); s->println(soc, 1);
      #ifdef PORT_CURR
        s->print(F("-    cap = ")); s->println(AMPHOURS(cap_qacs), 1);
        s->print(F("-  avail = ")); s->println(AMPHOURS(avail_qacs), 1);
      #endif
      s->println();
      
      s->print(F("- MCC    = ")); s->println(max_charge_current);
      s->print(F("- MCC0   = ")); s->println(max_charge_current_0c);
      s->print(F("- SC     = ")); s->println(chg_stop_soc);
      s->print(F("- MPW    = ")); s->print(max_drive_power);
        s->print(' '); s->println(max_recup_power);
      s->print(F("- MPW0   = ")); s->print(max_drive_power_0c);
        s->print(' '); s->println(max_recup_power_0c);
      s->print(F("- DCB    = ")); s->print(drv_cutback_soc1);
        s->print(' '); s->print(drv_cutback_soc2);
        s->print(' '); s->println(drv_cutback_lvl2);
      s->print(F("- VRD    = ")); s->print(vmin_drv, 3);
        s->print(' '); s->println(vmax_drv, 3);
      s->print(F("- VRC    = ")); s->print(vmin_chg, 3);
        s->print(' '); s->println(vmax_chg, 3);
      s->print(F("- SVP    = ")); s->print(soc_volt_prio_above);
        s->print(' '); s->println(soc_volt_prio_below);
      s->print(F("- SCD    = ")); s->print(soc_coul_degr_above);
        s->print(' '); s->println(soc_coul_degr_below);
      s->println();
    }
    
    // Be quiet for 10 seconds:
    quiet = 10;
    
  } // bms.executeCommand()


} bms; // class KlausBMS


// --------------------------------------------------------------------------
// Callback object wrappers
// 

void bmsEnterState(TwizyState currentState, TwizyState newState) {
  bms.enterState(currentState, newState);
}

void bmsTicker(unsigned int clockCnt) {
  bms.ticker(clockCnt);
}


// -----------------------------------------------------
// SETUP
// 

void setup() {
  
  // Init communication:
  
  Serial.begin(SERIAL_BAUD);
  bt.begin(BT_BAUD);

  for (Stream *s : com_channels) {
    s->println(F(KLAUS_BMS_NAME " " KLAUS_BMS_VERSION));
  }
  
  // Init VirtualBMS & KlausBMS:
  
  twizy.begin();
  bms.begin();
  
  twizy.attachTicker(bmsTicker);
  twizy.attachEnterState(bmsEnterState);
  
  #if TWIZY_CAN_SEND == 0
    Serial.println(F("*** DRY RUN (NO CAN TX) ***"));
    twizy.enterState(Init);
  #endif
  
  #if CALIBRATION_MODE == 1
    Serial.println(F("*** CALIBRATION MODE ***"));
    twizy.enterState(Init);
  #endif
  
} // setup()


// -----------------------------------------------------
// MAIN LOOP
// 

void loop() {
  
  // VirtualBMS main loop:
  
  twizy.looper();
  
  // check for Serial/Bluetooth command:
  // (note: using shared input buffer for both to save space)

  static char lastc = 0;
  char c = 0;
  if (Serial.available()) {
    c = (char) Serial.read();
  }
  else if (bt.available()) {
    c = (char) bt.read();
  }
  
  if (c == '\r' || c == '\n') {
    if (c == '\r' || lastc != '\r') {
      inputbuf[inputpos] = 0;
      bms.executeCommand(inputbuf);
      inputpos = 0;
    }
    lastc = c;
  }
  else if (c >= 32 && inputpos < sizeof(inputbuf)-1) {
    inputbuf[inputpos++] = c;
    lastc = c;
  }
  
}