Accum-Tester/src/main.c

/*
    ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

#include <stdio.h>
#include <string.h>

#include "ch.h"
#include "hal.h"
#include "chprintf.h"
#include "ff.h"
#include "gfx.h"
#include "buttons.h"
#include "INA3221.h"

#define INA_SCAN_PERIOS_US    62500
#define INA_AVG_FACTOR        16

#define CHRGR_PAUSE1_S        300
#define CHRGR_PAUSE2_S        300

#define INA_ALL_VALUSE        EVENT_MASK(0)
#define INA_BUS_VALUES        EVENT_MASK(1)
#define CHRGR_ST_CHANGE       EVENT_MASK(2)
#define TIME_CHANGE           EVENT_MASK(3)
#define INSERTED_EVENT        EVENT_MASK(4)
#define REMOVED_EVENT         EVENT_MASK(5)

/* Card insertion monitor. */
#define POLLING_INTERVAL      10
#define POLLING_DELAY         10

/* Type definitions */
typedef enum chrgr_state {
  Stop = 0,
  Charge1,
  Pause1,
  Decharge,
  Pause2,
  Charge2,
  Error
} charger_state_t;

typedef struct timer {
  uint8_t hh;
  uint8_t mm;
  uint8_t ss;
} time_cnt_t;

typedef struct accum_profile {
  char * Chemistry;
  char * VoltageNominal;
  uint32_t Capacity_mAh;
  uint32_t VoltageMax_mv;
  uint32_t VoltageDechMin_mv;
  uint32_t CurrentMax_mA;
  uint32_t CurrentChrgMin_mA;
  time_cnt_t TimeChargeMax;
  char * Description;
} accum_profile_t;

#define ACCUM_PROFILE_NUM   5
accum_profile_t Profile[ACCUM_PROFILE_NUM] = {
  {"SLA", "12V", 12000, 15000, 10500, 3600, 240, {24,0,0}, "GP12120"},
  {"SLA", "6V", 1200, 7500, 5250, 360, 24, {24,0,0}, "GP6012"},
  {"SLA", "12V", 45000, 145000, 10500, 13500, 900, {24,0,0}, "Auto-45AH"},
  {"LiIon", "1S1", 3200, 4200, 2500, 1625, 65, {4,0,0}, "NCR18650B"},
  {"LiIon", "1S2", 2600, 4200, 2750, 1300, 130, {3,0,0}, "ICR18650-26F"}
};

enum menu_items {
  MenuItem_1 = 0,
  MenuItem_2 = 1,
  MenuItem_3 = 2,
  MenuItem_4 = 3,
  MenuItems = 4
};

typedef struct {
  color_t activeFG;
  color_t activeBG;
  color_t inactiveFG;
  color_t inactiveBG;
} menu_colors_t;

// types and description of accums - SLA, LiIon

// variants and description of voltaged accroding to cemistry type - 6/12V, 1-4S
// and db of min and max voltages for each

// some set of amperage or capacitance - range of current for charge/decharge stop condition

typedef struct chrgr_channel {
  int16_t shunt;        /**< resistance in milliOhms. */
  int16_t max_current;  /**< max channel current in milliAmpers. */
} charger_channel_t;

/*
 * I2C1 config.
 */
static const I2CConfig i2cfg1 = {
    OPMODE_I2C,
    400000,
    FAST_DUTY_CYCLE_2,
};
/*
 * 
 */
static void gpt_cb(GPTDriver *gptp);
static GPTConfig gpt_cfg = {
  1000000,  /* Timer clock 1 MHz. */
  gpt_cb,   /* Timer callback. */
  0, 0
};

/* Private constants */
static const charger_channel_t charger_Channels[INA3221_CH_NUM] = {
  {20, 8190}, {50, 3276}, {10, 16380}
};

/* Privae functions */
static void prepare_Screen(void);
static void ina_Process(void);
static void mode_vt_cb(virtual_timer_t *vtp, void *st);
static void btn1_handler(const button_state_t);
static void btn2_handler(const button_state_t);
static void show_MenuItem(const int item);
static void sdc_tmrfunc(virtual_timer_t *vtp, void *p);
static void sdc_tmr_init(void *p);
static void InsertHandler(eventid_t id); /* Card insertion event. */
static void RemoveHandler(eventid_t id); /* Card removal event. */
static FRESULT scan_files(BaseSequentialStream *chp, char *path);
static void cmd_tree(BaseSequentialStream *chp, int argc, char *argv[]);

