Обновление
This commit is contained in:
cfif
parent
f87be212d5
commit
342d0a5425
|
|
@ -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,65 +182,68 @@ 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;
|
||||
}
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
|
||||
if ((env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_SUCCESSFUL) ||
|
||||
(env->linTaskActuator0.linCommandActuator[0].COM == LIN_ACT_CFR_NONE)) {
|
||||
busy = true;
|
||||
}
|
||||
|
||||
if (isBusy == false) {
|
||||
if (busy == true) {
|
||||
busy = false;
|
||||
|
||||
switch (step) {
|
||||
case 0: {
|
||||
if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
|
||||
env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_STOP;
|
||||
++step;
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 1: {
|
||||
if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
|
||||
env->linTaskActuator0.linCommandActuator[0].POS = 6000;
|
||||
++step;
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 2: {
|
||||
if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_MOD;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
|
||||
env->linTaskActuator0.linCommandActuator[0].MODE = LIN_MODE_NORMAL;
|
||||
++step;
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case 3: {
|
||||
if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
|
||||
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;
|
||||
|
|
@ -231,11 +251,8 @@ static _Noreturn void Mma_Thread(tMma *env) {
|
|||
env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
|
||||
env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
++step;
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -250,22 +267,23 @@ static _Noreturn void Mma_Thread(tMma *env) {
|
|||
}
|
||||
|
||||
case 5: {
|
||||
if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_INI;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0;
|
||||
env->linTaskActuator0.linCommandActuator[0].POS = 0;
|
||||
++step;
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case 6: {
|
||||
if (osMutexAcquire(env->linTaskActuator0.access, 5000) == osOK) {
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
env->linTaskActuator0.linCommandActuator[0].COM = LIN_ACT_CFR_SET;
|
||||
env->linTaskActuator0.linCommandActuator[0].BUS_ADR = 0x20;
|
||||
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;
|
||||
|
|
@ -273,11 +291,8 @@ static _Noreturn void Mma_Thread(tMma *env) {
|
|||
env->linTaskActuator0.linCommandActuator[0].Speed_SET = 3;
|
||||
env->linTaskActuator0.linCommandActuator[0].Coils_Stop_SET = 0;
|
||||
|
||||
resetStall(&env->linTaskActuator0);
|
||||
|
||||
++step;
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -293,26 +308,6 @@ static _Noreturn void Mma_Thread(tMma *env) {
|
|||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -322,12 +317,17 @@ static _Noreturn void Mma_Thread(tMma *env) {
|
|||
default: {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
osMutexRelease(env->linTaskActuator0.access);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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")
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue