Subversion Repositories shark

#include <math.h>

#include "tmwtypes.h"

#ifdef USE_RTMODEL

# include "simstruc_types.h"

#else

# include "simstruc.h"

#endif

#include "rt_sim.h"

#include "kernel/kern.h"

#ifndef RT_MALLOC /* statically declare data */

/*==========*

 * Struct's *

 *==========*/

/*

 * TimingData

 */

typedef struct TimingData_Tag {

    real_T period[NUMST];       /* Task periods in seconds                   */

    real_T offset[NUMST];       /* Task offsets in seconds                   */

    real_T clockTick[NUMST];    /* Flint task time tick counter              */

    int_T  taskTick[NUMST];     /* Counter for determining task hits         */

    int_T  nTaskTicks[NUMST];   /* Number base rate ticks for a task hit     */

    int_T  firstDiscIdx;        /* First discrete task index                 */

} TimingData;

/*=========================*

 * Data local to this file *

 *=========================*/

static TimingData td;

/*==================*

 * Visible routines *

 *==================*/

/* Function: rt_SimInitTimingEngine ============================================

 * Abstract:

 *      This function is for use with single tasking or multitasking

 *      real-time systems.  

 *

 *      Initializes the timing engine for a fixed-step real-time system.

 *      It is assumed that start time is 0.0.

 *

 * Returns:

 *      NULL     - success

 *      non-NULL - error string

 */

const char *rt_SimInitTimingEngine(int_T       rtmNumSampTimes,

                                   real_T      rtmStepSize,

                                   real_T      *rtmSampleTimePtr,

                                   real_T      *rtmOffsetTimePtr,

                                   int_T       *rtmSampleHitPtr,

                                   int_T       *rtmSampleTimeTaskIDPtr,

                                   real_T      rtmTStart,

                                   SimTimeStep *rtmSimTimeStepPtr,

                                   void        **rtmTimingDataPtr)

{

    int_T     i;

    int       *tsMap     = rtmSampleTimeTaskIDPtr;

    real_T    *period    = rtmSampleTimePtr;

    real_T    *offset    = rtmOffsetTimePtr;

    int_T     *sampleHit = rtmSampleHitPtr;

    real_T    stepSize   = rtmStepSize;

    if (rtmTStart != 0.0) {

        return("Start time must be zero for real-time systems");

    }

    *rtmSimTimeStepPtr = MAJOR_TIME_STEP;

    *rtmTimingDataPtr = (void*)&td;

    for (i = 0; i < NUMST; i++) {

        tsMap[i]         = i;

        td.period[i]     = period[i];

        td.offset[i]     = offset[i];

        td.nTaskTicks[i] = (int_T)floor(period[i]/stepSize + 0.5);

        if (td.period[i] == CONTINUOUS_SAMPLE_TIME ||

            td.offset[i] == 0.0) {

            td.taskTick[i]  = 0;

            td.clockTick[i] = 0.0;

            sampleHit[i]    = 1;

        } else {

            td.taskTick[i]  = (int_T)floor((td.period[i]-td.offset[i]) /

                                            stepSize+0.5);

            td.clockTick[i] = -1.0;

            sampleHit[i]    = 0;

        }

    }

    /* Correct first sample time if continuous task */

    td.period[0]     = stepSize;

    td.nTaskTicks[0] = 1;

    /* Set first discrete task index */

#if NUMST == 1    

    td.firstDiscIdx = (int_T)(period[0] == CONTINUOUS_SAMPLE_TIME);

#else

    td.firstDiscIdx = ((int_T)(period[0] == CONTINUOUS_SAMPLE_TIME) +

                       (int_T)(period[1] == CONTINUOUS_SAMPLE_TIME));

#endif

    return(NULL); /* success */

} /* end rt_SimInitTimingEngine */

#if !defined(MULTITASKING)

/*###########################################################################*/

/*########################### SINGLE TASKING ################################*/

/*###########################################################################*/

/* Function: rt_SimGetNextSampleHit ============================================

 * Abstract:

 *      For a single tasking real-time system, return time of next sample hit.

 */

time_T rt_SimGetNextSampleHit(void)

