Stab-STM32/lib/stab/stab.c

#include "ch.h"
#include "hal.h"
#include "chprintf.h"
#include "stab_param.h"
//#include "stab_usart.h"
#include "st7735.h"

/* Defines */
/* Версия скетча и длина версии скетча в символах для правильного вывода на дисплей */
#define VERSION "v0.100"
#define VERSION_LEN 6

/* Variables */
static uint16_t Pnom; // Номинальная мощность ТЭНа (хранится в EEPROM и устанавливается из менюшки)
//const uint8_t ARRAY_SIZE = max(Pnom_ARR_SIZE,PDMset_ARR_SIZE);
static uint16_t PDMset[2][ARRAY_SIZE] = {};  // Массив уставок мощности ТЭНа с адресами
//static uint16_t (&Pnom_arr)[ARRAY_SIZE] = PDMset[0];  // Массив мощностей ТЭНа как ссылка на нулевую строку массива уставок
// (&Pnom_arr) - посилання на PDMset[0], в С не працює, замінив по тексту.

static volatile uint32_t sum;         // Сумматор квадратов отсчетов АЦП
static volatile uint16_t sc = 0;      // Счетчик просуммированных квадратов
static volatile uint16_t sc_sum = 0;  // Счетчик просуммированных квадратов, готовый к обработке
static volatile uint16_t Pust = 0;    // Установленная мощность ТЭНа
static volatile uint16_t pdm = 0;     // Текущий уровень PDM (принимает значения от 0 до CICLE)
static volatile int32_t pdm_err = 0;  // Ошибка дискретизации
static volatile uint16_t PDMust = 0;  // PDM, соответствующий установленной мощности ТЭНа

static volatile uint32_t U_sum = 0;   // Среднеквадратичное в сети за секунду, умноженное на 10
static uint16_t U_real = U_LINE;      // Среднеквадратичное за секунду (целая часть)
static uint8_t U_real_dec = 0;        // Среднеквадратичное за секунду (дробная часть)
static char buf[24];                  // common string buffer for chsnprintf()

static volatile uint8_t PID_ust = LINE_FREQ; // Данные для установки регистра сравнения таймера2

static virtual_timer_t second_vt, hz50_vt;

/* Организуем флаги и индикаторы в структуру */
static volatile struct flags {  // Флаги
  unsigned  dspRefresh : 1;   // Флаг выхода из режима меню / полного обновления экрана
  unsigned  dspTimeout : 1;   // Флаг истечения времени ожидания выхода из меню
  unsigned  dspNewData : 1;   // Флаг обновления данных на экране
  unsigned  PP : 1;           // Флаг полупериода сети на входе АЦП (отрицательная полуволна = 0, положительная = 1)
  unsigned  PP_fir : 1;       // Флаг полупериода после КИХ ФНЧ (отрицательная полуволна = 0, положительная = 1)
  unsigned  PP_tm : 1;        // Флаг полупериода по внутреннему таймеру (отрицательная полуволна = 0, положительная = 1)
  unsigned  zero : 1;         // Флаг перехода через ноль
  unsigned  NotZero : 1;      // Флаг аварии сети (не детектируются переходы через ноль)
  unsigned  sum : 1;          // Флаг готовности насуммированных данных к обработке
  unsigned  Tout : 1;         // Флаг включения ТЭНа (твердотельное реле)
  unsigned  TRelay : 1;       // Флаг включения ТЭНа (контактное реле)
  unsigned  Ulow : 1;         // Флаг невозможности выдать установленный уровень мощности
  unsigned  Udown : 1;        // Флаг аварии сети (действующее напряжение ниже 100В)
  unsigned  razg : 1;         // Флаг режима "разгон"
  unsigned  razg_on : 1;      // Флаг начала режима "разгон"
  unsigned  razg_off : 1;     // Флаг останова режима "разгон"
  unsigned  stab_off : 1;     // Флаг аварийного останова стабилизатора
  unsigned  butt : 1;         // Флаг опроса кнопок
#ifdef USE_EEPROM
  unsigned  writable : 1;     // Флаг записи уставок в EEPROM
#endif
#ifdef USE_USART  
  unsigned  uartUnhold : 1;   // Флаг разрешения передачи данных по USART
  unsigned  uartReport : 1;   // Флаг разрешения отправки данных внешнему контроллеру
  unsigned  uartTimeout : 1;  // Флаг истечения времени приема посылки по USART
#endif
} fl = {};  /* Инициализируем структуру с нулевыми членами */

static uint8_t cnt_Pnom_count;  // Количество предустановок мощности
static uint8_t cnt_Pnom_number; // Номер активной предустановки мощности
static uint8_t cnt_PDMcount;    // Счетчик для перебора уставок мощности ТЭНа

static uint8_t cnt_menuWDT;     // Счетчик секунд для организации отсчета ожидания выхода из меню
static uint8_t cnt_dspMenu;     // Индикатор режима меню

//static adcsample_t samples1[ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH];
static adcsample_t sample1;

/* Functions */
static uint8_t X_position(const uint8_t x, const uint16_t arg, const uint8_t pix); // Функция возвращает начальную позицию по Х для десятичного числа, в зависимости от количества знаков в нём.
static uint8_t X_centred(const uint8_t len); // Функция возвращает начальную позицию по Х для текста длинной len знаков, для размещения оного по центру дисплея.
static uint16_t calc_proportion(const uint16_t multiplier1, const uint16_t multiplier2, const uint32_t divider);
static void Buttons_(void);
static void second_new_cb(virtual_timer_t *vtp, void *p);
static void hz50_cb(virtual_timer_t *vtp, void *p);
static void ADC_cb(ADCDriver *adcp);
static void gpt_cb(GPTDriver *drv);

/*
 * Perephireal
 */
static const ADCConversionGroup adcgrpcfg1 = {
  FALSE,
  ADC_GRP1_NUM_CHANNELS,
  ADC_cb,
  NULL,
  0, 0,                         /* CR1, CR2 */
  ADC_SMPR1_SMP_AN10(ADC_SAMPLE_239P5), /* smpr1 */
  0,                            /* SMPR2 */
  ADC_SQR1_NUM_CH(ADC_GRP1_NUM_CHANNELS), /* sqr1 */
  0,                            /* SQR2 */
  ADC_SQR3_SQ1_N(ADC_CHANNEL_NUM) /* sqr3 */
};

static const GPTConfig gptcfg1 = {
  .frequency    =  5000U, /* 50 Hz * 100, 200 us */
  .callback     =  gpt_cb,
  .cr2          =  0, /* TIM_CR2_MMS_1,   MMS = 010 = TRGO on Update Event. */
  .dier         =  0U
};

