grt.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include "model.h"
00030 #include "grt.h"
00031 
00032 #if GRTINTERFACE == 1
00033 # error "classic call interface is not supported"
00034 #endif
00035 
00036 /*==================================================*
00037  * External functions for Simplified Call Interface *
00038  *==================================================*/
00039 # define MODEL_INITIALIZE    CONCAT(MODEL,_initialize)
00040 #if ONESTEPFCN == 1
00041 # define MODEL_STEP          CONCAT(MODEL,_step)
00042 # define MODEL_OUTPUT        MODEL_STEP
00043 #else
00044 # define MODEL_OUTPUT        CONCAT(MODEL,_output)
00045 # define MODEL_UPDATE        CONCAT(MODEL,_update)
00046 #endif
00047 
00048 # define MODEL_TERMINATE     CONCAT(MODEL,_terminate)
00049 # define RT_MDL              CONCAT(MODEL,_M)
00050 
00051 extern void MODEL_INITIALIZE(void);
00052 extern void MODEL_TERMINATE(void);
00053 
00054 #if !defined(MULTITASKING)
00055 #if ONESTEPFCN == 1
00056 # define MODEL_UPDATE()              /* No op */
00057 extern void MODEL_STEP(void);        /* single rate step function */
00058 #else
00059 extern void MODEL_OUTPUT(void);      /* single rate output function */
00060 extern void MODEL_UPDATE(void);      /* single rate update function */
00061 #endif
00062 #else
00063 #if ONESTEPFCN == 1
00064 # define MODEL_UPDATE(S)             /* No op */
00065 extern void MODEL_STEP(int_T tid);   /* multirate step function */
00066 #else
00067 extern void MODEL_OUTPUT(int_T tid); /* multirate output function */
00068 extern void MODEL_UPDATE(int_T tid); /* multirate update function */
00069 #endif
00070 #endif  /* !defined(MULTITASKING) */
00071 
00072 #ifdef EXT_MODE
00073 #  define rtExtModeSingleTaskUpload(S)                          \
00074    {                                                            \
00075         int stIdx;                                              \
00076         rtExtModeUploadCheckTrigger(rtmGetNumSampleTimes(S));   \
00077         for (stIdx=0; stIdx<NUMST; stIdx++) {                   \
00078             if (rtmIsSampleHit(S, stIdx, 0 /*unused*/)) {       \
00079                 rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx)); \
00080             }                                                   \
00081         }                                                       \
00082    }
00083 #else
00084 #  define rtExtModeSingleTaskUpload(S) /* Do nothing */
00085 #endif
00086 
00087 /*=================*
00088  * Local functions *
00089  *=================*/
00090 
00091 namespace rosrtw {
00092 
00093 #if !defined(MULTITASKING)  /* SINGLETASKING */
00094 
00095 /* Function: rtOneStep ========================================================
00096  *
00097  * Abstract:
00098  *      Perform one step of the model. This function is modeled such that
00099  *      it could be called from an interrupt service routine (ISR) with minor
00100  *      modifications.
00101  */
00102 void Model::rt_OneStep(RT_MODEL *S)
00103 {
00104     real_T tnext;
00105 
00106     /***********************************************
00107      * Check and see if base step time is too fast *
00108      ***********************************************/
00109     if (GBLbuf.isrOverrun++) {
00110         GBLbuf.stopExecutionFlag = 1;
00111         return;
00112     }
00113 
00114     /***********************************************
00115      * Check and see if error status has been set  *
00116      ***********************************************/
00117     if (rtmGetErrorStatus(S) != NULL) {
00118         GBLbuf.stopExecutionFlag = 1;
00119         return;
00120     }
00121 
00122     /* enable interrupts here */
00123 
00124     tnext = rt_SimGetNextSampleHit();
00125     rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
00126 
00127     MODEL_OUTPUT();
00128 
00129     rtExtModeSingleTaskUpload(S);
00130 
00131     GBLbuf.errmsg = rt_UpdateTXYLogVars(rtmGetRTWLogInfo(S),
00132                                         rtmGetTPtr(S));
00133     if (GBLbuf.errmsg != NULL) {
00134         GBLbuf.stopExecutionFlag = 1;
00135         return;
00136     }
00137 
00138     rt_UpdateSigLogVars(rtmGetRTWLogInfo(S), rtmGetTPtr(S));
00139 
00140     MODEL_UPDATE();
00141 
00142     GBLbuf.isrOverrun--;
00143 
00144     rtExtModeCheckEndTrigger();
00145 
00146 } /* end rtOneStep */
00147 
00148 #else /* MULTITASKING */
00149 
00150 # if TID01EQ == 1
00151 #  define FIRST_TID 1
00152 # else
00153 #  define FIRST_TID 0
00154 # endif
00155 
00156 /* Function: rtOneStep ========================================================
00157  *
00158  * Abstract:
00159  *      Perform one step of the model. This function is modeled such that
00160  *      it could be called from an interrupt service routine (ISR) with minor
00161  *      modifications.
00162  *
00163  *      This routine is modeled for use in a multitasking environment and
00164  *      therefore needs to be fully re-entrant when it is called from an
00165  *      interrupt service routine.
00166  *
00167  * Note:
00168  *      Error checking is provided which will only be used if this routine
00169  *      is attached to an interrupt.
00170  *
00171  */
00172 void Model::rt_OneStep(RT_MODEL *S)
00173 {
00174     int_T  i;
00175     real_T tnext;
00176     int_T  *sampleHit = rtmGetSampleHitPtr(S);
00177 
00178     /***********************************************
00179      * Check and see if base step time is too fast *
00180      ***********************************************/
00181     if (GBLbuf.isrOverrun++) {
00182         GBLbuf.stopExecutionFlag = 1;
00183         return;
00184     }
00185 
00186     /***********************************************
00187      * Check and see if error status has been set  *
00188      ***********************************************/
00189     if (rtmGetErrorStatus(S) != NULL) {
00190         GBLbuf.stopExecutionFlag = 1;
00191         return;
00192     }
00193     /* enable interrupts here */
00194 
00195     /***********************************************
00196      * Update discrete events                      *
00197      ***********************************************/
00198     tnext = rt_SimUpdateDiscreteEvents(rtmGetNumSampleTimes(S),
00199                                        rtmGetTimingData(S),
00200                                        rtmGetSampleHitPtr(S),
00201                                        rtmGetPerTaskSampleHitsPtr(S));
00202     rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
00203     for (i=FIRST_TID+1; i < NUMST; i++) {
00204         if (sampleHit[i] && GBLbuf.eventFlags[i]++) {
00205             GBLbuf.isrOverrun--;
00206             GBLbuf.overrunFlags[i]++;    /* Are we sampling too fast for */
00207             GBLbuf.stopExecutionFlag=1;  /*   sample time "i"?           */
00208             return;
00209         }
00210     }
00211     /*******************************************
00212      * Step the model for the base sample time *
00213      *******************************************/
00214     MODEL_OUTPUT(0);
00215 
00216     rtExtModeUploadCheckTrigger(rtmGetNumSampleTimes(S));
00217     rtExtModeUpload(FIRST_TID,rtmGetTaskTime(S, FIRST_TID));
00218 
00219     GBLbuf.errmsg = rt_UpdateTXYLogVars(rtmGetRTWLogInfo(S),
00220                                         rtmGetTPtr(S));
00221     if (GBLbuf.errmsg != NULL) {
00222         GBLbuf.stopExecutionFlag = 1;
00223         return;
00224     }
00225 
00226     rt_UpdateSigLogVars(rtmGetRTWLogInfo(S), rtmGetTPtr(S));
00227 
00228     MODEL_UPDATE(0);
00229 
00230     /************************************************************************
00231      * Model step complete for base sample time, now it is okay to          *
00232      * re-interrupt this ISR.                                               *
00233      ************************************************************************/
00234 
00235     GBLbuf.isrOverrun--;
00236 
00237 
00238     /*********************************************
00239      * Step the model for any other sample times *
00240      *********************************************/
00241     for (i=FIRST_TID+1; i<NUMST; i++) {
00242         /* If task "i" is running, don't run any lower priority task */
00243         if (GBLbuf.overrunFlags[i]) return;
00244 
00245         if (GBLbuf.eventFlags[i]) {
00246             GBLbuf.overrunFlags[i]++;
00247 
00248             MODEL_OUTPUT(i);
00249 
00250             rtExtModeUpload(i, rtmGetTaskTime(S,i));
00251 
00252             MODEL_UPDATE(i);
00253 
00254             /* Indicate task complete for sample time "i" */
00255             GBLbuf.overrunFlags[i]--;
00256             GBLbuf.eventFlags[i]--;
00257         }
00258     }
00259 
00260     rtExtModeCheckEndTrigger();
00261 
00262 } /* end rtOneStep */
00263 
00264 #endif /* MULTITASKING */
00265 
00266 
00267 /*===================*
00268  * Visible functions *
00269  *===================*/
00270 
00271 bool Model::initialize()
00272 {
00273   if (is_initialized) return false;
00274 
00275   /************************
00276    * Initialize the model *
00277    ************************/
00278   S = RT_MDL;
00279   MODEL_INITIALIZE();
00280 
00281   if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
00282 
00283 #ifdef UseMMIDataLogging
00284   rt_FillStateSigInfoFromMMI(rtmGetRTWLogInfo(S),&rtmGetErrorStatus(S));
00285   rt_FillSigLogInfoFromMMI(rtmGetRTWLogInfo(S),&rtmGetErrorStatus(S));
00286 #endif
00287   GBLbuf.errmsg = rt_StartDataLogging(rtmGetRTWLogInfo(S),
00288                                       rtmGetTFinal(S),
00289                                       rtmGetStepSize(S),
00290                                       &rtmGetErrorStatus(S));
00291   if (GBLbuf.errmsg != NULL) {
00292       (void)fprintf(stderr,"Error starting data logging: %s\n",GBLbuf.errmsg);
00293       return(EXIT_FAILURE);
00294   }
00295 
00296   rtExtModeCheckInit(rtmGetNumSampleTimes(S));
00297   rtExtModeWaitForStartPkt(rtmGetRTWExtModeInfo(S),
00298                            rtmGetNumSampleTimes(S),
00299                            (boolean_T *)&rtmGetStopRequested(S));
00300 
00301   (void)printf("\n** starting the model **\n");
00302 
00303   is_initialized = true;
00304   return true;
00305 }
00306 
00307 void Model::loop()
00308 {
00309   if (!is_initialized) return;
00310 
00311   /*************************************************************************
00312    * Execute the model.  You may attach rtOneStep to an ISR, if so replace *
00313    * the call to rtOneStep (below) with a call to a background task        *
00314    * application.                                                          *
00315    *************************************************************************/
00316   if (rtmGetTFinal(S) == RUN_FOREVER) {
00317       printf ("\n**May run forever. Model stop time set to infinity.**\n");
00318   }
00319 
00320   while(isRunning()) step();
00321 }
00322 
00323 
00324 bool Model::isRunning()
00325 {
00326   return (!GBLbuf.stopExecutionFlag &&
00327            (rtmGetTFinal(S) == RUN_FOREVER ||
00328             rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON));
00329 }
00330 
00331 void Model::step()
00332 {
00333   rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
00334                          rtmGetNumSampleTimes(S),
00335                          (boolean_T *)&rtmGetStopRequested(S));
00336 
00337   if (rtmGetStopRequested(S)) return;
00338   /* external mode */
00339   rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
00340           rtmGetNumSampleTimes(S),
00341           (boolean_T *)&rtmGetStopRequested(S));
00342 
00343   rt_OneStep(S);
00344 }
00345 
00346 void Model::terminate()
00347 {
00348   if (!is_initialized) return;
00349 
00350   if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
00351       /* external mode */
00352       rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
00353               rtmGetNumSampleTimes(S),
00354               (boolean_T *)&rtmGetStopRequested(S));
00355 
00356       /* Execute model last time step */
00357       rt_OneStep(S);
00358   }
00359 
00360   /********************
00361    * Cleanup and exit *
00362    ********************/
00363 #ifdef UseMMIDataLogging
00364   rt_CleanUpForStateLogWithMMI(rtmGetRTWLogInfo(S));
00365   rt_CleanUpForSigLogWithMMI(rtmGetRTWLogInfo(S));
00366 #endif
00367   rt_StopDataLogging(MATFILE,rtmGetRTWLogInfo(S));
00368 
00369   rtExtModeShutdown(rtmGetNumSampleTimes(S));
00370 
00371   if (GBLbuf.errmsg) {
00372       (void)fprintf(stderr,"%s\n",GBLbuf.errmsg);
00373       exit(EXIT_FAILURE);
00374   }
00375 
00376   if (rtmGetErrorStatus(S) != NULL) {
00377       (void)fprintf(stderr,"ErrorStatus set: \"%s\"\n", rtmGetErrorStatus(S));
00378       exit(EXIT_FAILURE);
00379   }
00380 
00381   if (GBLbuf.isrOverrun) {
00382       (void)fprintf(stderr,
00383                     "%s: ISR overrun - base sampling rate is too fast\n",
00384                     QUOTE(MODEL));
00385       exit(EXIT_FAILURE);
00386   }
00387 
00388 #ifdef MULTITASKING
00389   else {
00390       int_T i;
00391       for (i=1; i<NUMST; i++) {
00392           if (GBLbuf.overrunFlags[i]) {
00393               (void)fprintf(stderr,
00394                       "%s ISR overrun - sampling rate is too fast for "
00395                       "sample time index %d\n", QUOTE(MODEL), i);
00396               exit(EXIT_FAILURE);
00397           }
00398       }
00399   }
00400 #endif
00401 
00402   MODEL_TERMINATE();
00403 
00404   is_initialized = false;
00405 }
00406 
00407 } // namespace rosrtw


rosrtw
Author(s): Johannes Meyer
autogenerated on Fri Jul 25 2014 06:08:44