From 342d0a54255da21c6c8ced70ae51b5382a20d984 Mon Sep 17 00:00:00 2001
From: cfif <cham-zod@mail.ru>
Date: Wed, 24 Dec 2025 13:08:09 +0300
Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=D0=B2=D0=BB=D0=B5?=
 =?UTF-8?q?=D0=BD=D0=B8=D0=B5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 MainModesArbiter.c           | 328 +++++++++++++++++------------------
 MainModesArbiter_InitStage.c |   2 +-
 2 files changed, 165 insertions(+), 165 deletions(-)

diff --git a/MainModesArbiter.c b/MainModesArbiter.c
index 197c35d..86e8677 100644
--- a/MainModesArbiter.c
+++ b/MainModesArbiter.c
@@ -8,7 +8,9 @@
 
 #include "ADC_Temp_Table.h"
 
+
 const char LOG_TASK_ARB[] = "Arb";
+#define LOGGER &env->slog.logger
 
 void Mma_Init(
         tMma *env,
@@ -47,27 +49,30 @@ static _Noreturn void Mma_Thread(tMma *env) {
     int16_t temp2 = 0;
 
     if (RGM_SRS_WAKEUP_MASK == (RGM->SRS & RGM_SRS_WAKEUP_MASK)) {
-        LoggerInfoStatic(&env->slog.logger, LOG_TASK_ARB, "Wake up from standby")
+        LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "Wake up from standby")
     }
 
-//    SystemDelayMs(1000);
+    SystemDelayMs(1000);
 
-//    ModelTask_StartThread(&env->ModelTask);
+    ModelTask_StartThread(&env->ModelTask);
 
     bool isActuatorWorkBusy = false;
 
 //    env->pwms->pwmIo.run(env->pwms->pwmIo.env);
 
+    bool busy = true;
+
     for (;;) {
 
 
 
 
 
-/*
+
         if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
 
-            if (env->linTaskActuator0.busy == 0) {
+            // Если актуатор не занят
+            if (env->linTaskActuator0.busy == false) {
 
                 for (uint8_t i = 0; i < LIN0_ISSR_ALL; ++i) {
 
@@ -75,7 +80,10 @@ static _Noreturn void Mma_Thread(tMma *env) {
                     env->linTaskActuator0.linCommandActuator[i].BUS_ADR = env->rtY_local.Out1.BUS_ADR[i];
                     env->linTaskActuator0.linCommandActuator[i].MODE = env->rtY_local.Out1.MODE[i];
 
+                    // Если команда выполнена
                     if (env->linTaskActuator0.linCommandActuator[i].COM == LIN_ACT_CFR_SUCCESSFUL) {
+                        LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "SUCCESSFUL COMMAND")
+                        // Установка входной команды в LIN_ACT_CFR_NONE
                         env->rtY_local.Out1.COM[i] = LIN_ACT_CFR_NONE;
                     }
 
@@ -86,19 +94,18 @@ static _Noreturn void Mma_Thread(tMma *env) {
                     env->linTaskActuator0.linCommandActuator[i].Speed_SET = env->rtY_local.Out1.Speed_SET[i];
                     env->linTaskActuator0.linCommandActuator[i].Coils_Stop_SET = env->rtY_local.Out1.Coils_Stop_SET[i];
 
-                    env->rtU_local.in_CPOS_ALL_Ch0[i] = env->linTaskActuator0.linStateActuator[i].CPOS_ALL;
-                    env->rtU_local.in_Act_Emrf_Slave_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Emrf_Slave;
-                    env->rtU_local.in_Mode_Slave_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Mode_Slave;
-                    env->rtU_local.in_Act_Err1_Supply_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error1_Supply_Slave;
-                    env->rtU_local.in_Act_Err2_Communication_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error2_Communication_Slave;
-                    env->rtU_local.in_Act_Err3_Temperature_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error3_Temperature_Slave;
-                    env->rtU_local.in_Act_Err4_Permanent_Electrical_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Error4_Permanent_Electrical_Slave;
+                    env->rtU_local.in_CPOS_ALL_Ch0[i] = (int16_t)env->linTaskActuator0.linStateActuator[i].CPOS_ALL;
+                    env->rtU_local.in_Act_Emrf_Slave_Ch0[i] = (int8_t)env->linTaskActuator0.linStateActuator[i].Emrf_Slave;
+                    env->rtU_local.in_Mode_Slave_Ch0[i] = (int8_t)env->linTaskActuator0.linStateActuator[i].Mode_Slave;
+                    env->rtU_local.in_Act_Err1_Supply_Ch0[i] = (int8_t)env->linTaskActuator0.linStateActuator[i].Error1_Supply_Slave;
+                    env->rtU_local.in_Act_Err2_Communication_Ch0[i] = (int8_t)env->linTaskActuator0.linStateActuator[i].Error2_Communication_Slave;
+                    env->rtU_local.in_Act_Err3_Temperature_Ch0[i] = (int8_t)env->linTaskActuator0.linStateActuator[i].Error3_Temperature_Slave;
+                    env->rtU_local.in_Act_Err4_Permanent_Electrical_Ch0[i] = (int8_t)env->linTaskActuator0.linStateActuator[i].Error4_Permanent_Electrical_Slave;
                     env->rtU_local.in_Act_Stall_Slave_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Stall_Slave;
                     env->rtU_local.in_Act_Reset_Ch0[i] = env->linTaskActuator0.linStateActuator[i].Reset_Slave;
                 }
 
                 isActuatorWorkBusy = setBusy(&env->linTaskActuator0);
-                resetStall(&env->linTaskActuator0);
 
                 env->rtU_local.in_Error_Connect_Ch0 = env->linTaskActuator0.error_connect;
                 env->rtU_local.in_Busy_Ch0 = env->linTaskActuator0.busy;
@@ -110,28 +117,38 @@ static _Noreturn void Mma_Thread(tMma *env) {
 
         if (osMutexAcquire(env->ModelTask.access, 5000) == osOK) {
 
+            // Если принята команда
             if (env->ModelTask.triggerCommand == true) {
+                LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "DETECT COMMAND")
+                //
                 env->rtU_local.in_Busy_Ch0 = 1;
             }
 
-            memcpy(&rtU, &env->rtU_local, sizeof(ExtU));
-
-
+            // Если прията команда и актуатор не занят
             if ((env->ModelTask.triggerCommand == true) && (isActuatorWorkBusy == false)) {
+
                 env->ModelTask.triggerCommand = false;
                 isActuatorWorkBusy = true;
 
+                // Сброс STALL в состоянии актуатор и локальном состоянии
+                resetStall(&env->linTaskActuator0, (uint8_t *)env->rtU_local.in_Act_Stall_Slave_Ch0);
+
+                // Копирование команд
                 for (uint8_t i = 0; i < 9; ++i) {
                     rtY.Out1.COM[i] = env->ModelTask.numCommand[i];
                 }
 
+                // ВЫХОД МОДЕЛИ (АКТУАТОР)
                 memcpy(&env->rtY_local.Out1, &rtY.Out1, sizeof(rtY.Out1));
             }
 
+            // ВХОДЫ МОДЕЛИ
+            memcpy(&rtU, &env->rtU_local, sizeof(ExtU));
+
             osMutexRelease(env->ModelTask.access);
         }
 
-*/
+
 
 
 
@@ -146,7 +163,7 @@ static _Noreturn void Mma_Thread(tMma *env) {
 
 
 /*
-        LoggerInfoStatic(&env->slog.logger, LOG_TASK_ARB, "Zorro...")
+        LoggerInfoStatic(LOGGER, LOG_TASK_ARB, "Zorro...")
         printf("Test Test Test Test Test Test Test Test Test Test XA XA ...\n");
 
         GpioPinToggle(&env->gpios->led.LED1);
@@ -165,169 +182,152 @@ static _Noreturn void Mma_Thread(tMma *env) {
         if (osMutexAcquire(env->adcTask0.access, 1000) == osOK) {
             temp1 = get_temperature_fast(env->adcTask0.ADC_Data[0], fast_lookup_Incar, 512);
             osMutexRelease(env->adcTask0.access);
-            LoggerFormatInfo(&env->slog.logger, LOG_TASK_ARB, "Temp1 = %d", temp1)
+            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Temp1 = %d", temp1)
         }
 
 
         if (osMutexAcquire(env->adcTask1.access, 1000) == osOK) {
             temp2 = get_temperature_fast(env->adcTask1.ADC_Data[0], fast_lookup_KST45, 512);
             osMutexRelease(env->adcTask1.access);
-            LoggerFormatInfo(&env->slog.logger, LOG_TASK_ARB, "Temp2 = %d", temp2)
+            LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Temp2 = %d", temp2)
         }
 */
 
-
 /*
-        bool isBusy = true;
         if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-            if (env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_NONE) {
-                isBusy = false;
+
+            if ((env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_SUCCESSFUL) ||
+                (env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_NONE)) {
+                busy = true;
+            }
+
+            if (busy == true) {
+                busy = false;
+
+                switch (step) {
+                    case 0: {
+
+                        resetStall(&env->linTaskActuator0);
+
+                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
+                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
+                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_STOP;
+                        ++step;
+
+                        break;
+                    }
+                    case 1: {
+
+                        resetStall(&env->linTaskActuator0);
+
+                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
+                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
+                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
+                        ++step;
+
+                        break;
+                    }
+                    case 2: {
+
+                        resetStall(&env->linTaskActuator0);
+
+                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
+                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
+                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_NORMAL;
+                        ++step;
+
+                        break;
+                    }
+                    case 3: {
+
+                        resetStall(&env->linTaskActuator0);
+
+                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
+                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
+                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
+                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
+                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
+                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
+                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
+                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
+
+                        ++step;
+
+                        break;
+                    }
+
+                    case 4: {
+                        SystemDelayMs(10000);
+                        asm("nop");
+                        ++step;
+                        //if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL == 0) {
+                        //    ++step;
+                        //}
+                        break;
+                    }
+
+                    case 5: {
+
+                        resetStall(&env->linTaskActuator0);
+
+                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
+                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
+                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
+                        ++step;
+
+                        break;
+                    }
+
+                    case 6: {
+
+                        resetStall(&env->linTaskActuator0);
+
+                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
+                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
+                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
+                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
+                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
+                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
+                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
+                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
+
+                        ++step;
+
+                        break;
+                    }
+
+                    case 7: {
+                        SystemDelayMs(10000);
+                        asm("nop");
+                        ++step;
+
+                        //if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL >= 6000) {
+                        //    ++step;
+                        //}
+                        break;
+                    }
+
+                    case 8: {
+                        if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL >= 8000) {
+                            ++step;
+                        }
+                        break;
+                    }
+
+                    default: {
+                    }
+                }
             }
             osMutexRelease(env->linTaskActuator0.access);
         }
 
-        if (isBusy == false) {
-
-            switch (step) {
-                case 0: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_STOP;
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-                case 1: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-                case 2: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_NORMAL;
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-                case 3: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
-                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
-                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
-                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
-                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
-                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
-
-                        resetStall(&env->linTaskActuator0);
-
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-
-                case 4: {
-                    SystemDelayMs(10000);
-                    asm("nop");
-                    ++step;
-                    //if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL == 0) {
-                    //    ++step;
-                    //}
-                    break;
-                }
-
-                case 5: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].POS = 0;
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-
-                case 6: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].POS = 6000;
-                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
-                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
-                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
-                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
-                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
-
-                        resetStall(&env->linTaskActuator0);
-
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-
-                case 7: {
-                    SystemDelayMs(10000);
-                    asm("nop");
-                    ++step;
-
-                    //if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL >= 6000) {
-                    //    ++step;
-                    //}
-                    break;
-                }
-
-                case 8: {
-                    if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
-
-                        env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
-                        env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
-                        env->linTaskActuator0.linCommandActuator[0].POS = 8000;
-                        env->linTaskActuator0.linCommandActuator[0].Stall_SET = 1;
-                        env->linTaskActuator0.linCommandActuator[0].Lnoise_SET = 0;
-                        env->linTaskActuator0.linCommandActuator[0].Autos_SET = 1;
-                        env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
-                        env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
-
-                        resetStall(&env->linTaskActuator0);
-
-                        ++step;
-                        osMutexRelease(env->linTaskActuator0.access);
-                    }
-                    break;
-                }
-
-                case 9: {
-                    if (env->linTaskActuator0.linStateActuator[7].CPOS_ALL >= 8000) {
-                        ++step;
-                    }
-                    break;
-                }
-
-                default: {
-                }
-            }
-
-
-        }
+        LoggerFormatInfo(LOGGER, LOG_TASK_ARB, "Step = %d", step)
 */
 
 
+
+
+
 /*
         uint16_t len = env->canPorts->Can0_IO.receive(env->canPorts->Can0_IO.env, 0, (uint8_t *)&frame_data, 1, 1000);
 
@@ -346,7 +346,7 @@ static _Noreturn void Mma_Thread(tMma *env) {
         }
 */
 
-        SystemDelayMs(100);
+        SystemDelayMs(10);
     }
 }
 
diff --git a/MainModesArbiter_InitStage.c b/MainModesArbiter_InitStage.c
index a5217e8..f6f9e87 100644
--- a/MainModesArbiter_InitStage.c
+++ b/MainModesArbiter_InitStage.c
@@ -92,7 +92,7 @@ static void Mma_InitSubSystems(tMma *env) {
 
     StandBy_Init(&env->standBy);
 
-    ModelTask_Init(&env->ModelTask);
+    ModelTask_Init(&env->ModelTask, &env->slog.logger);
 
     LoggerInfoStatic(&env->slog.logger, LOG_TASK_MAIN, "End of subsystem initialization")