/*
 * Threads
 */

/* BTN serving thread. */
static THD_WORKING_AREA(waBTNThread, 256);
static __attribute__((noreturn)) THD_FUNCTION(BTNThread, arg) {
  (void)arg;
  chRegSetThreadName("BTN_process");

  systime_t time = chVTGetSystemTimeX();
  while (true) {
    time += TIME_MS2I(BTN_SCAN_PERIOD);
    Buttons_();
    chThdSleepUntil(time);
  }
}

/*
 * ПРОЦЕДУРЫ И ФУНКЦИИ
 */

/**
 * @brief Функция возвращает начальную позицию по Х для десятичного числа,
 * в зависимости от количества знаков в нём.
 * @param arg выводимое число;
 * @param х позиция для arg, если бы оно было однозначно;
 * @param pix ширина шрифта в пикселях.
 */
static uint8_t X_position(const uint8_t x, const uint16_t arg, const uint8_t pix) {
  if (arg < 10) {
    return pix * x;
  } else if (arg < 100) {
    return pix * (x-1);
  } else if (arg < 1000) {
    return pix * (x-2);
  } else {
    return pix * (x-3);
  }
}

/**
 * @brief Функция возвращает начальную позицию по Х для текста длинной len знаков,
 * для размещения оного по центру дисплея.
 * @param len Количество знакомест в тексте
 */
static uint8_t X_centred(const uint8_t len) {
  uint8_t wdt = ST7735_WIDTH; // Ширина дисплея в пикселях
  uint8_t pix = 7;   // Ширина шрифта в пикселях
  if (len > wdt/pix) {
    return 0;
  } else {
    return (wdt - (len * pix))/2;
  }
}

#ifdef USE_EEPROM
/**
 * @brief Функция переводит символ ASCII в шестнадцатиричную цифру,
 * @return при ошибке возвращает 255
 * @param a символ 0...F
 */
static uint8_t A_to_HEX (const char a) {
  if (a >= 48 && a <= 57) { // Если а - от 0 до 9
    return (uint8_t)(a-48);
  } else if (a >= 65 && a <= 70) { // Если а - от A до F
    return (uint8_t)(a-55);
  } else if (a >= 97 && a <= 102) { // Если а - от a до f
    return (uint8_t)(a-87);
  } else {
    return 255;
  }
}

/**
 * @brief Функция переводит шестнадцатиричную цифру в символ ASCII,
 * @return при ошибке возвращает X
 * @param x число, кое необходимо перевести в ASCII-код
 */
static char HEX_to_A (const uint8_t x) {
  if (x <= 9) {
    return (char)(x + 48);
  } else if (x <= 15) {
    return (char)(x + 55);
  } else {
    return 'X';
  }
}
#endif

/**
 * @brief Подпрограммка остановки режима "Разгон"
 */
static void stop_razgon(void) {
  fl.razg_on = 0; //выключим режим разгона
  fl.TRelay  = 0; //выключим контактное реле
}

/**
 * @brief Подпрограммка подсчета Pust
 */
static void set_Pust(void) {
  Pust = calc_proportion(PDMust, Pnom, CICLE);
}

/**
 * @brief Функция пропорционального пересчета параметра
 * @returns Возвращает значение параметра с округлением,
 * пересчитанное из пропорции по формуле
 * (multiplier1 * multiplier2 / divider)
 * @param multiplier1 первый множитель,
 * @param multiplier2 второй множитель (по умолчанию Pnom),
 * @param divider делитель (по умолчанию CICLE).
 */
static uint16_t calc_proportion(const uint16_t multiplier1, const uint16_t multiplier2, const uint32_t divider) {
  uint32_t p;
  p = (long)multiplier1 * 2;
  p *= (long)multiplier2;
  p /= divider;
  p ++;
  p /= 2;
  return (uint16_t)p;
}

/**
 * @brief Пауза, измеряется в полупериодах
 */
static void pp_Delay(const uint16_t pp) {
  uint16_t PPcount = 0; // счетчик полупериодов
  bool PP_tm_last = 0;
  while (PPcount < pp) {
    //if (fl.PP_sint) {
    //  PPcount++;
    //  fl.PP_sint = 0;
    //}
    if (PP_tm_last != fl.PP_tm) {
      PPcount++;
      PP_tm_last = fl.PP_tm;
    }
  }
}

/**
 * @brief Подпрограмма запоминания последней уставки
 * Проверяет последнюю уставку на совпадение с уже записанными
 * в массив уставок и запоминает, если надо
 */
static void remember_last_power_setting(void) { // Запомним последнюю уставку
  bool isnew = 1;
  for (int8_t x = PDMset_ARR_SIZE - 1; x >= 0; x--) { // Проверим новое значение на совпадение с уже записанными
    if (PDMust == PDMset[0][x]) {
      isnew = 0;
      break;
    }
  }
  if (isnew) { // Если новое значение действительно новое, то...
    PDMset[0][PDMset_ARR_SIZE - 1] = PDMust; //Запоминаем текущую мощность ТЭНа
    PDMset[1][PDMset_ARR_SIZE - 1] = 0;      //Адрес зануляем на всякий случай
    cnt_PDMcount = PDMset_ARR_SIZE - 1;      //Ставим счетчик на запомненную уставку
  }
}

#ifdef USE_EEPROM
/**
 * @brief Подпрограмма обмена двух ячеек массива
 * @param arr массив,
 * @param index индекс первого измерения обмениваемых ячеек,
 * @param index1 индех второго измерения первой обмениваемой ячейки,
 * @param index2 индех второго измерения второй обмениваемой ячейки.
 */
static void change_arr_cell(uint16_t arr[2][ARRAY_SIZE], const uint8_t index, const uint8_t index1, const uint8_t index2) {
  uint16_t k = arr[index][index1];
  arr[index][index1] = arr[index][index2];// Обмениваемся
  arr[index][index2] = k;
}
#endif

/**
 * @brief Подпрограмма обработки режима разгона
 * Обеспечивает шунтирование контактов контактного реле
 * симистором твердотельного
 * в момент включения/выключения режима "Разгон"
 */