/* Private variables */
static binary_semaphore_t ina_bsem, charger_bsem;
static virtual_timer_t mode_vt, sdc_tmr;
static event_source_t ina_all_event, ina_bus_event;
static event_source_t chrgr_st_event, time_event;
static event_source_t inserted_event, removed_event;
static charger_state_t charger_State;
static ina3221_ch_t charger_Channel;
static int charger_Profile = 0;
static uint32_t Current;
static uint32_t Voltage;
static uint32_t Power, Capacity_I=0, Capacity_P=0;
static uint32_t dech_Capacity_I, dech_Capacity_P, ch_Capacity_I, ch_Capacity_P;
static int INA_Present = 0;
static BaseSequentialStream * chp = (BaseSequentialStream*) &SD1;
GHandle GW1;  // The handle for our console
ina3221_t ina3221;
static gFont font1;
static gFont font2;
static time_cnt_t Timer = {0}, dechTimer, chTimer;
static btn_hndlr bha[Button_Num] = {btn1_handler, btn2_handler};
static int menu_Active = MenuItem_1;
static menu_colors_t menu_Colors = {Navy, Olive, Yellow, Navy};
static unsigned cnt;
static FATFS SDC_FS; /* FS object. */
static bool fs_ready = false; /* FS mounted and ready.*/
static uint8_t fbuff[1024]; /* Generic large buffer.*/

/*
 * INA process thread.
 */
static THD_WORKING_AREA(waInaThread, 512);
static THD_FUNCTION(InaThread, arg) {
  (void)arg;

  while (true) {
    chBSemWait(&ina_bsem);
    ina_Process();
  }
}

/*
 * Charger process thread. Once per second.
 * Timer for charger
 */
static THD_WORKING_AREA(waChrgThread, 512);
static THD_FUNCTION(ChrgThread, arg) {
  (void)arg;

  while (true) {
    chBSemWait(&charger_bsem);

    if (charger_State != Stop) {
      if ((charger_State == Pause1) || (charger_State == Pause2)) {
        /* Pause countdown */
        if (Timer.ss > 0) {
          Timer.ss --;
        } else {
          if (Timer.mm > 0) {
            Timer.mm --;
          } else {
            if (Timer.hh > 0){
              Timer.hh --;
            } else {
              break;
            }
            Timer.mm = 59;
          }
          Timer.ss = 59;
        }
      } else {
        /* Charge/Decharge counter */
        if (Timer.ss < 59) {
          Timer.ss ++;
        } else {
          Timer.ss = 0;
          if (Timer.mm < 59) {
            Timer.mm ++;
          } else {
            Timer.mm = 0;
            Timer.hh ++;
          }
        }
      }
      /* Show new timer value */
      chEvtBroadcast(&time_event);
    }
  }
}

/*
 * Green LED blinker thread, times are in milliseconds.
 */
static THD_WORKING_AREA(waLedThread, 256);
static THD_FUNCTION(LedThread, arg) {
  (void)arg;
  chRegSetThreadName("blinker");
  while (true) {
    switch (charger_State) {
    case Stop:
      palClearLine(LINE_LED1);
      chThdSleepMilliseconds(500);
      palSetLine(LINE_LED1);
      chThdSleepMilliseconds(500);
      break;
    
    case Error:
      palClearLine(LINE_LED1);
      chThdSleepMilliseconds(200);
      palSetLine(LINE_LED1);
      chThdSleepMilliseconds(200);
      break;

    case Decharge:
      palClearLine(LINE_LED1);
      chThdSleepMilliseconds(900);
      palSetLine(LINE_LED1);
      chThdSleepMilliseconds(100);
      break;

    default:
      palClearLine(LINE_LED1);
      chThdSleepMilliseconds(100);
      palSetLine(LINE_LED1);
      chThdSleepMilliseconds(900);
      break;
    }
  }
}

/*
 * Application entry point.
 */