{

    time_T timeOfNextHit;

    td.clockTick[0] += 1;

    timeOfNextHit = td.clockTick[0] * td.period[0];

#   if NUMST > 1

    {

        int i;

        for (i = 1; i < NUMST; i++) {

            if (++td.taskTick[i] == td.nTaskTicks[i]) {

                td.taskTick[i] = 0;

                td.clockTick[i]++;

            }

        }

    }

#   endif

    return(timeOfNextHit);

} /* end rt_SimGetNextSampleHit */

/* Function: rt_SimUpdateDiscreteTaskSampleHits ================================

 * Abstract:

 *      This function is for use with single tasking real-time systems.  

 *      

 *      If the number of sample times is greater than one, then we need to

 *      update the discrete task sample hits for the next time step. Note,

 *      task 0 always has a hit since it's sample time is the fundamental

 *      sample time.

 */

void rt_SimUpdateDiscreteTaskSampleHits(int_T  rtmNumSampTimes,

                                        void   *rtmTimingData,

                                        int_T  *rtmSampleHitPtr,

                                        real_T *rtmTPtr)

{

    int_T *sampleHit = rtmSampleHitPtr;

    int   i;

    UNUSED_PARAMETER(rtmTimingData);

    UNUSED_PARAMETER(rtmNumSampTimes);

    for (i = td.firstDiscIdx; i < NUMST; i++) {

        int_T hit = (td.taskTick[i] == 0);

        if (hit) {

            rttiSetTaskTime(rtmTPtr, i,

                            td.clockTick[i]*td.period[i] + td.offset[i]);

        }

        sampleHit[i] = hit;

    }

} /* rt_SimUpdateDiscreteTaskSampleHits */

#else /* defined(MULTITASKING) */

/*###########################################################################*/

/*############################## MULTITASKING ###############################*/

/*###########################################################################*/

/* Function: rt_SimUpdateDiscreteEvents ========================================

 * Abstract:

 *      This function is for use with multitasking real-time systems.

 *

 *      This function updates the status of the RT_MODEL sampleHits

 *      flags and the perTaskSampleHits matrix which is used to determine

 *      when special sample hits occur.

 *

 *      The RT_MODEL contains a matrix, called perTaskSampleHits.

 *      This matrix is used by the ssIsSpecialSampleHit macro. The row and

 *      column indices are both task id's (equivalent to the root RT_MODEL

 *      sample time indices). This is a upper triangle matrix. This routine

 *      only updates the slower task hits (kept in column j) for row

 *      i if we have a sample hit in row i.

 *

 *                       column j

 *           tid   0   1   2   3   4   5  

 *               -------------------------

 *             0 |   | X | X | X | X | X |

 *         r     -------------------------

 *         o   1 |   |   | X | X | X | X |      This matrix(i,j) answers:

 *         w     -------------------------      If we are in task i, does

 *             2 |   |   |   | X | X | X |      slower task j have a hit now?

 *         i     -------------------------

 *             3 |   |   |   |   | X | X |

 *               -------------------------

 *             4 |   |   |   |   |   | X |      X = 0 or 1

 *               -------------------------

 *             5 |   |   |   |   |   |   |

 *               -------------------------

 *

 *      How macros index this matrix:

 *

 *          ssSetSampleHitInTask(S, j, i, X)   => matrix(i,j) = X

 *

 *          ssIsSpecialSampleHit(S, my_sti, promoted_sti, tid) =>

 *              (tid_for(promoted_sti) == tid && !minor_time_step &&

 *               matrix(tid,tid_for(my_sti))

 *              )

 *

 *            my_sti       = My (the block's) original sample time index.

 *            promoted_sti = The block's promoted sample time index resulting

 *                           from a transition via a ZOH from a fast to a

 *                           slow block or a transition via a unit delay from

 *                           a slow to a fast block.

 *

 *      The perTaskSampleHits array, of dimension n*n, is accessed using

 *      perTaskSampleHits[j + i*n] where n is the total number of sample

 *      times, 0 <= i < n, and 0 <= j < n.  The C language stores arrays in

 *      row-major order, that is, row 0 followed by row 1, etc.

 *

 */

time_T rt_SimUpdateDiscreteEvents(int_T  rtmNumSampTimes,

                                  void   *rtmTimingData,

                                  int_T  *rtmSampleHitPtr,

                                  int_T  *rtmPerTaskSampleHits)