static void Razgon_(void) {
  static uint8_t cnt_P_relay=0; // Счетчик полупериодов шунтирования контактного реле
  if (fl.razg_on &&             // Если включен разгон..
      !fl.TRelay &&             // ..и НЕ включено контактное реле
      (++cnt_P_relay == RELAY_SHUNTING_TIME)) { // ..и все это длится уже более 500мс,
    fl.TRelay = 1;
    cnt_P_relay = 0; // то включим контактное реле и обнулим счетчик
  }
  if (fl.razg &&                // Если включен максимум для твердотельного реле..
      !fl.razg_on &&            // ..и выключен разгон
      (++cnt_P_relay == RELAY_SHUNTING_TIME)) { // ..и все это длится уже более 500мс,
    fl.razg = 0;
    cnt_P_relay = 0;  // то выключим реле и обнулим счетчик
  }
}

/**
 * @brief Подпрограмма управления твердотельным реле ТЭНа
 */
static void PDM_(void) {
  if (fl.razg) {
    pdm = CICLE; // В режиме разгона твердотельное всегда открыто
  } /* ??? поставити також fl.Tout та вийти. авіщо при разгоні все інше? */

  static int8_t ps = 0; // Текущее значение постоянной составляющей
  int32_t lev = pdm + pdm_err; // Текущий уровень с учетом ошибки дискретизации, сделанной на предыдущем полупериоде.
  // Текущее значение постоянной составляющей
  if (fl.PP_tm) {
    if (fl.Tout) {
      ps --;
    }
  } else {
    if (fl.Tout) {
      ps ++;
    }
  }

  if ((lev >= CICLE/2) && ((ps == 0) || (fl.PP_tm && (ps < 0)) || (!fl.PP_tm && (ps > 0)))) { // Ставим флаг включения ТЭНа с учетом значения постоянной составляющей
    fl.Tout = 1;
    pdm_err = lev - CICLE; // и считаем ошибку для следующего полупериода
  } else {
    fl.Tout = 0;
    pdm_err = lev; // Снимаем флаг включения ТЭНа и считаем ошибку
  }
}

/**
 * @brief Опрос кнопок
 */
