void runPID(void *Parameters)
{
float currError = 0, prevError = 0, errorSum = 0, errorDiff = 0,
outD = 0, outP = 0, outI = 0, output = 0;
int delayTicks;
struct PIDParameters * params = Parameters;
CoWaitForSingleFlag(params->InitCompleteFlagID, 0);
while (1)
{
if (RegisterMap[REGISTER_CONTROL_MODE].Bits & CONTROL_MODE_AUTO_MODE && RegisterMap[REGISTER_CONTROL_MODE].Bits & params->ControlBitMask)
{
CoPendSem(params->ThisPIDSemID, 0);
if(!params->IsTopLevelUnit)
CoPendSem(params->PrevPIDSemID, 0);
CoPendSem(params->DataSemID, 0);
currError = *(params->SetPoint) - *(params->ProcessVariable);
errorSum += currError;
errorDiff = currError - prevError;
prevError = currError;
outD = *(params->KD) * errorDiff;
outI = *(params->KI) * errorSum;
outP = *(params->KP) * currError;
output = outD + outP + outI;
*(params->OutputVariable) = output;
CoPostSem(params->ThisPIDSemID);
if(!params->IsTopLevelUnit)
CoPostSem(params->PrevPIDSemID);
CoPostSem(params->DataSemID);
}
else
{
// Reset if the PID controller is turned off
currError = 0;
errorSum = 0;
errorDiff = 0;
prevError = 0;
}
delayTicks = delaymsToTicks(*(params->UpdateTime_ms));
CoTickDelay(delayTicks);
}
}
static void sem2_execute(void)
{
int i;
StatusType err;
for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++)
{
Task_Id[i] = 0;
}
sem1ID = CoCreateSem(0,5,EVENT_SORT_TYPE_PRIO);
testAssert(sem1ID != (OS_EventID)-1,"#3");;
Task_Id[0] = CoCreateTask(task1, "A", MAINTEST_PRIMARY_PRIORITY - 3, &Task_Stack[0][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE);
Task_Id[1] = CoCreateTask(task1, "B", MAINTEST_PRIMARY_PRIORITY - 4, &Task_Stack[1][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE);
Task_Id[2] = CoCreateTask(task1, "C", MAINTEST_PRIMARY_PRIORITY - 1, &Task_Stack[2][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE);
Task_Id[3] = CoCreateTask(task1, "D", MAINTEST_PRIMARY_PRIORITY - 5, &Task_Stack[3][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE);
Task_Id[4] = CoCreateTask(task1, "E", MAINTEST_PRIMARY_PRIORITY - 2, &Task_Stack[4][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE);
for(i = 0; i < 5; i++)
{
err = CoPostSem(sem1ID);
}
testAssertSequence("DBAEC");
err = CoDelSem(sem1ID,OPT_DEL_ANYWAY);
testAssert(err == E_OK,"#4");
}
/**
*******************************************************************************
* @brief Respond the request in the service request queue.
* @param[in] None
* @param[out] None
* @retval None
*
* @par Description
* @details This function be called to respond the request in the service
* request queue.
* @note
*******************************************************************************
*/
void RespondSRQ(void)
{
#if CFG_MAX_SERVICE_REQUEST > 0
U16 i;
P_SQC pcell;
#endif
#if (CFG_TASK_WAITTING_EN > 0)
if(TimeReq == TRUE) /* Time delay request? */
{
TimeDispose(); /* Yes,call handler */
TimeReq = FALSE; /* Reset time delay request false */
}
#endif
#if CFG_TMR_EN > 0
if(TimerReq == TRUE) /* Timer request? */
{
TmrDispose(); /* Yes,call handler */
TimerReq = FALSE; /* Reset timer request false */
}
#endif
#if CFG_MAX_SERVICE_REQUEST > 0
pcell = &ServiceReq.cell[0]; /* Get the head item of service request list*/
for(i=0;i<ServiceReq.cnt;i++,pcell++)
{
switch(pcell->type) /* Judge service request type */
{
#if CFG_SEM_EN > 0
case SEM_REQ: /* Semaphore post request,call handler*/
CoPostSem(pcell->id);
break;
#endif
#if CFG_MAILBOX_EN > 0
case MBOX_REQ: /* Mailbox post request,call handler */
CoPostMail(pcell->id,pcell->arg);
break;
#endif
#if CFG_FLAG_EN > 0
case FLAG_REQ: /* Flag set request,call handler */
CoSetFlag(pcell->id);
break;
#endif
#if CFG_QUEUE_EN > 0
case QUEUE_REQ: /* Queue post request,call handler */
CoPostQueueMail(pcell->id,pcell->arg);
break;
#endif
default: /* Others,break */
break;
}
pcell->type = 0; /* Initialize the service request cell*/
pcell->id = 0;
pcell->arg = 0;
}
ServiceReq.cnt = 0; /* Initialize the service request queue */
#endif
IsrReq = FALSE;
}
StatusType isr_PostSem(OS_EventID id)
{
if(OSSchedLock > 0) /* If scheduler is locked,(the caller is ISR) */
{
/* Initiate a post service handling request */
if(InsertInSRQ(SEM_REQ,id,Co_NULL) == Co_FALSE)
{
return E_SEV_REQ_FULL; /* If service request queue is full */
}
else /* Operate successfully */
{
return E_OK;
}
}
else
{
return(CoPostSem(id)); /* Post semaphore */
}
}
/**
*******************************************************************************
* @brief Respond the request in the service request queue.
* @param[in] None
* @param[out] None
* @retval None
*
* @par Description
* @details This function be called to respond the request in the service
* request queue.
* @note
*******************************************************************************
*/
void RespondSRQ(void) {
#if CFG_MAX_SERVICE_REQUEST > 0
SQC cell;
#endif
#if (CFG_TASK_WAITTING_EN > 0)
if(TimeReq == Co_TRUE) { /* Time delay request? */
TimeDispose(); /* Yes,call handler */
TimeReq = Co_FALSE; /* Reset time delay request Co_FALSE */
}
#endif
#if CFG_TMR_EN > 0
if(TimerReq == Co_TRUE) { /* Timer request? */
TmrDispose(); /* Yes,call handler */
TimerReq = Co_FALSE; /* Reset timer request Co_FALSE */
}
#endif
#if CFG_MAX_SERVICE_REQUEST > 0
while (ServiceReq.cnt != 0) {
IRQ_DISABLE_SAVE (); /* need to protect the following */
cell = ServiceReq.cell[ServiceReq.head]; /* extract one cell */
ServiceReq.head = (ServiceReq.head + 1) % /* move head (pop) */
CFG_MAX_SERVICE_REQUEST;
ServiceReq.cnt--;
IRQ_ENABLE_RESTORE (); /* now use the cell copy */
switch(cell.type) { /* Judge service request type */
#if CFG_SEM_EN > 0
case SEM_REQ: /* Semaphore post request,call handler*/
CoPostSem(cell.id);
break;
#endif
#if CFG_MAILBOX_EN > 0
case MBOX_REQ: /* Mailbox post request,call handler */
CoPostMail(cell.id, cell.arg);
break;
#endif
#if CFG_FLAG_EN > 0
case FLAG_REQ: /* Flag set request,call handler */
CoSetFlag(cell.id);
break;
#endif
#if CFG_QUEUE_EN > 0
case QUEUE_REQ: /* Queue post request,call handler */
CoPostQueueMail(cell.id, cell.arg);
break;
#endif
default: /* Others,break */
break;
}
}
#endif
IRQ_DISABLE_SAVE (); /* need to protect the following */
if (ServiceReq.cnt == 0) { /* another item in the queue already? */
IsrReq = Co_FALSE; /* queue still empty here */
}
IRQ_ENABLE_RESTORE (); /* now it is done and return */
}
static void task2(void* pdata)
{
CoTickDelay(50);
CoPostSem(sem1ID);
CoExitTask();
}
void sys_sem_signal(sys_sem_t *sem) {
CoPostSem(*sem);
}
