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);
}
}
/*
Blocks the thread while waiting for the semaphore to be
signaled. If the "timeout" argument is non-zero, the thread should
only be blocked for the specified time (measured in
milliseconds).
If the timeout argument is non-zero, the return value is the number of
milliseconds spent waiting for the semaphore to be signaled. If the
semaphore wasn't signaled within the specified time, the return value is
SYS_ARCH_TIMEOUT. If the thread didn't have to wait for the semaphore
(i.e., it was already signaled), the function may return zero.
Notice that lwIP implements a function with a similar name,
sys_sem_wait(), that uses the sys_arch_sem_wait() function.
*/
u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout) {
StatusType result;
u32_t timeoutTick;
u32_t startTick, endTick, elapseTime;
//in lwip ,timeout is millisecond
//in Coos ,timeout is timer tick!
//chang timeout from millisecond to Coos tick
timeoutTick = (timeout * CFG_SYSTICK_FREQ) / 1000;
if(timeoutTick==0 && timeout != 0) {
timeoutTick = 1;
}
if(timeoutTick > 0xFFFF) {
timeoutTick = 0xFFFF;
}
startTick = CoGetOSTime();
result = CoPendSem(*sem, timeoutTick);
if(timeoutTick != 0 && result != E_OK) {
return SYS_ARCH_TIMEOUT;
}
endTick = CoGetOSTime();
//chang timeout from Coos tick to millisecond
elapseTime = ((endTick - startTick) *1000) / CFG_SYSTICK_FREQ;
return elapseTime;
}
static void task1(void *pdata)
{
StatusType err;
//StatusType PendSem(OS_EventID id,U32 timeout)
err = CoPendSem(sem1ID,0);
if (err == E_OK)
{
testEmitToken(*(char*)pdata);
}
CoExitTask();
}
static void sem3_execute(void)
{
StatusType err;
U64 expectTime;
int i;
sem1ID = CoCreateSem(0,5,EVENT_SORT_TYPE_FIFO);
// Test 1 AcceptSem
expectTime = CoGetOSTime();
err = CoAcceptSem(sem1ID);
testAssert(err == E_SEM_EMPTY,"#5");
testAssertTimeWindow(expectTime,expectTime + ALLOWED_DELAY_TICK);
// Test 2 PendSem with timeout
//printf("#5");
expectTime = CoGetOSTime() + (MAX_SLAVE_TEST_TASKS * 50);
for(i = 0; i < MAX_SLAVE_TEST_TASKS; i++)
{
err = CoPendSem(sem1ID,50);
testAssert(err == E_TIMEOUT,"#6");
}
testAssertTimeWindow(expectTime,expectTime + ALLOWED_DELAY_TICK);
//printf("#6");
// Test 3 PendSem without timeout
Task_Id[0] = CoCreateTask(task2, "A", MAINTEST_PRIMARY_PRIORITY - 3, &Task_Stack[0][SLAVE_TASK_STK_SIZE-1], SLAVE_TASK_STK_SIZE);
err = CoPendSem(sem1ID,100);
testAssert(err == E_OK,"#8");
// PostSem only inc 1 && PendSem dec 1
//printf("#7");
err = CoPendSem(sem1ID,100);
testAssert(err == E_TIMEOUT,"#9");
err = CoDelSem(sem1ID,OPT_DEL_NO_PEND);
testAssert(err == E_OK,"#7");
}
/**
* This function should be called when a packet is ready to be read
* from the interface. It uses the function low_level_input() that
* should handle the actual reception of bytes from the network
* interface. Then the type of the received packet is determined and
* the appropriate input function is called.
*
* @param netif the lwip network interface structure for this ethernetif
*/
static void ethernetif_input(void *pdata) {
struct netif *netif = (struct netif*)pdata;
struct eth_hdr *ethhdr;
struct pbuf *p;
while (1) {
do {
/* move received packet into a new pbuf */
p = low_level_input(netif);
/* no packet could be read, silently ignore this */
if (p == NULL) {
/* No packet could be read. Wait a for an interrupt to tell us
there is more data available. */
CoPendSem( sem_rx, BLOCK_TIME_WAITING_FOR_INPUT);
}
} while (NULL == p);
/* points to packet payload, which starts with an Ethernet header */
ethhdr = p->payload;
switch (htons(ethhdr->type)) {
/* IP or ARP packet? */
case ETHTYPE_IP:
case ETHTYPE_ARP:
#if PPPOE_SUPPORT
/* PPPoE packet? */
case ETHTYPE_PPPOEDISC:
case ETHTYPE_PPPOE:
#endif /* PPPOE_SUPPORT */
/* full packet send to tcpip_thread to process */
if (ERR_OK != netif->input(p, netif)) {
LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n"));
pbuf_free(p);
p = NULL;
}
break;
default:
pbuf_free(p);
p = NULL;
break;
}
}
}
/**
* This function should do the actual transmission of the packet. The packet is
* contained in the pbuf that is passed to the function. This pbuf
* might be chained.
*
* @param netif the lwip network interface structure for this ethernetif
* @param p the MAC packet to send (e.g. IP packet including MAC addresses and type)
* @return ERR_OK if the packet could be sent
* an err_t value if the packet couldn't be sent
*
* @note Returning ERR_MEM here if a DMA queue of your MAC is full can lead to
* strange results. You might consider waiting for space in the DMA queue
* to become availale since the stack doesn't retry to send a packet
* dropped because of memory failure (except for the TCP timers).
*/
static err_t low_level_output(struct netif *netif, struct pbuf *p) {
struct pbuf *q;
u16_t l = 0;
uint8_t *buf = NULL;
int i;
//initiate transfer();
#if 0 == ENET_HARDWARE_SHIFT
#if ETH_PAD_SIZE
pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */
#endif
#endif
/* Get a DMA buffer into which we can write the data to send. */
for (i = 0; i < BUF_WAIT_ATTEMPTS; i++) {
if (tx_bd[tx_next_buf].status & ENET_TX_BD_R) {
/* wait for the buffer to become available */
CoTickDelay(BUF_WAIT_DELAY);
}
else {
#ifdef ENET_LITTLE_ENDIAN
buf = (uint8_t*)__REV((uint32_t)tx_bd[tx_next_buf].data);
#else
buf = tx_bd[tx_next_buf].data;
#endif
break;
}
}
if (NULL == buf) {
return ERR_BUF;
}
else {
for (q = p; q != NULL; q = q->next) {
/* Send the data from the pbuf to the interface, one pbuf at a
time. The size of the data in each pbuf is kept in the ->len
variable. */
memcpy(&buf[l], (u8_t*)q->payload, q->len);
l += q->len;
}
}
//signal that packet should be sent();
/* Setup the buffer descriptor for transmission */
#ifdef ENET_LITTLE_ENDIAN
tx_bd[tx_next_buf].length = __REVSH(l);//nbuf->length + ETH_HDR_LEN;
#else
tx_bd[tx_next_buf].length = l;//nbuf->length + ETH_HDR_LEN;
#endif
tx_bd[tx_next_buf].status |= (ENET_TX_BD_R | ENET_TX_BD_L);
tx_next_buf++;
if (tx_next_buf >= NUM_ENET_TX_BUFS) {
tx_next_buf = 0;
}
#if 0 == ENET_HARDWARE_SHIFT
#if ETH_PAD_SIZE
pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */
#endif
#endif
LINK_STATS_INC(link.xmit);
/* only one task can be here. wait until pkt is sent, then go ahead */
/* semaphore released inside isr */
/* start expiring semaphore: no more than 3 ticks */
/* no blocking code */
CoPendSem(sem_tx, 3);
/* Request xmit process to MAC-NET */
ENET_TDAR = ENET_TDAR_TDAR_MASK;
return ERR_OK;
}