static void Buttons_(void) {
  static uint8_t butt = 0; // код текущей нажатой кнопки
  static uint8_t last_butt = 0;  // код предыдущей нажатой кнопки
  static struct buttons {
    unsigned butt_1 : 1; // текущее состояние кнопки (0 - не нажата)
    unsigned butt_2 : 1; // текущее состояние кнопки
    unsigned butt_3 : 1; // текущее состояние кнопки
    unsigned butt_4 : 1; // текущее состояние кнопки
    unsigned no_select : 1; // вспомогательный флажок для начального меню
#ifdef USE_EEPROM
    unsigned writePnom : 1; // вспомогательный флажок записи нового Pnom в EEPROM
    unsigned clear_old : 1; // вспомогательный флажок стирания старой уставки из EEPROM
#endif
  } bt = {}; // Инициализируем структуру с нулевыми членами
  static uint8_t butt_count = 0; // счетчик для устранения дребезга
  static uint8_t butt_force_count = 0; // счетчик для форсирования инкремента/декремента

#ifdef USE_EEPROM
  if (bt.clear_old) { // Стираем старую уставку, если нужно
    eeprom_update_word((uint16_t*)clear_old_addr,EMPTY_CELL_VALUE); // Стираем самую старую уставку
    bt.clear_old = 0; // Снимаем флажок стирания
  }
#endif

  bt.butt_1 = pin_butt_1_STATE;
  bt.butt_2 = pin_butt_2_STATE;
  bt.butt_3 = pin_butt_3_STATE;
  bt.butt_4 = pin_butt_4_STATE;

  uint8_t button_sum = bt.butt_1 + bt.butt_2 + bt.butt_3 + bt.butt_4;
  if ((button_sum == 0) && butt_force_count) {
    butt_force_count --; // уменьшаем счетчик форсирования инкремента/декремента
  }
  if ( button_sum == fl.butt ) { // Или нажата одна кнопка или ни одной
    butt = bt.butt_1 + (bt.butt_2 << 1) + (bt.butt_3 << 2) + (bt.butt_4 << 3);
    if ( butt == last_butt ) {
      butt_count++;
    } else {
      butt_count = 1;
      last_butt = butt;
    }
  } else if (--butt_count < 1) {
    butt_count = 1;
  }

  if ( (butt_count == DEBOUNCE) || fl.dspTimeout ) { // Есть нажатая кнопка или достаточная пауза после нажатия или таймаут выхода из меню
    if (!fl.stab_off) { // Если нет аварийного останова...
      switch (cnt_dspMenu) { // Проверяем режимы меню
        case 2:  { // Если мы в начальном меню выбора номинальной мощности, то...
          if (fl.dspTimeout) { // Если кнопки слишком долго не нажимались...
            if (PDMset[0][0] != 0xffff) { // и есть записанное значение, уходим
              cnt_Pnom_number = 0;
              Pnom = PDMset[0][0]; // По умолчанию установим номинальную мощность из нулевой ячейки
#ifdef USE_EEPROM
              fl.writable = 1;     // Уставки пишутся в EERPOM
              EEPROM_read_PDMs();  // Читаем уставки
#endif
#ifdef USE_USART
              fl.uartUnhold = 1;   // Разрешим обращение к USART
#endif              
              cnt_dspMenu = 0;     // Выйдем из менюшки
              fl.dspRefresh = 1;   // Ставим флаг обновления экрана
            }
            fl.dspTimeout = 0;     // Снимаем флаг таймаута выхода из меню
            break;
          }
          switch (butt) {
            case 1: { //Кнопкой "P-" перебираем записанные значения или уменьшаем значение Pnom
              if (bt.no_select) { //Если не выбираем, а вводим значение,...
                if (butt_force_count > 20) { // Если очень долго держим...
                  if (Pnom > 100) {
                    Pnom -= 100; // Убавляем по соточке, пока есть куда
                  } else {
                    butt_force_count = 10; // Если некуда убавлять - снижаем форсаж
                  }
                } else if (butt_force_count > 10) { // Если долго держим...
                  if (Pnom > 10) {
                      Pnom -= 10; // Убавляем по десяточке, пока есть куда
                  } else {
                    butt_force_count = 0; // Если некуда убавлять - снижаем форсаж
                  }
                } else {
                  if (--Pnom == 0) {
                    Pnom=1; // Убавляем по чуть-чуть
                  }
                }
              } else { //Если выбираем из записанных в EEPROM...
                if (++cnt_PDMcount > cnt_Pnom_count) {
                  cnt_PDMcount=0; // Перебираем значения уставок мощности ТЭНа
                }
                Pnom = PDMset[0][cnt_PDMcount];
              }
              butt_force_count++;
              break; //Закончили
            }
            case 2: { //Кнопкой "P+" увеличиваем значение Pnom
              if (butt_force_count > 20) {
                if ((Pnom += 100) > 9999) {
                  Pnom=9999; // Если очень долго держим, прибавляем по соточке
                }
              } else if (butt_force_count > 10) {
                if ((Pnom += 10) > 9999) {
                  Pnom=9999; // Если долго держим, прибавляем по десяточке
                }
              } else {
                if (++Pnom > 9999) {
                  Pnom=9999; // Прибавляем по чуть-чуть
                }
              }
              bt.no_select = 1;
              butt_force_count++;
              break; //Закончили
            }
            case 4: { //Кнопкой "Стоп" пишем значение в память и выходим из менюшки
#ifdef USE_EEPROM
              bt.writePnom = 1; // Ставим флаг записи нового значения Pnom в EEPROM
              fl.writable = 1;  // Ставим флаг записи уставок в EEPROM
#endif
            }
            case 8: { //Кнопкой "Разгон" выходим из менюшки
              if (Pnom < 10000) { // Если значение реальное...
                cnt_Pnom_number = cnt_PDMcount; // Запомним порядковый номер выбранного Pnom
                if (bt.no_select) {             // Если значение НЕ выбрано из записанных в EEPROM, а введено...
                  for (int8_t x = cnt_Pnom_count; x >= 0; x--) { // Проверим новое значение на совпадение с уже записанными
                    if (Pnom == PDMset[0][x]) { // Если такое значение уже есть в EEPROM...
                      cnt_Pnom_number = x;      // Запомним порядковый номер совпавшего Pnom
#ifdef USE_EEPROM
                      bt.writePnom = 0;         // Снимем флаг записи нового значения Pnom в EEPROM
                      fl.writable = 1;          // Ставим флаг записи уставок в EEPROM
#endif
                      break;
                    }
                  }
                } else {                        // Если значение выбрано из записанных в EEPROM...
#ifdef USE_EEPROM
                  bt.writePnom = 0;             // Снимем флаг записи нового значения Pnom в EEPROM
                  fl.writable = 1;              // Ставим флаг записи уставок в EEPROM
#endif
                }

                cnt_PDMcount=0;                 //Сбрасываем счетчик
#ifdef USE_EEPROM
                if (fl.writable) {              // Если уставки пишутся в EERPOM, то
                  EEPROM_read_PDMs();           // читаем ранее записанное
                }
#endif
#ifdef USE_EEPROM
                if (bt.writePnom) { // Запишем новое значение Pnom, если необходимо
                  eeprom_update_word((uint16_t*)(cnt_Pnom_number * 2),Pnom);
                  bt.writePnom = 0; // и сбросим флаг записи нового значения Pnom
                }
#endif
                cnt_dspMenu = 0;    // Снимаем флаг перехода в меню
                //
#ifdef USE_USART
                fl.uartUnhold = 1;  // Разрешим обращение к USART
#endif /* USE_USART */
                fl.dspRefresh = 1;  // Ставим флаг обновления экрана
              }
              fl.butt = 0;          // После нажатия должна быть пауза
              break;                // Закончили
            }
            default:
              fl.butt = 1;          // достаточная пауза между нажатиями
          }
          break;
        }
        case 1:  { // Если мы в меню выбора уставки, то...
          if (fl.dspTimeout) {  // Если кнопки слишком долго не нажимались, уходим
            cnt_dspMenu = 0;    // Выйдем из менюшки
            fl.dspRefresh = 1;  // Ставим флаг обновления экрана
            fl.dspTimeout = 0;  // Снимаем флаг таймаута выхода из меню
            break;
          }
          switch (butt) {
            case 1: { // По кнопке "Р-" перебираем значения
              if (++cnt_PDMcount > PDMset_ARR_SIZE - 1) {
                cnt_PDMcount=0; //Перебираем значения уставок мощности ТЭНа
              }
              //fl.butt = 0; //После нажатия должна быть пауза
              break; //Закончили
            }
            case 2: { // По кнопке "Р+" перебираем значения
              if (cnt_PDMcount-- == 0) {
                cnt_PDMcount=PDMset_ARR_SIZE - 1; //Перебираем значения уставок мощности ТЭНа
              }
              //fl.butt = 0; //После нажатия должна быть пауза
              break; //Закончили
            }
            case 4: { //По кнопке "стоп" записываем уставку, если нужно, принимаем и выходим
              PDMust = PDMset[0][cnt_PDMcount]; //Устанавливаем выбранную мощность ТЭНа
#ifdef USE_EEPROM
              if (fl.writable) { // Если уставки запоминаются...
                if (!PDMset[1][cnt_PDMcount]) { // Если просят записать НЕ уже записанное...
                  //eeprom_update_word((uint16_t*)new_addr,PDMset[0][cnt_PDMcount]); // Пишем новую уставку
                  PDMset[1][cnt_PDMcount] = new_addr; // Заносим в массив адрес свежезаписанной уставки
                  new_addr += 2;
                  if (new_addr > end_addr) {
                    new_addr = start_addr; // Инкрементируем адрес для новой уставки и следим, чтобы не выходило за границы области
                  }
                  if (cnt_PDMcount == PDMset_ARR_SIZE - 1) {  // Если новое значение - последнее в списке
                    if (!old_addr) { // Если в массиве уставок есть незаписанные в EEPROM значения, то сначала стираем их
                      bool swapped = 1;
                      uint8_t upper_index = PDMset_ARR_SIZE - 1; //Пузырьковая сортировка
                      while (swapped) { // Пока есть обмены, сортируем
                        swapped = 0;
                        for (uint8_t i = 1; i < upper_index; i++) {
                          if (PDMset[1][i] < PDMset[1][i - 1]) {
                            change_arr_cell(PDMset, 0, i, i - 1);
                            change_arr_cell(PDMset, 1, i, i - 1);
                            swapped = 1;
                          }
                        }
                        upper_index --;
                      } //Закончили сортировку
                      old_addr = PDMset[1][0];  // Обновляем адрес самой старой уставки
                    }

                    if (old_addr) {           // Если в массиве уставок все значения записаны в EEPROM, то стираем самое старое
                      bt.clear_old = 1;       // Ставим флажок стирания (сотрём в следующий вызов подпрограммы опроса кнопок)
                      clear_old_addr = old_addr; // Плодим сущности без устали!
                    }

                    uint16_t k = PDMset[0][0];
                    for (uint8_t x = 0; x < PDMset_ARR_SIZE - 1; x++) { // Сдвинем массив
                      PDMset[0][x] = PDMset[0][x + 1];
                      PDMset[1][x] = PDMset[1][x + 1];
                    }

                    PDMset[0][PDMset_ARR_SIZE - 1] = k; // Запишем во временную ячейку свежеудаленное значение
                    PDMset[1][PDMset_ARR_SIZE - 1] = 0;
                    cnt_PDMcount --;
                    old_addr = PDMset[1][0];    // Обновляем адрес самой старой уставки
                  }
                }
              }
#endif /* USE_EEPROM */
              cnt_dspMenu = 0;   //Снимаем флаг перехода в меню
              fl.dspRefresh = 1; //Ставим флаг обновления экрана
              fl.butt = 0;       //После нажатия должна быть пауза
              break;             //Закончили
            }
            case 8: { // По кнопке "разгон" принимаем и выходим
              PDMust = PDMset[0][cnt_PDMcount]; //Устанавливаем выбранную мощность ТЭНа
              cnt_dspMenu = 0;   //Снимаем флаг перехода в меню
              fl.dspRefresh = 1; //Ставим флаг обновления экрана
              fl.butt = 0;       //После нажатия должна быть пауза
              break;             //Закончили
            }
            default:
              fl.butt = 1;       // достаточная пауза между нажатиями
          }
          break;
        }
        default: { // А если не в меню, то...
          switch (butt) {
            case 1:
              if (PDMust-- == 0) {
                PDMust = 0; //Уменьшаем установленную мощность до минимума
              }
              break;
            case 2:
              if (++PDMust > CICLE) {
                PDMust = CICLE; //Увеличиваем установленную мощность до максимума
              }
              break;
            case 4:
              if (PDMust == 0) { // Если мы не в меню и мощность ТЭНа нулевая, то...
                cnt_dspMenu = 1;   //Ставим флаг перехода в меню
                fl.dspRefresh = 1; //Ставим флаг обновления экрана
              } else {             //Если мы не в меню и мощность ТЭНа НЕнулевая, то...
                remember_last_power_setting();// Запомним последнюю уставку
                PDMust = 0;        //Экстренно выключим ТЭН
                stop_razgon();     //Остановим разгон
              }
              fl.butt = 0;         //После нажатия должна быть пауза
              break;
            case 8:
              fl.razg_on = ((!fl.NotZero) & (!fl.Udown) & (!fl.razg_off) & (!fl.razg_on)); //Триггер режима разгона (гистерезис организован в обработке начала полупериода)
              fl.razg |= fl.razg_on;   //Если разгон включили, то твердотельное реле на максимум сразу
              fl.TRelay &= fl.razg_on; //Если разгон выключили, то контактное реле выключаем сразу
              fl.butt = 0;             //После нажатия должна быть пауза
              break;
            default:
              fl.butt = 1; // достаточная пауза между нажатиями
          }
        }
      }
    }

    if (butt) { // Если нажата кнопка,
      cnt_menuWDT = 0; // сбросим таймер ожидания выхода из меню
      fl.stab_off = 0; // и сбросим флажок аварийного останова
    }
    butt_count = 1;
    butt = 0;
    set_Pust();        // Пересчитаем Pust
    fl.dspNewData = 1; //Обновление информации на дисплее
  }

  if (pin_STAB_OFF_STATE && !fl.stab_off) { // Если есть сигнал аварийного останова
    if (PDMust) { // Если уставка ненулевая...
      remember_last_power_setting();// Запомним последнюю уставку
      PDMust = 0;                   // Экстренно выключим ТЭН
      Pust = 0;                     // Пересчитаем Pust
    }
    stop_razgon();    // Остановим разгон
    fl.dspNewData = 1;//Обновление информации на дисплее
    fl.stab_off = 1;  // Поставим соответствующий флажок
  } else {
    fl.razg_off = pin_RAZGON_OFF_STATE; // Прочитаем состояние вывода отключения разгона
    if (fl.razg_off && fl.razg_on) {  // Если разгон и есть внешний сигнал останова разгона...
      fl.dspNewData = 1;  //Обновление информации на дисплее
      stop_razgon();  // остановим разгон
    }
  }
}

