grt_malloc.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 #ifndef RT_MALLOC
00033 # error "grt_malloc_main.c require RT_MALLOC to be defined"
00034 #endif
00035 
00036 /*====================*
00037  * External functions *
00038  *====================*/
00039 #ifdef __cplusplus
00040 
00041 extern "C" {
00042 
00043 #endif
00044 
00045 extern RT_MODEL *MODEL(void);
00046 
00047 #ifdef __cplusplus
00048 
00049 }
00050 #endif
00051 
00052 #if NCSTATES > 0
00053 #ifdef __cplusplus
00054 
00055 extern "C" {
00056 
00057 #endif
00058   extern void rt_ODECreateIntegrationData(RTWSolverInfo *si);
00059   extern void rt_ODEUpdateContinuousStates(RTWSolverInfo *si);
00060 # if defined(RT_MALLOC)
00061    extern void rt_ODEDestroyIntegrationData(RTWSolverInfo *si);
00062 # endif
00063 #ifdef __cplusplus
00064 
00065 }
00066 #endif
00067 #else
00068 # define rt_ODECreateIntegrationData(si)  \
00069   rtsiSetSolverName(si, "FixedStepDiscrete");
00070 # define rt_ODEUpdateContinuousStates(si) \
00071   rtsiSetT(si,rtsiGetSolverStopTime(si))
00072 #endif
00073 
00074 
00075 /*=============*
00076  * Global data *
00077  *=============*/
00078 
00079 const char *RT_MEMORY_ALLOCATION_ERROR = "memory allocation error";
00080 
00081 /*==================================*
00082  * Global data local to this module *
00083  *==================================*/
00084 
00085 #ifdef EXT_MODE
00086 #  define rtExtModeSingleTaskUpload(S)                          \
00087    {                                                            \
00088         int stIdx;                                              \
00089         rtExtModeUploadCheckTrigger(rtmGetNumSampleTimes(S));   \
00090         for (stIdx=0; stIdx<NUMST; stIdx++) {                   \
00091             if (rtmIsSampleHit(S, stIdx, 0 /*unused*/)) {       \
00092                 rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx)); \
00093             }                                                   \
00094         }                                                       \
00095    }
00096 #else
00097 #  define rtExtModeSingleTaskUpload(S) /* Do nothing */
00098 #endif
00099 
00100 /*=================*
00101  * Local functions *
00102  *=================*/
00103 
00104 namespace rosrtw {
00105 
00106 #if !defined(MULTITASKING)  /* SINGLETASKING */
00107 
00108 /* Function: rtOneStep ========================================================
00109  *
00110  * Abstract:
00111  *      Perform one step of the model. This function is modeled such that
00112  *      it could be called from an interrupt service routine (ISR) with minor
00113  *      modifications.
00114  */
00115 void Model::rt_OneStep(RT_MODEL *S)
00116 {
00117     real_T tnext;
00118 
00119     /***********************************************
00120      * Check and see if base step time is too fast *
00121      ***********************************************/
00122     if (GBLbuf.isrOverrun++) {
00123         GBLbuf.stopExecutionFlag = 1;
00124         return;
00125     }
00126 
00127     /***********************************************
00128      * Check and see if error status has been set  *
00129      ***********************************************/
00130     if (rtmGetErrorStatus(S) != NULL) {
00131         GBLbuf.stopExecutionFlag = 1;
00132         return;
00133     }
00134 
00135     /* enable interrupts here */
00136 
00137     tnext = rt_SimGetNextSampleHit(rtmGetTimingData(S),
00138                                    rtmGetNumSampleTimes(S));
00139     rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
00140 
00141     rtmiOutputs(rtmGetRTWRTModelMethodsInfo(S),0);
00142 
00143     rtExtModeSingleTaskUpload(S);
00144 
00145     GBLbuf.errmsg = rt_UpdateTXYLogVars(rtmGetRTWLogInfo(S),
00146                                         rtmGetTPtr(S));
00147     if (GBLbuf.errmsg != NULL) {
00148         GBLbuf.stopExecutionFlag = 1;
00149         return;
00150     }
00151 
00152     rtmiUpdate(rtmGetRTWRTModelMethodsInfo(S),0);
00153 
00154     rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(S),
00155                                        rtmGetTimingData(S),
00156                                        rtmGetSampleHitPtr(S),
00157                                        rtmGetTPtr(S));
00158 
00159     if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
00160         rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(S));
00161     }
00162 
00163     GBLbuf.isrOverrun--;
00164 
00165     rtExtModeCheckEndTrigger();
00166 
00167 } /* end rtOneStep */
00168 
00169 #else /* MULTITASKING */
00170 
00171 # if TID01EQ == 1
00172 #  define FIRST_TID 1
00173 # else
00174 #  define FIRST_TID 0
00175 # endif
00176 
00177 /* Function: rtOneStep ========================================================
00178  *
00179  * Abstract:
00180  *      Perform one step of the model. This function is modeled such that
00181  *      it could be called from an interrupt service routine (ISR) with minor
00182  *      modifications.
00183  *
00184  *      This routine is modeled for use in a multitasking environment and
00185  *      therefore needs to be fully re-entrant when it is called from an
00186  *      interrupt service routine.
00187  *
00188  * Note:
00189  *      Error checking is provided which will only be used if this routine
00190  *      is attached to an interrupt.
00191  *
00192  */
00193 void Model::rt_OneStep(RT_MODEL *S)
00194 {
00195     int_T  i;
00196     real_T tnext;
00197     int_T  *sampleHit = rtmGetSampleHitPtr(S);
00198 
00199     /***********************************************
00200      * Check and see if base step time is too fast *
00201      ***********************************************/
00202     if (GBLbuf.isrOverrun++) {
00203         GBLbuf.stopExecutionFlag = 1;
00204         return;
00205     }
00206 
00207     /***********************************************
00208      * Check and see if error status has been set  *
00209      ***********************************************/
00210     if (rtmGetErrorStatus(S) != NULL) {
00211         GBLbuf.stopExecutionFlag = 1;
00212         return;
00213     }
00214     /* enable interrupts here */
00215 
00216     /***********************************************
00217      * Update discrete events                      *
00218      ***********************************************/
00219     tnext = rt_SimUpdateDiscreteEvents(rtmGetNumSampleTimes(S),
00220                                        rtmGetTimingData(S),
00221                                        rtmGetSampleHitPtr(S),
00222                                        rtmGetPerTaskSampleHitsPtr(S));
00223     rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
00224     for (i=FIRST_TID+1; i < NUMST; i++) {
00225         if (sampleHit[i] && GBLbuf.eventFlags[i]++) {
00226             GBLbuf.isrOverrun--;
00227             GBLbuf.overrunFlags[i]++;    /* Are we sampling too fast for */
00228             GBLbuf.stopExecutionFlag=1;  /*   sample time "i"?           */
00229             return;
00230         }
00231     }
00232 
00233 
00234     /*******************************************
00235      * Step the model for the base sample time *
00236      *******************************************/
00237     rtmiOutputs(rtmGetRTWRTModelMethodsInfo(S),FIRST_TID);
00238 
00239     rtExtModeUploadCheckTrigger(rtmGetNumSampleTimes(S));
00240     rtExtModeUpload(FIRST_TID,rtmGetTaskTime(S, FIRST_TID));
00241 
00242     GBLbuf.errmsg = rt_UpdateTXYLogVars(rtmGetRTWLogInfo(S),
00243                                         rtmGetTPtr(S));
00244     if (GBLbuf.errmsg != NULL) {
00245         GBLbuf.stopExecutionFlag = 1;
00246         return;
00247     }
00248 
00249     rtmiUpdate(rtmGetRTWRTModelMethodsInfo(S),FIRST_TID);
00250 
00251     if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
00252         rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(S));
00253     }
00254      else {
00255         rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(S),
00256                                      rtmGetTimingData(S),0);
00257     }
00258 
00259 #if FIRST_TID == 1
00260     rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(S),
00261                                  rtmGetTimingData(S),1);
00262 #endif
00263 
00264 
00265     /************************************************************************
00266      * Model step complete for base sample time, now it is okay to          *
00267      * re-interrupt this ISR.                                               *
00268      ************************************************************************/
00269     GBLbuf.isrOverrun--;
00270 
00271 
00272     /*********************************************
00273      * Step the model for any other sample times *
00274      *********************************************/
00275     for (i=FIRST_TID+1; i<NUMST; i++) {
00276         /* If task "i" is running, don't run any lower priority task */
00277         if (GBLbuf.overrunFlags[i]) return;
00278 
00279         if (GBLbuf.eventFlags[i]) {
00280             GBLbuf.overrunFlags[i]++;
00281 
00282             rtmiOutputs(rtmGetRTWRTModelMethodsInfo(S),i);
00283 
00284             rtExtModeUpload(i, rtmGetTaskTime(S,i));
00285 
00286             rtmiUpdate(rtmGetRTWRTModelMethodsInfo(S),i);
00287 
00288             rt_SimUpdateDiscreteTaskTime(rtmGetTPtr(S),
00289                                          rtmGetTimingData(S),i);
00290 
00291             /* Indicate task complete for sample time "i" */
00292             GBLbuf.overrunFlags[i]--;
00293             GBLbuf.eventFlags[i]--;
00294         }
00295     }
00296 
00297     rtExtModeCheckEndTrigger();
00298 
00299 } /* end rtOneStep */
00300 
00301 #endif /* MULTITASKING */
00302 
00303 /*===================*
00304  * Visible functions *
00305  *===================*/
00306 
00307 bool Model::initialize()
00308 {
00309   if (is_initialized) return false;
00310 
00311   /************************
00312    * Initialize the model *
00313    ************************/
00314   S = MODEL();
00315   if (S == NULL) {
00316       (void)fprintf(stderr,"Memory allocation error during model "
00317                     "registration");
00318 //      exit(EXIT_FAILURE);
00319       terminate();
00320       return false;
00321   }
00322 
00323   if (rtmGetErrorStatus(S) != NULL) {
00324       (void)fprintf(stderr,"Error during model registration: %s\n",
00325                     rtmGetErrorStatus(S));
00326       rtmiTerminate(rtmGetRTWRTModelMethodsInfo(S));
00327 //      exit(EXIT_FAILURE);
00328       terminate();
00329       return false;
00330   }
00331 
00332   if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
00333 
00334   rtmiInitializeSizes(rtmGetRTWRTModelMethodsInfo(S));
00335   rtmiInitializeSampleTimes(rtmGetRTWRTModelMethodsInfo(S));
00336 
00337   const char *status = 0;
00338   status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
00339                                   rtmGetStepSize(S),
00340                                   rtmGetSampleTimePtr(S),
00341                                   rtmGetOffsetTimePtr(S),
00342                                   rtmGetSampleHitPtr(S),
00343                                   rtmGetSampleTimeTaskIDPtr(S),
00344                                   rtmGetTStart(S),
00345                                   &rtmGetSimTimeStep(S),
00346                                   &rtmGetTimingData(S));
00347 
00348   if (status != NULL) {
00349       (void)fprintf(stderr,
00350               "Failed to initialize sample time engine: %s\n", status);
00351 //      exit(EXIT_FAILURE);
00352       terminate();
00353       return false;
00354   }
00355   rt_ODECreateIntegrationData(rtmGetRTWSolverInfo(S));
00356 #if NCSTATES > 0
00357   if(rtmGetErrorStatus(S) != NULL) {
00358       (void)fprintf(stderr, "Error creating integration data.\n");
00359       rt_ODEDestroyIntegrationData(rtmGetRTWSolverInfo(S));
00360       rtmiTerminate(rtmGetRTWRTModelMethodsInfo(S));
00361 //      exit(EXIT_FAILURE);
00362       terminate();
00363       return false;
00364   }
00365 #endif
00366 
00367   GBLbuf.errmsg = rt_StartDataLogging(rtmGetRTWLogInfo(S),
00368                                       rtmGetTFinal(S),
00369                                       rtmGetStepSize(S),
00370                                       &rtmGetErrorStatus(S));
00371   if (GBLbuf.errmsg != NULL) {
00372       (void)fprintf(stderr,"Error starting data logging: %s\n",GBLbuf.errmsg);
00373       terminate();
00374       return false;
00375   }
00376 
00377   rtExtModeCheckInit(rtmGetNumSampleTimes(S));
00378   rtExtModeWaitForStartPkt(rtmGetRTWExtModeInfo(S),
00379                            rtmGetNumSampleTimes(S),
00380                            (boolean_T *)&rtmGetStopRequested(S));
00381 
00382   (void)printf("\n** starting the model **\n");
00383 
00384   rtmiStart(rtmGetRTWRTModelMethodsInfo(S));
00385   if (rtmGetErrorStatus(S) != NULL) {
00386     GBLbuf.stopExecutionFlag = 1;
00387   }
00388 
00389   is_initialized = true;
00390   return true;
00391 }
00392 
00393 void Model::loop()
00394 {
00395   if (!is_initialized) return;
00396 
00397   /*************************************************************************
00398    * Execute the model.  You may attach rtOneStep to an ISR, if so replace *
00399    * the call to rtOneStep (below) with a call to a background task        *
00400    * application.                                                          *
00401    *************************************************************************/
00402   if (rtmGetTFinal(S) == RUN_FOREVER) {
00403       printf ("\n**May run forever. Model stop time set to infinity.**\n");
00404   }
00405 
00406   while(isRunning()) step();
00407 }
00408 
00409 
00410 bool Model::isRunning()
00411 {
00412   return (!GBLbuf.stopExecutionFlag &&
00413            (rtmGetTFinal(S) == RUN_FOREVER ||
00414             rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON));
00415 }
00416 
00417 void Model::step()
00418 {
00419   rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
00420                          rtmGetNumSampleTimes(S),
00421                          (boolean_T *)&rtmGetStopRequested(S));
00422 
00423   if (rtmGetStopRequested(S)) return;
00424   /* External mode */
00425   rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
00426           rtmGetNumSampleTimes(S),
00427           (boolean_T *)&rtmGetStopRequested(S));
00428 
00429   rt_OneStep(S);
00430 }
00431 
00432 void Model::terminate()
00433 {
00434   if (!is_initialized) return;
00435 
00436   if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
00437       /* external mode */
00438       rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
00439               rtmGetNumSampleTimes(S),
00440               (boolean_T *)&rtmGetStopRequested(S));
00441 
00442       /* Execute model last time step */
00443       rt_OneStep(S);
00444   }
00445 
00446   /********************
00447    * Cleanup and exit *
00448    ********************/
00449   rt_StopDataLogging(MATFILE,rtmGetRTWLogInfo(S));
00450   rtExtModeShutdown(rtmGetNumSampleTimes(S));
00451 
00452   if (GBLbuf.errmsg) {
00453       (void)fprintf(stderr,"%s\n",GBLbuf.errmsg);
00454 //      exit(EXIT_FAILURE);
00455   }
00456 
00457   if (GBLbuf.isrOverrun) {
00458       (void)fprintf(stderr,
00459                     "%s: ISR overrun - base sampling rate is too fast\n",
00460                     QUOTE(MODEL));
00461 //      exit(EXIT_FAILURE);
00462   }
00463 
00464   if (rtmGetErrorStatus(S) != NULL) {
00465       (void)fprintf(stderr,"%s\n", rtmGetErrorStatus(S));
00466 //      exit(EXIT_FAILURE);
00467   }
00468 #ifdef MULTITASKING
00469   else {
00470       int_T i;
00471       for (i=1; i<NUMST; i++) {
00472           if (GBLbuf.overrunFlags[i]) {
00473               (void)fprintf(stderr,
00474                       "%s ISR overrun - sampling rate is too fast for "
00475                       "sample time index %d\n", QUOTE(MODEL), i);
00476 //              exit(EXIT_FAILURE);
00477           }
00478       }
00479   }
00480 #endif
00481 
00482   /* timing data */
00483   rt_SimDestroyTimingEngine(rtmGetTimingData(S));
00484 #if NCSTATES > 0
00485   /* integration data */
00486   rt_ODEDestroyIntegrationData(rtmGetRTWSolverInfo(S));
00487 #endif
00488 
00489   rtmiTerminate(rtmGetRTWRTModelMethodsInfo(S));
00490 
00491   is_initialized = false;
00492 }
00493 
00494 } // namespace rosrtw


rosrtw
Author(s): Johannes Meyer
autogenerated on Fri Jul 25 2014 06:48:24