timertask.cpp

一个非常好的人工智能开发工具开源软件

/***
 *** See the file "L2_RTI_EO1/disclaimers-and-notices-L2.txt" for 
 *** information on usage and redistribution of this file, 
 *** and for a DISCLAIMER OF ALL WARRANTIES.
 ***/
/* DESCRIPTION */
/* This file takes care of timer request from the RTI task. */

/* includes */
#include "TimerTask.hpp"
#include <taskLib.h>
#include <sysLib.h>
#include <tickLib.h>
#include <stdio.h>   /* for errno */
#include <stdlib.h>
#include <string.h>
#include "ExceptionAction.hpp"

/* defines */
#define EXPIRY_STACK_SIZE 25
#define DELAY_TICK 15
#define BILLION 1000000000

typedef struct {
    MONITOR_DATA monitor;
    unsigned int timeoutTick;
} REQUEST_PACKET;

/* locals */
static int timerTaskId;
static unsigned int notDone;
static unsigned int expiryStackLength;
static REQUEST_PACKET expiryStack[EXPIRY_STACK_SIZE];


/*=========================================================================*\
* FUNCTION
*   compareTimes
*
* PURPOSE
*   To be used by "qsort" to sort time values in numerical order.
*
* PARAMETERS
*   arg1 = Pointer to the key array element.
*   arg2 = Pointer to the array element to be compared with the key.
*
* RETURNS
*   < 0 time 1 less than time 2
*   = 0 time 1 equivalent to time 2
*   > 0 time 1 greater than time 2
\*=========================================================================*/
int compareTimes(const void *arg1, const void *arg2) {
  if (((REQUEST_PACKET*)arg1)->timeoutTick >
      ((REQUEST_PACKET*)arg2)->timeoutTick) {
    return 1;
  }
  if (((REQUEST_PACKET*)arg1)->timeoutTick <
      ((REQUEST_PACKET*)arg2)->timeoutTick) {
    return -1;
  }
  return 0;
}


/*=========================================================================*\
* FUNCTION
*   TimerTask
*
* PURPOSE
*   A task to check for timer expirations. When there is a timer
*   expiration, a message is sent to the RTI task.
*
* PARAMETERS
*   None.
*
* RETURNS
*   Nothing.
*
* NOTE
*   This task should be set at a higher priority than the task that calls
*   the requestTimer function to avoid data corruption.
\*=========================================================================*/
void TimerTask(void) {
  /* Initialize */
  expiryStackLength = 0;
  notDone = 1;

  /* Let the requestTimer function know who I am. */
  timerTaskId = taskIdSelf();

  while(notDone) {
    if (expiryStackLength == 0) {
      tickSet(0);     /* Keep the tick values as low as possible */
                      /* to avoid overflows. */
      taskSuspend(0); /* Suspend myself until there's work to do. */
    }

    taskDelay(DELAY_TICK);

    while (expiryStackLength > 0 &&
           tickGet() >= expiryStack[0].timeoutTick) {
      if (msgQSend(SCL_TO_RTI_MQID,
                   (char*)&expiryStack[0].monitor,
                   sizeof(MONITOR_DATA),
                   NO_WAIT,
                   MSG_PRI_NORMAL) != 0) {
        ExceptionAction::sendFailure(ExceptionAction::SND_TIMER,
                                     ExceptionAction::RCV_REAL_TIME_INTERFACE,
                                     errno);
      }

      expiryStackLength--;

      /* Shift the stack down. */
      memcpy(&expiryStack[0],
             &expiryStack[1],
             expiryStackLength * sizeof(REQUEST_PACKET));
    }
  }
}


/*=========================================================================*\
* FUNCTION
*   requestTimer
*
* PURPOSE
*   Puts a timer request on the stack in ascending timer order.
*
* PARAMETERS
*   milliseconds    = Request notification in 'x' milliseconds.
*   timeoutVariable = L2 model variable
*   timeoutValue    = Value of L2 model variable
*
* RETURNS
*   Nothing.
\*=========================================================================*/
void requestTimer(unsigned int milliseconds,
                  int timeoutVariable,
                  int timeoutValue) {

  REQUEST_PACKET *reqPtr;

  if (expiryStackLength < EXPIRY_STACK_SIZE) { /* Array boundary checking. */
    taskSuspend(timerTaskId); /* Avoid data corruption. */

    reqPtr = &expiryStack[expiryStackLength];
    reqPtr->monitor.msgType         = TIMER_EXPIRATION;
    reqPtr->monitor.timeoutVariable = timeoutVariable;
    reqPtr->monitor.timeoutValue    = timeoutValue;
    reqPtr->timeoutTick             = tickGet() + (milliseconds *
                                      sysClkRateGet() / 1000);

    expiryStackLength++;
    qsort(expiryStack, expiryStackLength, sizeof(REQUEST_PACKET),
          compareTimes);

    taskResume(timerTaskId); /* Tell timer task it has work to do. */
  }
}

/*=========================================================================*\
* FUNCTION
*   requestHeartbeat
*
* PURPOSE
*   Puts a heartbeat timer request on the stack in ascending timer order.
*
* PARAMETERS
*   None.
*
* RETURNS
*   Nothing.
\*=========================================================================*/
void requestHeartbeat(void) {

  REQUEST_PACKET *reqPtr;

  if (expiryStackLength < EXPIRY_STACK_SIZE) { /* Array boundary checking. */
    taskSuspend(timerTaskId); /* Avoid data corruption. */

    reqPtr = &expiryStack[expiryStackLength];
    reqPtr->monitor.msgType         = TIMER_HEARTBEAT;
    reqPtr->timeoutTick             = tickGet() + (HEARTBEATRATE *
                                      sysClkRateGet() / 1000);

    expiryStackLength++;
    qsort(expiryStack, expiryStackLength, sizeof(REQUEST_PACKET),
          compareTimes);

    taskResume(timerTaskId); /* Tell timer task it has work to do. */
  }
}

/*=========================================================================*\
* FUNCTION
*   killTimerTask
*
* PURPOSE
*   To notify the timer task to gracefully shutdown.
*
* PARAMETERS
*   None.
*
* RETURNS
*   Nothing.
\*=========================================================================*/
void killTimerTask(void) {
  notDone = 0;
  taskResume(timerTaskId);
}