/**
 * @brief GPT callback function
 * 
 * @param drv 
 */
static void gpt_cb(GPTDriver *drv) {
  (void)drv;
  adcStartConversion(&ADCD1, &adcgrpcfg1, &sample1, ADC_GRP1_BUF_DEPTH);
}

/**
 * @brief Обработчик окончания преобразования АЦП
 */
static void ADC_cb(ADCDriver *adcp) {
  (void)adcp;

  static uint16_t TM2_current = 0;
  static int16_t Ufir = 0;   // Буферная переменная для НЧ-фильтрации
  static int16_t Udelta = 0; // Буферная переменная для НЧ-фильтрации
  {
    int32_t U_adc;
    uint8_t TM2_tmp;
    //TM2_tmp = TCNT2;  // забрали значение из таймера синхронизации с сетью
    U_adc = sample1; // забрали результат преобразования АЦП
    U_adc -= U_ZERO;  // Убираем постоянную составляющую из оцифрованного сигнала
    { //Суммирование квадратов
      sum += (long)U_adc * U_adc; // Возводим в квадрат выборку АЦП и суммируем с предыдущими
      ++ sc; // Счетчик выборок АЦП
    }
    /* детекция перехода через ноль и ПИД-синхронизация */
    Udelta += (U_adc - Ufir);
    U_adc = Udelta / 32; //КИХ ФНЧ 1-го порядка с коэффициентом 1/32

    static uint8_t cnt_P_sum = 0;    // Счетчик полупериодов для суммирования отсчетов АЦП
    static uint16_t cnt_notzero = 0; // Счетчик выборок АЦП без перехода через ноль

    if ((!fl.zero) && (U_adc >= 0) && (Ufir <= 0) && (U_adc != Ufir)) {
      // переход через ноль детектед
      cnt_notzero = 0; // Обнуляем счетчик выборок АЦП без перехода через ноль
      fl.NotZero = 0; // Снимаем флажок отсутствия детекции перехода через ноль

      /* Проверка насуммированных отсчетов */
      if (++cnt_P_sum == PSUM_MAX) {
        U_sum = sum;
        fl.sum = 1;
        sc_sum = sc; // Насуммированное готово к обработке
        sc = 0;
        sum = 0;
        cnt_P_sum = 0; // Сбрасываем счетчик, сумматор и счетчик полупериодов
      }
      // !!!TM2_current = TM2_tmp;  // Запомним значение для дальнейшей обработки
      fl.zero = 1;
    } else {
      // переход через ноль NOT детектед
      fl.zero = 0;
      if (++cnt_notzero == ZSUM_MAX) {  // Насуммировали достаточно
        fl.NotZero = 1;
        cnt_notzero = 0;
        PID_ust = LINE_FREQ;
        stop_razgon();
        pdm = 0;
        fl.Tout = 0; //выключим твердотельное реле
        U_real = 0;
        sc = 0;
        sum = 0;
        cnt_P_sum = 0; // Обнулим счетчик, сумматор, счетчик полупериодов и значение напряжения
        fl.dspNewData = 1;
      }
    }

    Ufir = U_adc;
  }

  /* детекция перехода через ноль и ПИД-синхронизация */
  if (fl.zero) { // ПИД-подстройка частоты внутреннего таймера к частоте сети
    static uint16_t PID_reg = LINE_FREQ << Km; // Функция управления ПИД
    static int32_t PID_err_old = 0; // Разность фаз из предыдущего шага
    static int32_t PID_int = 0; // Интегральная составляющая из предыдущего шага
    int32_t temp_32 = (TM2_current + PHASE) << Km; // Разность фаз

    if (!fl.PP_tm) {
      temp_32 -= PID_reg + (1 << Km);  // Разность фаз должна быть с соответствующим знаком
    }
    PID_int += (temp_32 >> Ki);                 // Считаем интегральную составляющую
    PID_reg += temp_32 >> Kp;                   // Считаем новую функцию управления
    PID_reg += PID_int;
    PID_reg += ( temp_32 - PID_err_old ) >> Kd;
    PID_err_old = temp_32;

    // Готовим данные для записи в регистр сравнения таймера 2
    if ( PID_reg > (T_MAX << Km)) {
      PID_reg = (T_MAX << Km);  // Ограничим сверху
    } else if ( PID_reg < (T_MIN << Km)) {
      PID_reg = (T_MIN << Km);  // Ограничим снизу
    }
    temp_32 = PID_reg >> (Km - 1); // ...и правильно округлим
    temp_32 ++;               // используя уже не нужную в этой подпрограмме
    PID_ust = temp_32 / 2;    // переменную temp_32
  } /* ПИД-подстройка частоты внутреннего таймера к частоте сети */
}

