#define Q_SIZE_FAST 16
volatile int F1_last;   // последний элемент в очереди
int F1_first;     // первый элемент в очереди
void (*F1_Queue[Q_SIZE_FAST])(); //очередь указателей на функции(процедуры)
void DummyF(void){;}
void F1_QueueIni(void){ // инициализация очереди
  F1_last = 0;
  F1_first = 0;
}
int F1_push(void (*pointerQ)(void)){ // добавить элемент в очередь
  if ((F1_last+1)%Q_SIZE_FAST == F1_first)return 1;
  F1_Queue[F1_last++] = pointerQ;
  F1_last %= Q_SIZE_FAST;
  return 0;
}
void (*F1_pull(void))(void){ // взять элемент из очереди -
                             //возвращает указатель
  void (*pullVar)(void);
  if (F1_last == F1_first)return DummyF;
  pullVar = F1_Queue[F1_first++];
  F1_first %= Q_SIZE_FAST;
  return pullVar;
}

void DelayOnF1(uint64_t delay){
uint64_t targetTime = delay + millis();
  while(millis() < targetTime) F1_pull()();
}

F1_QueueIni();
F2_QueueIni();
F3_QueueIni();
F4_QueueIni();
while(1){
  F1_pull()();
  F2_pull()();
  F3_pull()();
  F4_pull()();
}

F1_push(LED_On_Off);
F1_push(ReadChannelsVoltage);

F2_push(CalculateTemperatureMiddleValue);
F2_push(CalculateHumidityMiddleValue);

F3_push(Display_ScreenInfo);
F3_push(ResetSensor);

F4_push(ScanKeyBoard);
F4_push(ReadTouchScr);

struct fParams {  // структура для передачи аргументов
  int IntVar;
  float FloatVar;
};
volatile int FP_last;  // последний элемент в очереди
int FP_first;  // первый элемент в очереди
void (*FP_Queue[Q_SIZE_FAST])(struct fParams *); //очередь указателей
                                                 //на функции с параметрами
struct fParams PARAMS_array[Q_SIZE_FAST];        //очередь для хранения аргументов
void FP_QueueIni(void){ // инициализация очереди
  FP_last = 0;
  FP_first = 0;
}
int FP_push(void (*pointerQ)(struct fParams *), struct fParams * parameterQ){ // добавить функцию  очередь
  if ((FP_last+1)%Q_SIZE_FAST == FP_first)return 1;
  FP_Queue[FP_last] = pointerQ;
  PARAMS_array[FP_last++] = *parameterQ;
  FP_last %= Q_SIZE_FAST;
  return 0;
}
void FP_pull(void){ // взять и выполнить функцию из очереди, передав ей аргументы
  void (*pullVar)(struct fParams *);
  struct fParams * Params;
  if (FP_last == FP_first)return;
  Params = &PARAMS_array[FP_first];
  pullVar = FP_Queue[FP_first++];
  FP_first %= Q_SIZE_FAST;
  pullVar(Params);
}

FPQueueIni();
while(1){
FPpull();
}

FP_push(ApmControl,&(struct fParams){1,7.18}); //добавляем функцию AmpContol                                                          //  и передаём параметры 1 и 7.18

void SIM800_IniCMD(void) {
__HAL UART_ENABLE_IT(shuart2, UART_IT_IDLE); //разрешаем IDLE
__HAL UART_ENABLE_IT(shuart2, UART_IT_RXNE); //разрешаем IDLE
ResParse.bytes = 3; //сбрасываем прочитанные лог. уровни
Delay_ms_OnMediumQ(32); // ожидаем на "средней" очереди заданий 32 мс
SIM800_AddCMD((char *) GSM_ATcmd, sizeof (GSM_ATcmd)); // команда для SIM800 через DMA
Delay_ms_OnMediumQ(4000); // ожидаем на "средней" очереди заданий 4c
SIM800_AddCMD((char *) GSM_ATcmd_Disable_Echo, sizeof (GSM_ATcmd_Disable_Echo));
...

void blink(int count) { //поморгать LED
  while (count--) {
    HAL _GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET); // пин в лог.1
    HAL Delay(100);// задержка 100 мс.
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET);// пин в лог.0
  }
}

void blink(int count) { //поморгать LED
  while (count--) {
    HAL _GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_SET); // пин в лог.1
    Delay_ms_OnMediumQ(100);   // задержка 100 мс. выполняя задания
                              //из "средней" очереди
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1, GPIO_PIN_RESET); // пин в лог.0
  }
}

while(SignalNotStable);

while(SignalNotStable){
  S1_pull()(); //исполняем задания из S1 пока ждём
}

typedef void(*fP)(void);
class fQ {
private:
    int first;
    int last;
    fP * fQueue;
    int lengthQ;
public:
    fQ(int sizeQ);
    ~fQ();
    int push(fP);    // добавить процедуру в очередь
    int pull(void);  // взять и выполнить процедуру из очереди
};
fQ::fQ(int sizeQ){ // инициализация очереди
  fQueue = new fP[sizeQ];
  last = 0;
  first = 0;
  lengthQ = sizeQ;
}
fQ::~fQ(){ // деинициализация очереди
  delete [] fQueue;
}
int fQ::push(fP pointerF){ // добавить элемент в очередь
  if ((last+1)%lengthQ == first){
            return 1;
  }
  fQueue[last++] = pointerF;
  last = last%lengthQ;
  return 0;
}
int fQ::pull(void){ // взять элемент из очереди
  if (last != first){
  fQueue[first++]();
  first = first%lengthQ;
  return 0;
  }
  else{
   return 1;
  }
}

fQ F1(16); //инициализируем очереди различной длины
fQ F2(12);
fQ A1(8);
int main(){
  for(;;){
   A1.pull();
   usleep(10000); //спим по 10 миллисеунд
         }
return 0;
}

if(pin10.in == 1) F1.push(SwithOnRelay);
else F1.push(SwithOffRelay);

F2.push(ToggleLED);

A1.push(UpdateUI);

void UpdateUI(void){
  pin_CS_DISP.out = 0;
  Delay_ms_OnF1(2); //задержка на 2 мс. на очереди - исполняем задания из очереди F1
  DispLCD(Voltage);
  Delay_ms_OnF2(2); //задержка на 2 мс. на очереди - исполняем задания из очереди F2
  pin_CS_DISP.out = 1;
}