int main(void) {
  /*
   * System initializations.
   * - HAL initialization, this also initializes the configured device drivers
   *   and performs the board-specific initializations.
   * - Kernel initialization, the main() function becomes a thread and the
   *   RTOS is active.
   */
  halInit();
  chSysInit();

  /*
   * This initialization requires the OS already active because it uses delay
   * APIs inside.
   *
   * Initialize uGFX and the underlying system
   */
  gfxInit();
  prepare_Screen();

  /*
   * Activates the I2C driver 1.
   */
  i2cStart(&I2CD1, &i2cfg1);

  /*
   * Activates the serial driver 1 using the driver default configuration.
   */
  sdStart(&SD1, NULL);
  chprintf(chp, "\033[2J\033[1;1H"); // clear screen
  chprintf(chp, "Started\r\n");

  /*
   * Starting the button handle thread.
   */
  buttons_Init(bha);

  /*
   * Initializing semaphores in the "taken" state.
   */
  chBSemObjectInit(&ina_bsem, true);
  chBSemObjectInit(&charger_bsem, true);

  /*
   * Creates the blinker thread.
   */
  chThdCreateStatic(waLedThread, sizeof(waLedThread), NORMALPRIO-1, LedThread, NULL);

  /*
   * Creates the charger thread.
   */
  chThdCreateStatic(waChrgThread, sizeof(waChrgThread), NORMALPRIO, ChrgThread, NULL);

  /*
   * Creates the INA thread.
   */
  chThdCreateStatic(waInaThread, sizeof(waInaThread), NORMALPRIO, InaThread, NULL);
  //chVTSetContinuous(&ina_vt, TIME_MS2I(INA_SCAN_PERIOS_MS), ina_Process, NULL);

  gwinPrintf(GW1, "Try to init INA3221...\n");
  ina3221.i2c_addr = INA3221_ADDR40_GND;
  ina3221.i2cd = &I2CD1;
  ina3221.shuntRes[INA3221_CH1] = charger_Channels[INA3221_CH1].shunt;
  ina3221.shuntRes[INA3221_CH2] = charger_Channels[INA3221_CH2].shunt;
  ina3221.shuntRes[INA3221_CH3] = charger_Channels[INA3221_CH3].shunt;

  if (INA3221_getManufID(&ina3221) == 0x5449) {
    if (INA3221_getDieID(&ina3221) == 0x3220) {
      INA_Present = 1;
      gwinPrintf(GW1, "INA3221 present!\n");

      INA3221_reset(&ina3221);
      INA3221_setBusConversionTime(&ina3221, INA3221_REG_CONF_CT_8244US);
    }
  } else {
    gwinPrintf(GW1, "Sensor INA3221 not give proper IDs.\n", Voltage);
  }

  /* Starting GPT for ina scan and seconds counter */
  gptStart(&GPTD7, &gpt_cfg);
  gptStartContinuous(&GPTD7, INA_SCAN_PERIOS_US);

  /* Activates the  SDC driver 1 using default configuration. */
  sdcStart(&SDCD1, NULL);
  /* Activates the card insertion monitor. */
  sdc_tmr_init(&SDCD1);

  /* Init charger module */
  charger_State = Stop;
  charger_Channel = INA3221_CH1;

  event_listener_t el0, el1, el2, el3, el4, el5;

  /* Events initialization and registration.*/
  chEvtObjectInit(&ina_all_event);
  chEvtObjectInit(&ina_bus_event);
  chEvtObjectInit(&chrgr_st_event);
  chEvtObjectInit(&time_event);
  chEvtObjectInit(&inserted_event);
  chEvtObjectInit(&removed_event);
  
  chEvtRegister(&ina_all_event, &el0, 0);
  chEvtRegister(&ina_bus_event, &el1, 1);
  chEvtRegister(&chrgr_st_event, &el2, 2);
  chEvtRegister(&time_event, &el3, 3);
  chEvtRegister(&inserted_event, &el4, 4);
  chEvtRegister(&removed_event, &el5, 5);

  eventmask_t events;
  charger_state_t oldState = Stop;
  char buf[16];
  uint32_t tmp0, tmp1, tmp2;

  /*
   * Normal main() thread activity, in this demo it does nothing except
   * sleeping in a loop and check the button state.
   */
  while (true) {
    //chThdSleepMilliseconds(500);
    events = chEvtWaitAny(ALL_EVENTS);

    if (events & INA_ALL_VALUSE) {
      tmp1 = Voltage / 1000;
      tmp2 = Voltage % 1000;
      chsnprintf(buf, 11, "U:%2d.%03uV", tmp1, tmp2);
      gdispFillStringBox(1, 123, 158, 29, buf, font2, Red, Gray, gJustifyLeft);

      tmp1 = Current / 1000;
      tmp2 = Current % 1000;
      chsnprintf(buf, 11, "I:%2d.%03uA", tmp1, tmp2);
      gdispFillStringBox(1, 153, 158, 29, buf, font2, Red, Gray, gJustifyLeft);

      tmp1 = Power / 1000;
      tmp2 = Power % 1000;
      chsnprintf(buf, 11, "P:%2d.%03uW", tmp1, tmp2);
      gdispFillStringBox(1, 183, 158, 29, buf, font2, Red, Gray, gJustifyLeft);

      tmp0 = (Capacity_I + 1800) / 3600;
      tmp1 = tmp0 / 1000;
      tmp2 = tmp0 % 1000;
      chsnprintf(buf, 13, "CI:%2d.%03uAh", tmp1, tmp2);
      gdispFillStringBox(160, 153, 158, 29, buf, font2, Red, Gray, gJustifyLeft);

      tmp0 = (Capacity_P + 1800) / 3600;
      tmp1 = tmp0 / 1000;
      tmp2 = tmp0 % 1000;
      chsnprintf(buf, 13, "CP:%2d.%03uWh", tmp1, tmp2);
      gdispFillStringBox(160, 183, 158, 29, buf, font2, Red, Gray, gJustifyLeft);

      /* Check charge/decharge conditions */
      switch (charger_State) {
      case Charge1:
      case Charge2:
        if (Profile[charger_Profile].VoltageMax_mv < Voltage) {
          gwinPrintf(GW1, "Overvoltage detected!\n");
          charger_State = Error;
          events |= CHRGR_ST_CHANGE;
        }
        if (Profile[charger_Profile].CurrentMax_mA < Current) {
          gwinPrintf(GW1, "Overcurrent detected!\n");
          charger_State = Error;
          events |= CHRGR_ST_CHANGE;
        }
        if (Profile[charger_Profile].CurrentChrgMin_mA > Current) {
          gwinPrintf(GW1, "Charged Successful.\n");
          if (charger_State == Charge1) {
            charger_State = Pause1;
          } else {
            charger_State = Stop;
          }
          events |= CHRGR_ST_CHANGE;
        }
        break;
      
      case Decharge:
        if (Profile[charger_Profile].VoltageDechMin_mv > Voltage) {
          gwinPrintf(GW1, "Decharge finished.\n");
          charger_State = Pause2;
          events |= CHRGR_ST_CHANGE;
        }
        break;
      
      default:
        break;
      }
    }

    if (events & INA_BUS_VALUES) {
      tmp1 = Voltage / 1000;
      tmp2 = Voltage % 1000;
      chsnprintf(buf, 11, "U:%2d.%03uV", tmp1, tmp2);
      gdispFillStringBox(1, 123, 158, 29, buf, font2, Red, Gray, gJustifyLeft);
      /* here should control processes time ? */
    }

    if (events & CHRGR_ST_CHANGE) {
      switch (charger_State) {
      case Charge1:
        if (Profile[charger_Profile].VoltageDechMin_mv > Voltage) {
          gwinPrintf(GW1, "No Battery detected / Low voltage...\n");
          charger_State = Stop;
          gdispFillStringBox(1, 213, 318, 29, "Stop", font2, Red, Silver, gJustifyCenter);
          break;
        }
        oldState = Charge1;
        Capacity_I = 0;
        Capacity_P = 0;
        Timer.hh = 0;
        Timer.mm = 0;
        Timer.ss = 0;
        gwinPrintf(GW1, "Begin process: Charge_1\n");

        palClearLine(LINE_RELAY1);  // power relay on
        gdispFillStringBox(1, 213, 318, 29, "Charge 1", font2, Red, Silver, gJustifyCenter);
        break;

      case Pause1:
        oldState = Pause1;

        palSetLine(LINE_RELAY1);  // power relay off

        tmp0 = CHRGR_PAUSE1_S / 60;
        Timer.ss = CHRGR_PAUSE1_S % 60;
        Timer.hh = tmp0 / 60;
        Timer.mm = tmp0 % 60;

        tmp0 = (Capacity_I + 1800) / 3600;
        tmp1 = tmp0 / 1000;
        tmp2 = (tmp0 % 1000) / 10;
        gwinPrintf(GW1, "CI/CP: %2u.%02u", tmp1, tmp2);
        tmp0 = (Capacity_P + 1800) / 3600;
        tmp1 = tmp0 / 1000;
        tmp2 = (tmp0 % 1000) / 10;
        gwinPrintf(GW1, "/%2u.%02u\n", tmp1, tmp2);

        gwinPrintf(GW1, "Pause after Charge_1\n");

        gdispFillStringBox(1, 213, 318, 29, "Pause 1", font2, Red, Silver, gJustifyCenter);
        chVTSet(&mode_vt, TIME_S2I(CHRGR_PAUSE1_S), mode_vt_cb, (void *)Decharge);
        break;

      case Decharge:
        oldState = Decharge;
        Capacity_I = 0;
        Capacity_P = 0;
        Timer.hh = 0;
        Timer.mm = 0;
        Timer.ss = 0;
        gwinPrintf(GW1, "Begin process: Decharge\n");
        
        palClearLine(LINE_RELAY2);  // load relay on
        gdispFillStringBox(1, 213, 318, 29, "Decharge", font2, Red, Silver, gJustifyCenter);
        break;

      case Pause2:
        oldState = Pause2;

        palSetLine(LINE_RELAY2);  // load relay off

        dech_Capacity_I = Capacity_I;
        dech_Capacity_P = Capacity_P;
        dechTimer = Timer;

        tmp0 = CHRGR_PAUSE2_S / 60;
        Timer.ss = CHRGR_PAUSE2_S % 60;
        Timer.hh = tmp0 / 60;
        Timer.mm = tmp0 % 60;

        tmp0 = (Capacity_I + 1800) / 3600;
        tmp1 = tmp0 / 1000;
        tmp2 = (tmp0 % 1000) / 10;
        gwinPrintf(GW1, "CI/CP: %2u.%02u", tmp1, tmp2);
        tmp0 = (Capacity_P + 1800) / 3600;
        tmp1 = tmp0 / 1000;
        tmp2 = (tmp0 % 1000) / 10;
        gwinPrintf(GW1, "/%2u.%02u\n", tmp1, tmp2);

        gwinPrintf(GW1, "Pause after Decharge\n");
        gdispFillStringBox(1, 213, 318, 29, "Pause 2", font2, Red, Silver, gJustifyCenter);

        chVTSet(&mode_vt, TIME_S2I(CHRGR_PAUSE2_S), mode_vt_cb, (void *)Charge2);
        break;

      case Charge2:
        Capacity_I = 0;
        Capacity_P = 0;
        Timer.hh = 0;
        Timer.mm = 0;
        Timer.ss = 0;
        oldState = Charge2;
        gwinPrintf(GW1, "Begin process: Charge_2\n");

        palClearLine(LINE_RELAY1);  // power relay on
        gdispFillStringBox(1, 213, 318, 29, "Charge 2", font2, Red, Silver, gJustifyCenter);
        break;
      
      case Error:
        oldState = Error;
        // power and load relay off
        palSetLine(LINE_RELAY1);
        palSetLine(LINE_RELAY2);
        gdispFillStringBox(1, 213, 318, 29, "Error", font2, Silver, Red, gJustifyCenter);
        break;

      default:
        if (oldState == Charge2) {
          ch_Capacity_I = Capacity_I;
          ch_Capacity_P = Capacity_P;
          chTimer = Timer;
          Timer.hh = 0;
          Timer.mm = 0;
          Timer.ss = 0;

          tmp0 = (Capacity_I + 1800) / 3600;
          tmp1 = tmp0 / 1000;
          tmp2 = (tmp0 % 1000) / 10;
          gwinPrintf(GW1, "CI/CP: %2u.%02u", tmp1, tmp2);
          tmp0 = (Capacity_P + 1800) / 3600;
          tmp1 = tmp0 / 1000;
          tmp2 = (tmp0 % 1000) / 10;
          gwinPrintf(GW1, "/%2u.%02u\n", tmp1, tmp2);
        }
        oldState = Stop;
        // power and load relay off
        palSetLine(LINE_RELAY1);
        palSetLine(LINE_RELAY2);
        gdispFillStringBox(1, 213, 318, 29, "Stop", font2, Red, Silver, gJustifyCenter);
        break;
      }
    }

    if (events & TIME_CHANGE) {
      chsnprintf(buf, 12, "T:%02u:%02u:%02u", Timer.hh, Timer.mm, Timer.ss);
      gdispFillStringBox(160, 123, 158, 29, buf, font2, Red, Gray, gJustifyLeft);
    }

    if (events & INSERTED_EVENT) {
      InsertHandler(0);
    }

    if (events & REMOVED_EVENT) {
      RemoveHandler(0);
    }
  }
}

