diff --git a/MainModesArbiter.c b/MainModesArbiter.c index 191e2ad..0207e27 100644 --- a/MainModesArbiter.c +++ b/MainModesArbiter.c @@ -40,11 +40,6 @@ static bool isActuatorCommandSet_in_Model() { 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) { @@ -78,6 +73,11 @@ static _Noreturn void Mma_Thread(tMma *env) { 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]; + + if (env->linTaskActuator0.linCommandActuator[i].COM == LIN_ACT_CFR_SUCCESSFUL) { + env->rtY_local.Out1.COM[i] = LIN_ACT_CFR_NONE; + } + 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]; @@ -109,10 +109,14 @@ static _Noreturn void Mma_Thread(tMma *env) { if (osMutexAcquire(env->ModelTask.access, 5000) == osOK) { + if (env->ModelTask.isActuatorOverrideBusy == true) { + env->rtU_local.in_Busy_Ch0 = 1; + } + 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)); }