{

    int   i, j;

    int_T *sampleHit = rtmSampleHitPtr;

    UNUSED_PARAMETER(rtmTimingData);

    /*

     * Run this loop in reverse so that we do lower priority events first.

     */

    i = NUMST;

    while (--i >= 0) {

        if (td.taskTick[i] == 0) {

            /*

             * Got a sample hit, reset the counter, and update the clock

             * tick counter.

             */

            sampleHit[i] = 1;

            td.clockTick[i]++;

            /*

             * Record the state of all "slower" events

             */

            for (j = i + 1; j < NUMST; j++) {

                rttiSetSampleHitInTask(rtmPerTaskSampleHits, rtmNumSampTimes,

                                       j, i, sampleHit[j]);

            }

        } else {

            /*

             * no sample hit, increment the counter

             */

            sampleHit[i] = 0;

        }

        if (++td.taskTick[i] == td.nTaskTicks[i]) { /* update for next time */

            td.taskTick[i] = 0;

        }

    }

    return(td.clockTick[0]*td.period[0]);

} /* rt_SimUpdateDiscreteEvents */

/* Function: rt_SimUpdateDiscreteTaskTime ======================================

 * Abstract:

 *      This function is for use with multitasking systems.

 *

 *      After a discrete task output and update has been performed, this

 *      function must be called to update the discrete task time for next

 *      time around.

 */

void rt_SimUpdateDiscreteTaskTime(real_T *rtmTPtr,

                                  void   *rtmTimingData,

                                  int    tid)

{

    UNUSED_PARAMETER(rtmTimingData);

    rttiSetTaskTime(rtmTPtr, tid,

                    td.clockTick[tid]*td.period[tid] + td.offset[tid]);

}

#endif /* MULTITASKING */

#else

#include "mrt_sim.c" /* dynamically allocate data */

#endif /* RT_MALLOC */

/*

 *******************************************************************************

 * FUNCTIONS MAINTAINED FOR BACKWARDS COMPATIBILITY WITH THE SimStruct

 *******************************************************************************

 */

#ifndef USE_RTMODEL

const char *rt_InitTimingEngine(SimStruct *S)

{

    const char_T *retVal = rt_SimInitTimingEngine(

        ssGetNumSampleTimes(S),

        ssGetStepSize(S),

        ssGetSampleTimePtr(S),

        ssGetOffsetTimePtr(S),

        ssGetSampleHitPtr(S),

        ssGetSampleTimeTaskIDPtr(S),

        ssGetTStart(S),

        &ssGetSimTimeStep(S),

        &ssGetTimingData(S));

    return(retVal);

}

# ifdef RT_MALLOC

void rt_DestroyTimingEngine(SimStruct *S)

{

    rt_SimDestroyTimingEngine(ssGetTimingData(S));

}

# endif

# if !defined(MULTITASKING)

void rt_UpdateDiscreteTaskSampleHits(SimStruct *S)

{

    rt_SimUpdateDiscreteTaskSampleHits(

        ssGetNumSampleTimes(S),

        ssGetTimingData(S),

        ssGetSampleHitPtr(S),

        ssGetTPtr(S));

}

#   ifndef RT_MALLOC

time_T rt_GetNextSampleHit(void)

{

    return(rt_SimGetNextSampleHit());

}

#   else /* !RT_MALLOC */

time_T rt_GetNextSampleHit(SimStruct *S)

{

    return(rt_SimGetNextSampleHit(ssGetTimingData(S),

                                  ssGetNumSampleTimes(S)));

}

#   endif

# else /* MULTITASKING */

time_T rt_UpdateDiscreteEvents(SimStruct *S)

{

    return(rt_SimUpdateDiscreteEvents(

               ssGetNumSampleTimes(S),

               ssGetTimingData(S),

               ssGetSampleHitPtr(S),

               ssGetPerTaskSampleHitsPtr(S)));

}

void rt_UpdateDiscreteTaskTime(SimStruct *S, int tid)

{

    rt_SimUpdateDiscreteTaskTime(ssGetTPtr(S), ssGetTimingData(S), tid);

}

#endif

#endif

/* EOF: rt_sim.c */