/**
 * @brief Prepare screen for inforation presentation.
 */
static void prepare_Screen(void) {
  /* Set some fonts */
  font1 = gdispOpenFont("DejaVu Sans Book 12");
  font2 = gdispOpenFont("DejaVu Sans Book 24");
  gwinSetDefaultFont(font1);

  /* draw screen frame */
  const coord_t width = 320;
  const coord_t height = 240;

  gdispDrawBox(0, 0, width-1, height-1, Green); // all screen
  gdispDrawLine(0, 122, width-1, 122, Green); // h line
  gdispDrawLine(79, 0, 79, 122, Green); // v line, text | console
  gdispFillArea(1, 1, 78, 121, Blue); // text area

  /* print text to top-left area */
  show_MenuItem(MenuItem_1);
  show_MenuItem(MenuItem_2);
  show_MenuItem(MenuItem_3);
  show_MenuItem(MenuItem_4);

  /* clear bootom area */
  gdispFillStringBox(1, 123, 318, 115, " ", font2, Red, Gray, gJustifyLeft);
  gdispFillStringBox(1, 213, 318, 29, "Stop", font2, Red, Silver, gJustifyCenter);

  /* create the console window */
  {
    GWindowInit wi;

    gwinClearInit(&wi);
    wi.show = gTrue;
    wi.x = 82; wi.y = 2; wi.width = 239; wi.height = 120;
    GW1 = gwinConsoleCreate(0, &wi);
  }

  /* Set the fore- and background colors for each console */
  gwinSetColor(GW1, GFX_WHITE);
  gwinSetBgColor(GW1, GFX_BLACK);

  /* clear all console windows - to set background */
  gwinClear(GW1);

  /* Output some data on the console */
  gwinPrintf(GW1, "\033bConsole\033B started sucessful.\n");
}

