70 lines
1.7 KiB
C
70 lines
1.7 KiB
C
//
|
|
// Created by cfif on 19.12.2025.
|
|
//
|
|
#include "Model_Task.h"
|
|
#include <SystemDelayInterface.h>
|
|
#include <CmsisRtosThreadUtils.h>
|
|
#include "Model_actuator.h"
|
|
|
|
#define LOG_SIGN "Model"
|
|
#define LOGGER env->logger
|
|
|
|
void ModelTask_Init(
|
|
tModelTask *env,
|
|
tLoggerInterface *logger
|
|
) {
|
|
Model_actuator_initialize();
|
|
env->access = osMutexNew(NULL);
|
|
env->logger = logger;
|
|
InitThreadAtrStatic(&env->thread.attr, "ModelTask", env->thread.controlBlock, env->thread.stack, osPriorityNormal);
|
|
env->thread.id = 0;
|
|
}
|
|
|
|
static bool setActuatorBusy(tModelTask *env) {
|
|
|
|
for (uint8_t i = 0; i < 9; ++i) {
|
|
if (rtY.Out1.COM[i] != 0) {
|
|
|
|
LoggerInfoStatic(LOGGER, LOG_SIGN, "DETECT COMMAND")
|
|
|
|
env->triggerCommand = true;
|
|
|
|
for (uint8_t j = 0; j < 9; ++j) {
|
|
env->numCommand[j] = rtY.Out1.COM[j];
|
|
}
|
|
|
|
rtU.in_Busy_Ch0 = 1;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
static _Noreturn void ModelTask_Thread(tModelTask *env) {
|
|
for (;;) {
|
|
if (osMutexAcquire(env->access, 1000) == osOK) {
|
|
|
|
LoggerFormatInfo(LOGGER, LOG_SIGN, "in_Busy_Ch0 = %d in_Act_Stall_Slave_Ch0 = %d", rtU.in_Busy_Ch0, rtU.in_Act_Stall_Slave_Ch0[7])
|
|
|
|
Model_actuator_step();
|
|
setActuatorBusy(env);
|
|
osMutexRelease(env->access);
|
|
}
|
|
SystemDelayMs(250);
|
|
}
|
|
}
|
|
|
|
void ModelTask_StartThread(tModelTask *env) {
|
|
if (!env->thread.id) {
|
|
env->thread.id = osThreadNew((osThreadFunc_t) (ModelTask_Thread), (void *) (env), &env->thread.attr);
|
|
} else {
|
|
osThreadResume(env->thread.id);
|
|
}
|
|
}
|
|
|
|
void ModelTask_StopThread(tModelTask *env) {
|
|
if (env->thread.id) {
|
|
osThreadSuspend(env->thread.id);
|
|
}
|
|
} |