/**
 * @brief Подпрограмма обновления меню
 */
static void RefreshMenu (void) {
  ST7735_FillScreen(ST7735_BLACK);
  ST7735_WriteString(0, 6, "Ст Принять и записать", Font_7x10, ST7735_BLUE, ST7735_BLACK);
  ST7735_WriteString(0, 7, "Рз Принять без записи", Font_7x10, ST7735_BLUE, ST7735_BLACK);
  ST7735_WriteString(X_position(1, 0, 7), 3, "Управление:", Font_7x10, ST7735_BLUE, ST7735_BLACK);
  ST7735_WriteString(0, 4, "P- Выбор", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#ifdef INTERFACE_ALT

#else
  ST7735_WriteString(0, 0, "Выберите", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
  //
  switch (cnt_dspMenu) { //Проверяем режимы меню
    case 2:  { //Если мы в начальном меню, то...
 #ifdef INTERFACE_ALT
      chsnprintf(buf, 24, "%3u V", U_LINE);
      ST7735_WriteString(X_position(16, 0, 7), 0, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
      ST7735_WriteString(0, 1, "Рном=         Вт", Font_7x10, ST7735_BLUE, ST7735_BLACK);
 #else
      ST7735_WriteString(X_position(8, 0, 7), 0, "/введите Рном", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      chsnprintf(buf, 24, "Рном=      Вт, (%3u V)", U_LINE);
      ST7735_WriteString(0, 1, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
 #endif
      ST7735_WriteString(0, 2, "==Мощность нагрузки==", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      ST7735_WriteString(X_position(8, 0, 7), 4, "/уменьшение", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      ST7735_WriteString(0, 5, "P+ Увеличение", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      break;
    }

    case 1:  { //Если мы в меню выбора уставки, то...
 #ifdef INTERFACE_ALT
      ST7735_WriteString(0, 1, "Руст=         Вт", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#else
      ST7735_WriteString(X_position(9, 0, 7), 0, "уставку", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      ST7735_WriteString(0, 1, "Руст=      Вт", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
      ST7735_WriteString(0, 2, "=======Уставка=======", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      ST7735_WriteString(0, 5, "P+ Выбор", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#ifdef USE_EEPROM
      if (!fl.writable) {  // Если уставки не пишутся в EEPROM, то...
        ST7735_WriteString(0, 6, "Ст Принять без записи", Font_7x10, ST7735_BLUE, ST7735_BLACK);
      }
#endif
      break;
    }

    default: {
    }
  }
}

/**
 * @brief Подпрограмма печати строки минусов
 * @param str - номер строки, куда печатать минуса
 */
static void menu_print_minus(const uint8_t str) {
  ST7735_WriteString(0, str, "----------------------", Font_7x10, ST7735_RED, ST7735_BLACK);
}

/**
 * @brief Initialization of 'Power Stabilizator'
 */
void Stab_Init(void) {
  cnt_dspMenu = 2; // Сначала - начальное меню

  /* Инициализируем входы/выходы - was done in board.h */
  TURN_RELAY_OFF; // Выключаем ТЭН (контактное реле)
  TURN_SSR_OFF;   // Выключаем ТЭН (твердотельное реле)
  BTNS_ON;  // Activate buttons

  /* Activates the ADC1 driver and the temperature sensor. */
  adcStart(&ADCD1, NULL);

  /* Starting GPT3 driver, it is used for triggering the ADC. */
  gptStart(&GPTD3, &gptcfg1);

  /* Starting a virtual timers. */
  chVTSetContinuous(&hz50_vt, TIME_MS2I(LINE_PERIOD), hz50_cb, NULL);
  chVTSetContinuous(&second_vt, TIME_MS2I(1000), second_new_cb, NULL);

  /* Starting the Button thread. */
  chThdCreateStatic(waBTNThread, sizeof(waBTNThread), NORMALPRIO, BTNThread, NULL);

  pp_Delay(20);   // Подождем 20 полупериодов

#ifdef DisplayReset
  pin_OLEDres_HIGH; // Разрешаем работу дисплея
#endif

  pp_Delay(10);   // Подождем 10 полупериодов для гарантированного разрешения

  //ST7735_Init(); // done in main()
  ST7735_FillScreen(ST7735_BLACK);
  ST7735_WriteString(X_centred(21), 0, "Стабилизатор мощности", Font_7x10, ST7735_BLUE, ST7735_BLACK);
  ST7735_WriteString(X_centred(4), 1, "ТЭНа", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#ifdef INTERFACE_ALT
  ST7735_WriteString(X_centred(16), 2, "STAB-AVR", Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
  ST7735_WriteString(X_centred(8), 2, "STAB-AVR", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
  ST7735_WriteString(X_centred(VERSION_LEN), 4, VERSION, Font_7x10, ST7735_BLUE, ST7735_BLACK);
  ST7735_WriteString(X_centred(10), 6, "JohnJohnov", Font_7x10, ST7735_BLUE, ST7735_BLACK);
  ST7735_WriteString(X_centred(17), 7, "alcodistillers.ru", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#ifdef USE_EEPROM
  EEPROM_read_Pnoms(); // Прочитаем из EEPROM записанные номиналы ТЭНов
#endif
  pp_Delay(800);       // Подождем 600 полупериодов, пережидаем переходные процессы и любуемся заставкой

  fl.dspRefresh = 1;

#ifdef USE_USART
/*
 * USART initialization
 * Если задействовано управление регулятором ТЭНа через UART, инициализируем оный
 */
  USART_start();
#endif /* USE_USART */
}

/**
 * @brief 'Power Stabilizator' working cycle
 */
void Stab_WorkCycle(void) {
  /* Обработка данных от АЦП и корректировка выдаваемой мощности */
  if (fl.sum) {
#ifdef NOT_LM358
    // 0,55 - Коэффициент нормирования ((380/512)^2, 380В максимальное амплитудное) для Rail-to-Rail операционника
    U_sum /= sc_sum;    //Ненормированный квадрат среднеквадратичного
    U_sum *= 0.55;      //Нормированный квадрат среднеквадратичного
#else
    // 3 - Коэффициент нормирования ((380/220)^2, 380В максимальное амплитудное) для стандартно установленного LM358
    U_sum *= 3;         //Нормированная сумма квадратов среднеквадратичного
    U_sum /= sc_sum;    //Нормированный квадрат среднеквадратичного
#endif
    /* Корректируем pdm
    //    uint32_t tmp; // Величины великоваты, чтобы попасть в размерность приходится считать аккуратно
    //    //    pdm = U_LINE_Q*PDMust/(U_sum);
    //    tmp = (long)U_LINE_Q * 2;
    //    tmp *= (long)PDMust;
    //    tmp /= U_sum;
    //    tmp++;
    //    tmp /= 2;
    */
    uint16_t tmp = calc_proportion(PDMust, U_LINE_Q, U_sum);

    if (tmp > CICLE || fl.razg) { // Следим, чтобы pdm не превышала CICLE
      pdm = CICLE;
      fl.Ulow = !fl.razg; // Или напряжение сети не позволяет выдать установленный уровень мощности, или разгон
    } else {
      fl.Ulow = 0;
      pdm = tmp;
    }

    // Проверяем величину напряжения
    U_sum *= (long)400;      // Произведем некоторое математическое колдунство,
    U_sum *= (U_sum);     // чтобы получить один знак после запятой без float
    U_sum ++;
    U_sum /= 2;    // и с правильным округлением.
    U_real_dec = U_sum % 10; // Среднеквадратичное (дробная часть)
    U_real = U_sum / 10;     // Среднеквадратичное (целая часть)

    // Контролируем значение
    if ( U_real < U_MIN ) { //Действующее напряжение сети ниже U_MIN - отключим ТЭН (авария)
      fl.Udown = 1;       //поставим флажок низкого сетевого
      stop_razgon();
      pdm = 0;            //выключим твердотельное реле
    } else {
      fl.Udown = 0;
    }
    fl.sum = 0;
    fl.dspNewData = 1;  //Обновление информации на дисплее
  }

#ifdef USE_ADprotocol
  /* Отправка отчета внешнему контроллеру */
  if (fl.uartReport && fl.uartUnhold) {
    USART_report();
    fl.uartReport = 0;
  }
#endif
  /* Вывод информации на дисплей */
  if (fl.dspNewData) {
    if (fl.dspRefresh) {
      RefreshMenu(); //Обновляем дисплей, если надо
    }
    switch (cnt_dspMenu) { // Проверяем режимы меню
      case 2:  { // Если мы в начальном меню, то...
        static uint16_t Pnomold = 0;
        if (!Pnom || Pnom > 9999) {
          Pnomold = Pnom;
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(3, 0, 11), 0, "****", Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(6, 0, 7), 1, "****", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
        } else if ((Pnomold != Pnom) || fl.dspRefresh) {
          Pnomold = Pnom;
          chsnprintf(buf, 24, "%u", Pnom);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(6, Pnom, 11), 0, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(9, Pnom, 7), 1, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
        }
        fl.dspRefresh = 0;
        break;
      }
      case 1:  { // Если мы в меню выбора уставки, то...
        static uint16_t PDMold = 0;
        if ((PDMold != PDMset[0][cnt_PDMcount]) || fl.dspRefresh) {
          PDMold = PDMset[0][cnt_PDMcount];
          uint16_t p = calc_proportion(PDMold, Pnom, CICLE); // Считаем уставку с округлением
          chsnprintf(buf, 24, "%u", p);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(6, p, 11), 0, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(9, p, 7), 1, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
#ifdef USE_EEPROM
          if (PDMset[1][cnt_PDMcount]) { // Если значение записано в EEPROM
            ST7735_WriteString(X_position(20, 0, 7), 1, "R", Font_7x10, ST7735_BLUE, ST7735_BLACK); // поставим значок
          } else {
            ST7735_WriteString(X_position(20, 0, 7), 1, " ", Font_7x10, ST7735_BLUE, ST7735_BLACK); // а если не записано - уберем
          }
#endif
        }
        fl.dspRefresh = 0;
        break;
      }
      default: { // А если не в меню, то...
#ifdef INTERFACE_ALT
#define str_Ureal_big 0
#define str_Ureal 1
#define str_ust_big 3
#define str_ust 4
#define str_Ustat 2
#define str_Razgon 5
#define str_Pnom 6
#define str_Relay 7
#else
#define str_Ureal 0
#define str_Ustat 1
#define str_ust 3
#define str_Pnom 6
#define str_Razgon 4
#define str_Relay 7
#endif

        if (fl.dspRefresh) {  //Обновляем дисплей
          ST7735_FillScreen(ST7735_BLACK);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(8, 0, 7), str_ust, "Вт       ,  %", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(0, str_ust, "Руст      Вт;    ,  %", Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
          ST7735_WriteString(0, str_Ureal, "Напр.сети        ,  В", Font_7x10, ST7735_BLUE, ST7735_BLACK);
          chsnprintf(buf, 24, "Ном. мощность %u Вт", Pnom);
          ST7735_WriteString(0, str_Pnom, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
          //ST7735_WriteString(X_position(0, 0, 7), str_Relay, "Реле ", Font_7x10, ST7735_BLUE, ST7735_BLACK);
        }

        static uint16_t U_real_old = 0;
        if ((U_real_old != U_real) || fl.dspRefresh) {
          U_real_old = U_real;
          chsnprintf(buf, 24, "%u", U_real_old);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(7, U_real_old, 11) + 5, str_Ureal_big, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(16, U_real_old, 7), str_Ureal, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
        }
        static uint8_t U_real_dec_old = 0;
        if ((U_real_dec_old != U_real_dec) || fl.dspRefresh) {
          U_real_dec_old = U_real_dec;
          chsnprintf(buf, 24, "%u", U_real_dec);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(9, 0, 11), str_Ureal_big, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(18, 0, 7), str_Ureal_big, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#endif
        }

        static uint16_t Pust_old = 0;
        if ((Pust_old != Pust) || fl.dspRefresh) {
          Pust_old = Pust;
          chsnprintf(buf, 24, "%u", Pust_old);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(3,Pust_old,11), str_ust_big, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(8,Pust_old,7), str_ust, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
        }

        static uint16_t PDMust_old = 0;
        if ((PDMust_old != PDMust) || fl.dspRefresh) {
          PDMust_old = PDMust;
          uint32_t x = 1000*(long)PDMust_old;
          x /= CICLE;
          uint8_t percent = x / 10;     // посчитаем процент
          uint8_t percent_dec = x % 10; // посчитаем десятые процента
          chsnprintf(buf, 24, "%u.%u", percent, percent_dec);
#ifdef INTERFACE_ALT
          ST7735_WriteString(X_position(7,100,11) + 5, str_ust_big, buf, Font_11x18, ST7735_BLUE, ST7735_BLACK);
#else
          ST7735_WriteString(X_position(13, 0, 7), str_ust, buf, Font_7x10, ST7735_BLUE, ST7735_BLACK);
#endif
        }

        if (fl.Udown || fl.NotZero) {
          ST7735_WriteString(0, str_Ustat, "-----Авария сети-----", Font_7x10, ST7735_BLUE, ST7735_BLACK);
        } else if (fl.Ulow) {
          ST7735_WriteString(0, str_Ustat, "--Недост.напр. сети--", Font_7x10, ST7735_BLUE, ST7735_BLACK);
        } else {
          menu_print_minus(str_Ustat);
        }

        if (fl.razg_on) {
          static uint8_t count_1 = 0;
          uint8_t x1 = 5 - count_1;
          uint8_t x2 = 20 - x1;
          ST7735_WriteString(0, str_Razgon, "------<Разгон!>------", Font_7x10, ST7735_BLUE, ST7735_BLACK);
          ST7735_WriteString(X_position(x1, 0, 7), str_Razgon, "<", Font_7x10, ST7735_BLUE, ST7735_BLACK);
          ST7735_WriteString(X_position(x2, 0, 7), str_Razgon, ">", Font_7x10, ST7735_BLUE, ST7735_BLACK);
          if (++count_1 > 5) count_1 = 0;
        } else {
          menu_print_minus(str_Razgon);
        }

        {
          static uint8_t trigger = 1;
          if (trigger && fl.stab_off) {
            ST7735_WriteString(0, str_Relay, "!!АВАРИЙНЫЙ ОСТАНОВ!!", Font_7x10, ST7735_BLUE, ST7735_BLACK);
            trigger = 0;
          } else {
            menu_print_minus(str_Relay);
            trigger = 1;
          }
        }

        //if (fl.TRelay) {
        //  ST7735_WriteString(X_position(5, 0, 7), str_Relay, "включено", Font_7x10, ST7735_BLUE, ST7735_BLACK);
        //} else {
        //  ST7735_WriteString(X_position(5, 0, 7), str_Relay, "        ", Font_7x10, ST7735_BLUE, ST7735_BLACK);
        //}
        fl.dspRefresh = 0;
      }
    }
    //
    fl.dspNewData = 0;
  }

#ifdef USE_USART
  if (fl.uartUnhold) {
    USART_parser();
  }
#endif
}

/**
 * @brief Process asks for 1 second period
 * 
 * @param vtp 
 * @param p 
 */
static void second_new_cb(virtual_timer_t *vtp, void *p) {
  (void)vtp;
  (void)p;

  //fl.dspNewData = 1;  // Раз в секунду не грех обновить дисплей, мало ли...
  if ((cnt_dspMenu > 0) && (++cnt_menuWDT == MENU_TIMEOUT)) { // Если мы в меню и слишком долго не жмутся кнопки
    fl.dspTimeout = 1;                                        // Установим флаг таймаута
    cnt_menuWDT = 0;                                          // Сбросим таймер ожидания выхода из меню
  }

#ifdef USE_USART
  if (++cnt_uartWDT == 10) {  // Если прошло уже 10 секунд от начала приема посылки по USART
    fl.uartTimeout = 1;       // Установим флаг таймаута ожидания окончания посылки
    cnt_uartWDT = 0;          // Сбросим таймер ожидания окончания посылки
  }
#endif
#ifdef USE_ADprotocol
  fl.uartReport = 1;          // пора слать рапорт
#endif
}

/**
 * @brief Process tasks with line frewuency
 * 
 * @param vtp 
 * @param p 
 */
static void hz50_cb(virtual_timer_t *vtp, void *p) {
  (void)vtp;
  (void)p;

  Razgon_();
  if (pdm) {
    PDM_();
  } else {
    fl.Tout = 0;   // Не будем зря теребить подпрограмму, если pdm = 0
    //pdm_err = 0; // и обнулим ошибку дискретизации (а нужно ли?) TODO!
  }

  fl.PP_tm = !fl.PP_tm; // Инвертируем флаг полуволны
  // !!! OCR2A = PID_ust; // Грузим новое значение в регистр сравнения

  fl.Tout ? TURN_SSR_ON : TURN_SSR_OFF ;  // Включаем или выключаем ТЭН (твердотельное реле)
  fl.TRelay ? TURN_RELAY_ON : TURN_RELAY_OFF ;  // Включаем или выключаем ТЭН (контактное реле)
}