/**
 * @brief Select next menu item
 */
static void btn1_handler(button_state_t state) {
  if (state == BTN_st_Pressed) {
    switch (menu_Active) {
    case MenuItem_1:
      menu_Active = MenuItem_2;
      show_MenuItem(MenuItem_1);
      show_MenuItem(MenuItem_2);
      break;
    
    case MenuItem_2:
      menu_Active = MenuItem_3;
      show_MenuItem(MenuItem_2);
      show_MenuItem(MenuItem_3);
      break;
    
    case MenuItem_3:
      menu_Active = MenuItem_4;
      show_MenuItem(MenuItem_3);
      show_MenuItem(MenuItem_4);
      break;
    
    case MenuItem_4:
      menu_Active = MenuItem_1;
      show_MenuItem(MenuItem_4);
      show_MenuItem(MenuItem_1);
      break;
    
    default:
      menu_Active = MenuItem_1;
      break;
    }
  }
}

/**
 * @brief Process selected menu item
 */
static void btn2_handler(button_state_t state) {
  static int s3 = 0; // menu_item3 state

  if (state == BTN_st_Pressed) {

    switch (menu_Active) {
    case MenuItem_1:
      if (charger_State == Stop) {
        switch (charger_Channel) {
        case INA3221_CH1:
          charger_Channel = INA3221_CH2;
          break;
        case INA3221_CH2:
          charger_Channel = INA3221_CH3;
          break;
        case INA3221_CH3:
          charger_Channel = INA3221_CH1;
          break;
        
        default:
          charger_Channel = INA3221_CH1;
          break;
        }
        gwinPrintf(GW1, "Channel #%u selected.\n", (int)charger_Channel + 1);
        show_MenuItem(MenuItem_1);
      }
      break;
    
    case MenuItem_2:
      charger_Profile ++;
      if (charger_Profile >= ACCUM_PROFILE_NUM) {
        charger_Profile = 0;
      }
      gwinPrintf(GW1, "Select profile for %s\n", Profile[charger_Profile].Description);
      show_MenuItem(MenuItem_2);
      break;

    case MenuItem_3:
      if (s3 == 0) {
        s3 ++;
        gwinPrintf(GW1, "Show Charge results\n");
        Capacity_I = ch_Capacity_I;
        Capacity_P = ch_Capacity_P;
        Timer = chTimer;
      } else {
        s3 = 0;
        gwinPrintf(GW1, "Show Decharge results\n");
        Capacity_I = dech_Capacity_I;
        Capacity_P = dech_Capacity_P;
        Timer = dechTimer;
      }

      chEvtBroadcast(&ina_all_event);
      chEvtBroadcast(&time_event);
      break;

    case MenuItem_4:
      switch(charger_State) {
      case Stop:
        charger_State = Charge1;
        show_MenuItem(MenuItem_4);
        break;

      default:
        charger_State = Stop;
        Current = 0;
        Voltage = 0;
        Power = 0;
        show_MenuItem(MenuItem_4);
        break;
      }

      chEvtBroadcast(&chrgr_st_event);
      break;

    default:
      break;
    } /* switch menu_Active */

  } /* BTN pressed */ 
} /* end of btn2_handler() */

