From 2291e38e570a9f5a379aacd6767ec9a39f11ee4f Mon Sep 17 00:00:00 2001
From: cfif <cham-zod@mail.ru>
Date: Fri, 19 Dec 2025 16:07:24 +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           | 69 ++++++++++++++++++++++++++++++++++++
 MainModesArbiter.h           |  7 +++-
 MainModesArbiter_InitStage.c |  2 ++
 3 files changed, 77 insertions(+), 1 deletion(-)

diff --git a/MainModesArbiter.c b/MainModesArbiter.c
index fb0e43e..191e2ad 100644
--- a/MainModesArbiter.c
+++ b/MainModesArbiter.c
@@ -4,6 +4,7 @@
 #include "MainModesArbiter_Private.h"
 #include "stdio.h"
 #include "fc7xxx_driver_rgm.h"
+#include "Model_actuator.h"
 
 #include "ADC_Temp_Table.h"
 
@@ -31,6 +32,20 @@ void Mma_Init(
     env->thread.id = 0;
 }
 
+static bool isActuatorCommandSet_in_Model() {
+    for (uint8_t i = 0; i < LIN0_ISSR_ALL; ++i) {
+        if (rtY.Out1.COM[i] != 0)
+            return true;
+    }
+    return false;
+}
+
+static void clearActuatorCommand_in_Model() {
+    for (uint8_t i = 0; i < LIN0_ISSR_ALL; ++i) {
+        rtY.Out1.COM[i] = 0;
+    }
+}
+
 static _Noreturn void Mma_Thread(tMma *env) {
 
     // Запуск устройства
@@ -46,12 +61,66 @@ static _Noreturn void Mma_Thread(tMma *env) {
         LoggerInfoStatic(&env->slog.logger, LOG_TASK_ARB, "Wake up from standby")
     }
 
+    SystemDelayMs(1000);
+
+    ModelTask_StartThread(&env->ModelTask);
+
+    bool isActuatorBusy = false;
+
     for (;;) {
 
+        if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
+
+            if (env->linTaskActuator0.busy == 0) {
+
+                for (uint8_t i = 0; i < LIN0_ISSR_ALL; ++i) {
+
+                    env->linTaskActuator0.linCommandActuator[i].POS = env->rtY_local.Out1.POS[i];
+                    env->linTaskActuator0.linCommandActuator[i].BUS_ADR = env->rtY_local.Out1.BUS_ADR[i];
+                    env->linTaskActuator0.linCommandActuator[i].MODE = env->rtY_local.Out1.MODE[i];
+                    env->linTaskActuator0.linCommandActuator[i].COM = env->rtY_local.Out1.COM[i];
+                    env->linTaskActuator0.linCommandActuator[i].Stall_SET = env->rtY_local.Out1.Stall_SET[i];
+                    env->linTaskActuator0.linCommandActuator[i].Lnoise_SET = env->rtY_local.Out1.Lnoise_SET[i];
+                    env->linTaskActuator0.linCommandActuator[i].Autos_SET = env->rtY_local.Out1.Autos_SET[i];
+                    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_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;
+                }
+
+                isActuatorBusy = 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;
+            }
+
+            osMutexRelease(env->linTaskActuator0.access);
+        }
 
 
+        if (osMutexAcquire(env->ModelTask.access, 5000) == osOK) {
 
+            memcpy(&rtU, &env->rtU_local, sizeof(ExtU));
 
+            if ((isActuatorCommandSet_in_Model() == true) && (isActuatorBusy == false)) {
+                clearActuatorCommand_in_Model();
+                isActuatorBusy = true;
+                memcpy(&env->rtY_local.Out1, &rtY.Out1, sizeof(rtY.Out1));
+            }
+
+            osMutexRelease(env->ModelTask.access);
+        }
+
+        SystemDelayMs(100);
 
 
 /*
diff --git a/MainModesArbiter.h b/MainModesArbiter.h
index 5e01d66..105b1c5 100644
--- a/MainModesArbiter.h
+++ b/MainModesArbiter.h
@@ -25,7 +25,8 @@
 #include "LinTasks.h"
 #include "AdcTasks.h"
 #include "standby.h"
-
+#include "Model_Task.h"
+#include "Model_actuator.h"
 
 
 typedef struct {
@@ -60,6 +61,8 @@ typedef struct {
 
     tStandBy standBy;
 
+    tModelTask ModelTask;
+
     struct {
         osThreadId_t id;
         uint32_t stack[1024 * 1];
@@ -67,6 +70,8 @@ typedef struct {
         osThreadAttr_t attr;
     } thread;
 
+    ExtU rtU_local;
+    ExtY rtY_local;
 } tMma;
 
 void Mma_Init(
diff --git a/MainModesArbiter_InitStage.c b/MainModesArbiter_InitStage.c
index 557bdf6..5b878d4 100644
--- a/MainModesArbiter_InitStage.c
+++ b/MainModesArbiter_InitStage.c
@@ -89,6 +89,8 @@ static void Mma_InitSubSystems(tMma *env) {
 
     StandBy_Init(&env->standBy);
 
+    ModelTask_Init(&env->ModelTask);
+
     LoggerInfoStatic(&env->slog.logger, LOG_TASK_MAIN, "End of subsystem initialization")
 
 }