182 lines
8.2 KiB
C
182 lines
8.2 KiB
C
//
|
||
// Created by cfif on 20.01.2026.
|
||
//
|
||
#include "LinActuatorWork.h"
|
||
#include "string.h"
|
||
|
||
|
||
//#define LOG_SIGN "Lin"
|
||
#define LOGGER linTaskActuator->logger
|
||
|
||
static bool isBroadCastTriggered(tLinTaskActuator *env, ActuatorCmdBus *actuator_Command_Model_trigger_local) {
|
||
|
||
for (uint8_t j = 0; j < env->LIN_ISSR_ALL; ++j) {
|
||
if (actuator_Command_Model_trigger_local->BUS_ADR[j] != 0) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
static void showLogCommand(tLinTaskActuator *linTaskActuator, char *LOG_SIGN, uint8_t BUS_ADR, uint8_t COM) {
|
||
switch (COM) {
|
||
case LIN_ACT_CFR_NONE: {
|
||
LoggerFormatTrace(LOGGER, LOG_SIGN, "BUS_ADR: %d - NO COMMAND", BUS_ADR)
|
||
break;
|
||
}
|
||
|
||
case LIN_ACT_CFR_MOD: {
|
||
LoggerFormatTrace(LOGGER, LOG_SIGN, "BUS_ADR: %d - LIN_ACT_CFR_MOD", BUS_ADR)
|
||
break;
|
||
}
|
||
|
||
case LIN_ACT_CFR_INI: {
|
||
LoggerFormatTrace(LOGGER, LOG_SIGN, "BUS_ADR: %d - LIN_ACT_CFR_INI", BUS_ADR)
|
||
break;
|
||
}
|
||
|
||
case LIN_ACT_CFR_SET: {
|
||
LoggerFormatTrace(LOGGER, LOG_SIGN, "BUS_ADR: %d - LIN_ACT_CFR_SET", BUS_ADR)
|
||
break;
|
||
}
|
||
|
||
default: {
|
||
LoggerFormatError(LOGGER, LOG_SIGN, "BUS_ADR: %d - UNKNOWN COMMAND", BUS_ADR)
|
||
break;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
#if (LOG_LIN_ACTUATOR == 1)
|
||
#define IS_LOGGING_ENABLED(numAct) \
|
||
( (numAct == 1 && LOG_LIN1_ACTUATOR == 1) || \
|
||
(numAct == 2 && LOG_LIN2_ACTUATOR == 1) || \
|
||
(numAct == 3 && LOG_LIN3_ACTUATOR == 1) )
|
||
#endif
|
||
|
||
// actuator_Command_Model_local - Выход модели (команды для актуаторов) ЛОКАЛЬНО
|
||
// actuator_Command_Model_trigger_local - Выход модели (команды для актуаторов) ЗАХВАТ
|
||
// actuator_Output_Model_local - Вход модели (состояния актуаторов) ЛОКАЛЬНЫЙ
|
||
// actuator_Output_Model_model - Вход модели (состояния актуаторов) МОДЕЛИ
|
||
|
||
void LinActuatorWork(tLinTaskActuator *linTaskActuator, osMutexId_t modelTaskAccess, uint8_t numAct,
|
||
ActuatorCmdBus *actuator_Command_Model_local,
|
||
ActuatorCmdBus *actuator_Command_Model_trigger_local,
|
||
ActuatorCmdBusInput *actuator_Output_Model_local,
|
||
ActuatorCmdBusInput *actuator_Output_Model_model,
|
||
bool *triggerCommand,
|
||
char *LOG_SIGN) {
|
||
|
||
|
||
for (uint8_t i = 0; i < linTaskActuator->LIN_ISSR_ALL; ++i) {
|
||
|
||
linTaskActuator->linCommandActuator[i].POS = actuator_Command_Model_local->POS[i];
|
||
linTaskActuator->linCommandActuator[i].BUS_ADR = actuator_Command_Model_local->BUS_ADR[i];
|
||
linTaskActuator->linCommandActuator[i].MODE = actuator_Command_Model_local->MODE[i];
|
||
|
||
// Если команда выполнена
|
||
if (linTaskActuator->linCommandActuator[i].COM == LIN_ACT_CFR_SUCCESSFUL) {
|
||
#if (LOG_LIN_ACTUATOR == 1)
|
||
if (IS_LOGGING_ENABLED(numAct)) {
|
||
LoggerFormatTrace(LOGGER, LOG_SIGN, "BUS_ADR: %d - SUCCESSFUL COMMAND (UNSET BUSY)", i + 1)
|
||
}
|
||
#endif
|
||
// LoggerFormatTrace(LOGGER, LOG_SIGN, "ACTUATOR %d - SUCCESSFUL COMMAND (UNSET BUSY)", numAct)
|
||
|
||
actuator_Output_Model_local->Busy = 0;
|
||
// Разрешение обработки новой команды
|
||
//env->isActuatorNoGetNextCommand[numAct - 1] = false;
|
||
|
||
// Обнуляем команду в локальном буфере
|
||
actuator_Command_Model_local->COM[i] = LIN_ACT_CFR_NONE;
|
||
}
|
||
|
||
// ТОЛЬКО если команда не SUCCESSFUL, копируем новые данные
|
||
linTaskActuator->linCommandActuator[i].COM = actuator_Command_Model_local->COM[i];
|
||
linTaskActuator->linCommandActuator[i].Stall_SET = actuator_Command_Model_local->Stall_SET[i];
|
||
linTaskActuator->linCommandActuator[i].Lnoise_SET = actuator_Command_Model_local->Lnoise_SET[i];
|
||
linTaskActuator->linCommandActuator[i].Autos_SET = actuator_Command_Model_local->Autos_SET[i];
|
||
linTaskActuator->linCommandActuator[i].Speed_SET = actuator_Command_Model_local->Speed_SET[i];
|
||
linTaskActuator->linCommandActuator[i].Coils_Stop_SET = actuator_Command_Model_local->Coils_Stop_SET[i];
|
||
|
||
// Заполнение данных ВХОДЫ МОДЕЛИ
|
||
actuator_Output_Model_local->in_CPOS_ALL[i] = (int16_t) linTaskActuator->linStateActuator[i].CPOS_ALL;
|
||
actuator_Output_Model_local->in_Act_Emrf_Slave[i] = (int8_t) linTaskActuator->linStateActuator[i].Emrf_Slave;
|
||
actuator_Output_Model_local->in_Mode_Slave[i] = (int8_t) linTaskActuator->linStateActuator[i].Mode_Slave;
|
||
actuator_Output_Model_local->in_Act_Err1_Supply[i] = (int8_t) linTaskActuator->linStateActuator[i].Error1_Supply_Slave;
|
||
actuator_Output_Model_local->in_Act_Err2_Communication[i] = (int8_t) linTaskActuator->linStateActuator[i].Error2_Communication_Slave;
|
||
actuator_Output_Model_local->in_Act_Err3_Temperature[i] = (int8_t) linTaskActuator->linStateActuator[i].Error3_Temperature_Slave;
|
||
actuator_Output_Model_local->in_Act_Err4_Permanent_Electrical[i] = (int8_t) linTaskActuator->linStateActuator[i].Error4_Permanent_Electrical_Slave;
|
||
actuator_Output_Model_local->in_Act_Stall_Slave[i] = linTaskActuator->linStateActuator[i].Stall_Slave;
|
||
actuator_Output_Model_local->in_Act_Reset[i] = linTaskActuator->linStateActuator[i].Reset_Slave;
|
||
}
|
||
|
||
// Заполнение данных ВХОДЫ МОДЕЛИ
|
||
actuator_Output_Model_local->Error_Connect = linTaskActuator->error_connect;
|
||
|
||
// LoggerFormatTrace(LOGGER, LOG_SIGN, "Busy = %d Error_Connect = %d", actuator_Output_Model_local->Busy, actuator_Output_Model_local->Error_Connect)
|
||
|
||
|
||
|
||
|
||
|
||
if (osMutexAcquire(modelTaskAccess, 5000) == osOK) {
|
||
|
||
// Если прията команда и актуатор не занят (может принимать команды)
|
||
// if ((*triggerCommand == true) && (env->isActuatorNoGetNextCommand[numAct - 1] == false)) {
|
||
if (*triggerCommand == true) {
|
||
|
||
*triggerCommand = false;
|
||
|
||
#if (LOG_LIN_ACTUATOR == 1)
|
||
if (IS_LOGGING_ENABLED(numAct)) {
|
||
if (isBroadCastTriggered(linTaskActuator, actuator_Command_Model_trigger_local)) {
|
||
LoggerTraceStatic(LOGGER, LOG_SIGN, "DETECT COMMAND (BROADCAST):")
|
||
showLogCommand(linTaskActuator, LOG_SIGN, actuator_Command_Model_trigger_local->BUS_ADR[0],
|
||
actuator_Command_Model_trigger_local->COM[0]);
|
||
} else {
|
||
LoggerTraceStatic(LOGGER, LOG_SIGN, "DETECT COMMAND (NO BROADCAST):")
|
||
for (uint8_t i = 0; i < linTaskActuator->LIN_ISSR_ALL; ++i) {
|
||
showLogCommand(linTaskActuator, LOG_SIGN, actuator_Command_Model_trigger_local->BUS_ADR[i],
|
||
actuator_Command_Model_trigger_local->COM[i]);
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
//
|
||
actuator_Output_Model_local->Busy = 1;
|
||
|
||
// Запрещение обработки новой команды
|
||
//env->isActuatorNoGetNextCommand[numAct - 1] = true;
|
||
|
||
// начало --- ВЫХОД МОДЕЛИ ----------
|
||
|
||
// ВЫХОД МОДЕЛИ
|
||
memcpy(actuator_Command_Model_local, actuator_Command_Model_trigger_local, sizeof(ActuatorCmdBus));
|
||
|
||
// Сброс STALL в состоянии актуатор (локальном состоянии)
|
||
|
||
#if (LOG_LIN_ACTUATOR == 1)
|
||
if (IS_LOGGING_ENABLED(numAct)) {
|
||
LoggerTraceStatic(LOGGER, linTaskActuator->SIGN_LOG, "Reset LOCAL STALL (DETECT STALL RESET)")
|
||
}
|
||
#endif
|
||
for (uint8_t i = 0; i < linTaskActuator->LIN_ISSR_ALL; ++i) {
|
||
actuator_Output_Model_local->in_Act_Stall_Slave[i] = LIN_STALL_STA_OFF;
|
||
}
|
||
|
||
// конец --- ВЫХОД МОДЕЛИ ----------
|
||
|
||
}
|
||
|
||
// ВХОДЫ МОДЕЛИ
|
||
memcpy(actuator_Output_Model_model, actuator_Output_Model_local, sizeof(ActuatorCmdBusInput));
|
||
|
||
osMutexRelease(modelTaskAccess);
|
||
}
|
||
|
||
|
||
}
|