/**
 * @brief Virtual timer callback. Change charger state.
 */
static void mode_vt_cb(virtual_timer_t *vtp, void * st) {
  (void)vtp;

  charger_State = (charger_state_t)st; // new charger state

  chSysLockFromISR();
  chEvtBroadcastI(&chrgr_st_event);
  chSysUnlockFromISR();
}

/** 
 * @brief Show/update menu items.
 */
static void show_MenuItem(const int item) {
  char buf1[16] = {0};
  char buf2[16] = {0};
  int tmp;
  coord_t y1=0, y2=0;
  static const coord_t x=1, cx=78, cy=15;
  static const justify_t j1 = gJustifyLeft;
  justify_t j2 = gJustifyLeft;
  color_t fgc, bgc;

  if (item == menu_Active) {
    fgc = menu_Colors.activeFG;
    bgc = menu_Colors.activeBG;
  } else {
    fgc = menu_Colors.inactiveFG;
    bgc = menu_Colors.inactiveBG;
  }

  switch (item) {
  case MenuItem_1:
    tmp = (int)charger_Channel + 1;
    chsnprintf(buf1, 10, "[1] ch.%u", tmp);
    chsnprintf(buf2, 12, "%2uA", charger_Channels[charger_Channel].max_current/1000);

    y1 = 1;
    y2 = 16;
    j2 = gJustifyCenter;
    break;
  
  case MenuItem_2:
    chsnprintf(buf1, 10, "[2] %s", Profile[charger_Profile].Chemistry);
    tmp = Profile[charger_Profile].Capacity_mAh / 1000;
    chsnprintf(buf2, 12, "%s/%uAh", Profile[charger_Profile].VoltageNominal, tmp);

    y1 = 31;
    y2 = 46;
    j2 = gJustifyCenter;
    break;

  case MenuItem_3:
    chsnprintf(buf1, 12, "[3] show ch");
    chsnprintf(buf2, 12, "dech results");
    y1 = 61;
    y2 = 76;
    break;

  case MenuItem_4:
    if (charger_State == Stop) {
      chsnprintf(buf1, 12, "[4] Start");
    } else {
      chsnprintf(buf1, 12, "[4] Stop");
    }
    //chsnprintf(buf2, 12, "          ");
    y1 = 91;
    y2 = 106;
    break;

  default:
    break;
  }

  gdispFillStringBox(x, y1, cx, cy, buf1, font1, fgc, bgc, j1);
//  gdispFillStringBox(x, y2, cx, cy, buf2, font1, fgc, bgc, j2);
  gdispFillStringBox(x, y2, cx, cy, buf2, font1, menu_Colors.inactiveFG, menu_Colors.inactiveBG, j2);
}

/**
 * @brief Read Voltage & Current, update screen info.
 */
static void ina_Process(void) {
  int32_t current;
  uint32_t volt;
  static int idx = 0;
  static uint32_t sumCurrent=0, sumVoltage=0;

  if (INA_Present != 0) {
    current = INA3221_getCurrent(&ina3221, charger_Channel);
    volt = INA3221_getVoltage(&ina3221, charger_Channel);

    if (current < 0) {
      current = 1 - current;
    }

    sumCurrent += current;
    sumVoltage += volt;

    idx ++;
    if (idx >= INA_AVG_FACTOR) {
      idx = 0;
      /* Count, summarize & show values */
      if (charger_State != Stop) {
        Current = (sumCurrent + (INA_AVG_FACTOR/2)) / INA_AVG_FACTOR;
        Voltage = (sumVoltage + (INA_AVG_FACTOR/2)) / INA_AVG_FACTOR;
        sumCurrent = 0;
        sumVoltage = 0;

        Power = ((Voltage * Current) + 500) / 1000;

        Capacity_I += Current;
        Capacity_P += Power;

        chEvtBroadcast(&ina_all_event);
      } else {
      /* Show bus voltage in STOP mode */
        Voltage = (sumVoltage + (INA_AVG_FACTOR/2)) / INA_AVG_FACTOR;
        sumCurrent=0;
        sumVoltage=0;

        chEvtBroadcast(&ina_bus_event);
      }
    }
  }
}

/**
 * @brief GPT callback. One 16 pulse per second.
 */
static void gpt_cb(GPTDriver *gptp) {
  (void)gptp;
  static int cnt = 0;

  chSysLockFromISR();
  chBSemSignalI(&ina_bsem);
  chSysUnlockFromISR();
  
  cnt ++;
  if (cnt >= INA_AVG_FACTOR) {
    /* One second */
    cnt = 0;
  
    chSysLockFromISR();
    chBSemSignalI(&charger_bsem);
    chSysUnlockFromISR();
  }
}

/**
 * @brief   Insertion monitor timer callback function.
 *
 * @param[in] p         pointer to the @p BaseBlockDevice object
 *
 * @notapi
 */
static void sdc_tmrfunc(virtual_timer_t *vtp, void *p) {
  BaseBlockDevice *bbdp = p;

  (void)vtp;

  chSysLockFromISR();
  if (cnt > 0) {
    if (blkIsInserted(bbdp)) {
      if (--cnt == 0) {
        chEvtBroadcastI(&inserted_event);
      }
    }
    else
      cnt = POLLING_INTERVAL;
  }
  else {
    if (!blkIsInserted(bbdp)) {
      cnt = POLLING_INTERVAL;
      chEvtBroadcastI(&removed_event);
    }
  }
  chVTSetI(&sdc_tmr, TIME_MS2I(POLLING_DELAY), sdc_tmrfunc, bbdp);
  chSysUnlockFromISR();
}

/**
 * @brief   Polling monitor start.
 *
 * @param[in] p         pointer to an object implementing @p BaseBlockDevice
 *
 * @notapi
 */
static void sdc_tmr_init(void *p) {

  chEvtObjectInit(&inserted_event);
  chEvtObjectInit(&removed_event);
  chSysLock();
  cnt = POLLING_INTERVAL;
  chVTSetI(&sdc_tmr, TIME_MS2I(POLLING_DELAY), sdc_tmrfunc, p);
  chSysUnlock();
}

/*
 * Card insertion event.
 */
static void InsertHandler(eventid_t id) {
  FRESULT err;

  (void)id;
  /*
   * On insertion SDC initialization and FS mount.
   */
  if (sdcConnect(&SDCD1))
    return;

  err = f_mount(&SDC_FS, "/", 1);
  if (err != FR_OK) {
    sdcDisconnect(&SDCD1);
    return;
  }
  fs_ready = true;
  gwinPrintf(GW1, "SD Card connected and mounted.\n");
}

/*
 * Card removal event.
 */
static void RemoveHandler(eventid_t id) {
  (void)id;
  sdcDisconnect(&SDCD1);
  fs_ready = false;
  gwinPrintf(GW1, "SD Card disconnected.\n");
}


static FRESULT scan_files(BaseSequentialStream *chp, char *path) {
  static FILINFO fno;
  FRESULT res;
  DIR dir;
  size_t i;
  char *fn;

  res = f_opendir(&dir, path);
  if (res == FR_OK) {
    i = strlen(path);
    while (((res = f_readdir(&dir, &fno)) == FR_OK) && fno.fname[0]) {
      if (FF_FS_RPATH && fno.fname[0] == '.')
        continue;
      fn = fno.fname;
      if (fno.fattrib & AM_DIR) {
        *(path + i) = '/';
        strcpy(path + i + 1, fn);
        res = scan_files(chp, path);
        *(path + i) = '\0';
        if (res != FR_OK)
          break;
      }
      else {
        chprintf(chp, "%s/%s\r\n", path, fn);
      }
    }
  }
  return res;
}

static void cmd_tree(BaseSequentialStream *chp, int argc, char *argv[]) {
  FRESULT err;
  uint32_t fre_clust;
  FATFS *fsp;

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: tree\r\n");
    return;
  }
  if (!fs_ready) {
    chprintf(chp, "File System not mounted\r\n");
    return;
  }
  err = f_getfree("/", &fre_clust, &fsp);
  if (err != FR_OK) {
    chprintf(chp, "FS: f_getfree() failed\r\n");
    return;
  }
  chprintf(chp,
           "FS: %lu free clusters with %lu sectors (%lu bytes) per cluster\r\n",
           fre_clust, (uint32_t)fsp->csize, (uint32_t)fsp->csize * 512);
  fbuff[0] = 0;
  scan_files(chp, (char *)fbuff);
}