/**
* This function is called when the control program is loaded to zenom.
* Use this function to register control parameters, to register log variables
* and to initialize control parameters.
*
* @return Return non-zero to indicate an error.
*/
int ZenomMatlab::initialize()
{
int paramIdx, sigIdx;
int nBlockParams, nSignals;
const char* status;
// İsmi gözükmeyen parametrelerin sayısını tutar.
unknown_param_counter = 0;
/* Here is where Q8 dirver is loaded to kernel space */
//system(STR(sudo insmod DQ8));
fd = rt_dev_open(DEV_NAME, O_RDWR);
if(fd < 0)
fprintf(stderr, "target:Q8 device open error!\n");
init_xenomai();
rtM = MODEL();
if (rtmGetErrorStatus(rtM) != NULL) {
(void)fprintf(stderr,"Error during model registration: %s\n",
rtmGetErrorStatus(rtM));
exit(EXIT_FAILURE);
}
MdlInitializeSizes();
MdlInitializeSampleTimes();
status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(rtM),
rtmGetStepSize(rtM),
rtmGetSampleTimePtr(rtM),
rtmGetOffsetTimePtr(rtM),
rtmGetSampleHitPtr(rtM),
rtmGetSampleTimeTaskIDPtr(rtM),
rtmGetTStart(rtM),
&rtmGetSimTimeStep(rtM),
&rtmGetTimingData(rtM));
if (status != NULL) {
(void)fprintf(stderr, "Failed to initialize sample time engine: %s\n", status);
exit(EXIT_FAILURE);
}
rt_CreateIntegrationData(rtM);
mmi = &(rtmGetDataMapInfo(rtM).mmi);
if (mmi!=NULL){
//exception here
}
bp = rtwCAPI_GetBlockParameters(mmi);
sig = rtwCAPI_GetSignals(mmi);
nBlockParams = rtwCAPI_GetNumBlockParameters(mmi);
nSignals = rtwCAPI_GetNumSignals(mmi);
xrtpi = new XrtTargetParamInfo[nBlockParams];
xtsi = new XrtTargetSignalInfo[nSignals];
/** Get parameter and register */
Xrt_GetParameterInfo(mmi);
Xrt_GetSignalInfo(mmi);
/**************** ZENOM PART ***************/
for(paramIdx = 0; paramIdx < rtwCAPI_GetNumBlockParameters(mmi); paramIdx++){
std::string paramName(xrtpi[paramIdx].paramName);
std::string blockName(xrtpi[paramIdx].blockName);
if(paramName.empty()){
paramName = std::string("unknownBlock");
}
for (std::size_t found = paramName.find_first_of(" ");
found != std::string::npos ;
found = paramName.find_first_of(" "))
{
paramName.erase (found);
}
for (std::size_t found = blockName.find_first_of(" ");
found != std::string::npos ;
found = blockName.find_first_of(" "))
{
blockName.erase (found);
}
paramName = blockName + "-" + paramName;
registerControlVariable( xrtpi[paramIdx].dataValue,
paramName,
xrtpi[paramIdx].numRows,
xrtpi[paramIdx].numColumns );
}
for (sigIdx = 0; sigIdx < rtwCAPI_GetNumSignals(mmi); ++sigIdx){
std::string sigName(xtsi[sigIdx].signalName);
if(sigName.empty()){
sigName = std::string("unknownSignal");
sigName += unknown_param_counter;
}
for (std::size_t found = sigName.find_first_of(" ");
found != std::string::npos ;
found = sigName.find_first_of(" "))
{
sigName.erase (found);
}
registerLogVariable(xtsi[sigIdx].dataValue,
sigName,
xtsi[sigIdx].numRows,
xtsi[sigIdx].numColumns);
}
setFrequency( (double)rtmGetStepSize(rtM) );
setDuration ( (double)rtmGetTFinal(rtM) );
fprintf(stderr, "init done!");
return 0;
}
/* Function: main ==============================================================
*
* Execute model on a generic target such as a workstation.
*/
int_T main(int_T argc, char_T *argv[])
{
SimStruct *S = NULL;
boolean_T calledMdlStart = FALSE;
boolean_T dataLoggingActive = FALSE;
boolean_T initializedExtMode = FALSE;
const char *result;
const char *program;
time_t now;
int matFileFormat;
const char *errorPrefix = NULL;
void *parforSchedulerInit = NULL;
program = argv[0];
gblInstalledSigHandlers = FALSE;
/* No re-defining of data types allowed. */
if ((sizeof(real_T) != 8) ||
(sizeof(real32_T) != 4) ||
(sizeof(int8_T) != 1) ||
(sizeof(uint8_T) != 1) ||
(sizeof(int16_T) != 2) ||
(sizeof(uint16_T) != 2) ||
(sizeof(int32_T) != 4) ||
(sizeof(uint32_T) != 4) ||
(sizeof(boolean_T)!= 1)) {
ERROR_EXIT("Error: %s\n", "Re-defining data types such as REAL_T is not supported by RSIM");
}
rt_InitInfAndNaN(sizeof(real_T));
/* Parse arguments */
gblErrorStatus = ParseArgs(argc, argv);
ERROR_EXIT("Error parsing input arguments: %s\n", gblErrorStatus);
/* Initialize the model */
S = raccel_register_model();
ERROR_EXIT("Error during model registration: %s\n", ssGetErrorStatus(S));
ssClearFirstInitCondCalled(S);
/* Override StopTime */
if (gblFinalTimeChanged) ssSetTFinal(S,gblFinalTime);
MdlInitializeSizes();
MdlInitializeSampleTimes();
/* We don't have GOTO_EXIT_IF_ERROR here as engine is not initialized
via rsimInitializeEngine */
rt_RapidReadMatFileAndUpdateParams(S);
ERROR_EXIT("Error reading parameter data from mat-file: %s\n",
ssGetErrorStatus(S));
/* load solver options */
rsimLoadSolverOpts(S);
ERROR_EXIT("Error loading solver options: %s\n", ssGetErrorStatus(S));
# if defined(DEBUG_SOLVER)
rsimEnableDebugOutput(sizeof(struct SimStruct_tag),
sizeof(struct _ssMdlInfo));
# endif
#ifdef RACCEL_PARALLEL_FOREACH
parforSchedulerInit = rt_ParallelForEachInitScheduler(S, RACCEL_PARFOREACH_NUM_THREADS, RACCEL_NUM_PARFOREACH_SS);
#endif
rsimInitializeEngine(S);
ERROR_EXIT("Error initializing RSIM engine: %s\n", ssGetErrorStatus(S));
/* initialize external model */
if (gblExtModeEnabled) {
rtExtModeCheckInit(ssGetNumSampleTimes(S));
initializedExtMode = TRUE;
}
raccel_setup_MMIStateLog(S);
if (ssIsVariableStepSolver(S)) {
(void)rt_StartDataLoggingWithStartTime(ssGetRTWLogInfo(S),
ssGetTStart(S),
ssGetTFinal(S),
0.0,
&ssGetErrorStatus(S));
} else {
(void)rt_StartDataLoggingWithStartTime(ssGetRTWLogInfo(S),
ssGetTStart(S),
ssGetTFinal(S),
ssGetStepSize(S),
&ssGetErrorStatus(S));
}
GOTO_EXIT_IF_ERROR("Error starting data logging: %s\n",
ssGetErrorStatus(S));
dataLoggingActive = TRUE;
if (gblExtModeEnabled) {
/* If -w flag is specified wait here for connect signal from host */
rtExtModeWaitForStartPkt(ssGetRTWExtModeInfo(S),
ssGetNumSampleTimes(S),
(boolean_T *)&ssGetStopRequested(S));
if (ssGetStopRequested(S)) goto EXIT_POINT;
}
/* Start the model */
now = time(NULL);
if(gblVerboseFlag) {
(void)printf("\n** Starting model @ %s", ctime(&now));
}
/* Enable logging in the MdlStart method */
ssSetLogOutput(S,TRUE);
/* Enable -i option to load inport data file */
result = rt_RapidReadInportsMatFile(gblInportFileName, &matFileFormat);
if (result!= NULL) {
ssSetErrorStatus(S,result);
GOTO_EXIT_IF_ERROR("Error starting model: %s\n", ssGetErrorStatus(S));
}
MdlStart();
ssSetLogOutput(S,FALSE);
calledMdlStart = TRUE;
GOTO_EXIT_IF_ERROR("Error starting model: %s\n", ssGetErrorStatus(S));
result = rt_RapidCheckRemappings();
ssSetErrorStatus(S,result);
GOTO_EXIT_IF_ERROR("Error: %s\n", ssGetErrorStatus(S));
/* Create solver data */
rsimCreateSolverData(S, gblSlvrJacPatternFileName);
GOTO_EXIT_IF_ERROR("Error creating solver data: %s\n", ssGetErrorStatus(S));
ssSetFirstInitCondCalled(S);
/*********************
* Execute the model *
*********************/
/* Install Signal and Run time limit handlers */
rsimInstallAllHandlers(S,WriteResultsToMatFile,gblTimeLimit);
gblInstalledSigHandlers = TRUE;
GOTO_EXIT_IF_ERROR("Error: %s\n", ssGetErrorStatus(S));
while ( ((ssGetTFinal(S)-ssGetT(S)) > (fabs(ssGetT(S))*DBL_EPSILON)) ) {
if (gblExtModeEnabled) {
rtExtModePauseIfNeeded(ssGetRTWExtModeInfo(S),
ssGetNumSampleTimes(S),
(boolean_T *)&ssGetStopRequested(S));
}
if (ssGetStopRequested(S)) break;
if (gbl_raccel_isMultitasking) {
rsimOneStepMT(S);
} else {
rsimOneStepST(S);
}
if (ssGetErrorStatus(S)) break;
}
if (ssGetErrorStatus(S) == NULL && !ssGetStopRequested(S)) {
/* Do a major step at the final time */
if (gbl_raccel_isMultitasking) {
rsimOneStepMT(S);
} else {
rsimOutputLogUpdate(S);
}
}
EXIT_POINT:
/********************
* Cleanup and exit *
********************/
if (ssGetErrorStatus(S) != NULL) {
if (errorPrefix) {
(void)fprintf(stderr, errorPrefix, ssGetErrorStatus(S));
} else {
(void)fprintf(stderr, "Error: %s\n", ssGetErrorStatus(S));
}
}
if (gblInstalledSigHandlers) {
rsimUninstallNonfatalHandlers();
gblInstalledSigHandlers = FALSE;
}
if (dataLoggingActive){
WriteResultsToMatFile(S);
}
rsimTerminateEngine(S,0);
if (initializedExtMode) {
rtExtModeShutdown(ssGetNumSampleTimes(S));
}
if (calledMdlStart) {
MdlTerminate();
}
#ifdef RACCEL_PARALLEL_FOREACH
rt_ParallelForEachClearScheduler(parforSchedulerInit);
#endif
rt_RapidFreeGbls(matFileFormat);
return ssGetErrorStatus(S) ? EXIT_FAILURE : EXIT_SUCCESS;
} /* end main */
static void initialize_sample_times(RT_MODEL * S)
{
MdlInitializeSampleTimes();
}
/* Function: main =============================================================
*
* Abstract:
* Execute model on a generic target such as a workstation.
*/
int_T main(int_T argc, const char_T *argv[])
{
RT_MODEL *S;
const char *status;
real_T finaltime = -2.0;
int_T oldStyle_argc;
const char_T *oldStyle_argv[5];
/******************************
* MathError Handling for BC++ *
******************************/
#ifdef BORLAND
signal(SIGFPE, (fptr)divideByZero);
#endif
/*******************
* Parse arguments *
*******************/
if ((argc > 1) && (argv[1][0] != '-')) {
/* old style */
if ( argc > 3 ) {
displayUsage();
exit(EXIT_FAILURE);
}
oldStyle_argc = 1;
oldStyle_argv[0] = argv[0];
if (argc >= 2) {
oldStyle_argc = 3;
oldStyle_argv[1] = "-tf";
oldStyle_argv[2] = argv[1];
}
if (argc == 3) {
oldStyle_argc = 5;
oldStyle_argv[3] = "-port";
oldStyle_argv[4] = argv[2];
}
argc = oldStyle_argc;
argv = oldStyle_argv;
}
{
/* new style: */
double tmpDouble;
char_T tmpStr2[200];
int_T count = 1;
int_T parseError = FALSE;
/*
* Parse the standard RTW parameters. Let all unrecognized parameters
* pass through to external mode for parsing. NULL out all args handled
* so that the external mode parsing can ignore them.
*/
while(count < argc) {
const char_T *option = argv[count++];
/* final time */
if ((strcmp(option, "-tf") == 0) && (count != argc)) {
const char_T *tfStr = argv[count++];
sscanf(tfStr, "%200s", tmpStr2);
if (strcmp(tmpStr2, "inf") == 0) {
tmpDouble = RUN_FOREVER;
} else {
char_T tmpstr[2];
if ( (sscanf(tmpStr2,"%lf%1s", &tmpDouble, tmpstr) != 1) ||
(tmpDouble < 0.0) ) {
(void)printf("finaltime must be a positive, real value or inf\n");
parseError = TRUE;
break;
}
}
finaltime = (real_T) tmpDouble;
argv[count-2] = NULL;
argv[count-1] = NULL;
}
}
if (parseError) {
(void)printf("\nUsage: %s -option1 val1 -option2 val2 -option3 "
"...\n\n", QUOTE(MODEL));
(void)printf("\t-tf 20 - sets final time to 20 seconds\n");
exit(EXIT_FAILURE);
}
rtExtModeParseArgs(argc, argv, NULL);
/*
* Check for unprocessed ("unhandled") args.
*/
{
int i;
for (i=1; i<argc; i++) {
if (argv[i] != NULL) {
printf("Unexpected command line argument: %s\n",argv[i]);
exit(EXIT_FAILURE);
}
}
}
}
/****************************
* Initialize global memory *
****************************/
(void)memset(&GBLbuf, 0, sizeof(GBLbuf));
/************************
* Initialize the model *
************************/
rt_InitInfAndNaN(sizeof(real_T));
S = MODEL();
if (rtmGetErrorStatus(S) != NULL) {
(void)fprintf(stderr,"Error during model registration: %s\n",
rtmGetErrorStatus(S));
exit(EXIT_FAILURE);
}
if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
MdlInitializeSizes();
MdlInitializeSampleTimes();
status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
rtmGetStepSize(S),
rtmGetSampleTimePtr(S),
rtmGetOffsetTimePtr(S),
rtmGetSampleHitPtr(S),
rtmGetSampleTimeTaskIDPtr(S),
rtmGetTStart(S),
&rtmGetSimTimeStep(S),
&rtmGetTimingData(S));
if (status != NULL) {
(void)fprintf(stderr,
"Failed to initialize sample time engine: %s\n", status);
exit(EXIT_FAILURE);
}
rt_CreateIntegrationData(S);
/*
#ifdef UseMMIDataLogging
rt_FillStateSigInfoFromMMI(rtmGetRTWLogInfo(S),&rtmGetErrorStatus(S));
rt_FillSigLogInfoFromMMI(rtmGetRTWLogInfo(S),&rtmGetErrorStatus(S));
#endif*/
/* GBLbuf.errmsg = rt_StartDataLogging(rtmGetRTWLogInfo(S),
rtmGetTFinal(S),
rtmGetStepSize(S),
&rtmGetErrorStatus(S));
if (GBLbuf.errmsg != NULL) {
(void)fprintf(stderr,"Error starting data logging: %s\n",GBLbuf.errmsg);
return(EXIT_FAILURE);
}*//*removed datalogging*/
rtExtModeCheckInit(rtmGetNumSampleTimes(S));
rtExtModeWaitForStartPkt(rtmGetRTWExtModeInfo(S),
rtmGetNumSampleTimes(S),
(boolean_T *)&rtmGetStopRequested(S));
(void)printf("\n** starting the model **\n");
MdlStart();
if (rtmGetErrorStatus(S) != NULL) {
GBLbuf.stopExecutionFlag = 1;
}
/*************************************************************************
* Execute the model. You may attach rtOneStep to an ISR, if so replace *
* the call to rtOneStep (below) with a call to a background task *
* application. *
*************************************************************************/
if (rtmGetTFinal(S) == RUN_FOREVER) {
printf ("\n**May run forever. Model stop time set to infinity.**\n");
}
stepTime=(FPS*CLOCKS_PER_SEC)/1000;
nextStart = clock();
nextStart+=stepTime;
while (!GBLbuf.stopExecutionFlag &&
(rtmGetTFinal(S) == RUN_FOREVER ||
rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
rtmGetNumSampleTimes(S),
(boolean_T *)&rtmGetStopRequested(S));
if( clock() >= nextStart)
{
if( stepTime > 0)
{
printf("***Execution slower than requested rate: Actual speed =%d ms***\n",(1000*(stepTime+clock()-nextStart))/CLOCKS_PER_SEC);
nextStart=clock();
}
}
while (clock() < nextStart) {}
nextStart+=stepTime;
if (rtmGetStopRequested(S)) break;
rt_OneStep(S);
}
if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
/* Execute model last time step */
rt_OneStep(S);
}
/********************
* Cleanup and exit *
********************/
/*
#ifdef UseMMIDataLogging
rt_CleanUpForStateLogWithMMI(rtmGetRTWLogInfo(S));
rt_CleanUpForSigLogWithMMI(rtmGetRTWLogInfo(S));
#endif
rt_StopDataLogging(MATFILE,rtmGetRTWLogInfo(S));*/
rtExtModeShutdown(rtmGetNumSampleTimes(S));
if (GBLbuf.errmsg) {
(void)fprintf(stderr,"%s\n",GBLbuf.errmsg);
exit(EXIT_FAILURE);
}
if (rtmGetErrorStatus(S) != NULL) {
(void)fprintf(stderr,"ErrorStatus set: \"%s\"\n", rtmGetErrorStatus(S));
exit(EXIT_FAILURE);
}
if (GBLbuf.isrOverrun) {
(void)fprintf(stderr,
"%s: ISR overrun - base sampling rate is too fast\n",
QUOTE(MODEL));
exit(EXIT_FAILURE);
}
#ifdef MULTITASKING
else {
int_T i;
for (i=1; i<NUMST; i++) {
if (GBLbuf.overrunFlags[i]) {
(void)fprintf(stderr,
"%s ISR overrun - sampling rate is too fast for "
"sample time index %d\n", QUOTE(MODEL), i);
exit(EXIT_FAILURE);
}
}
}
#endif
MdlTerminate();
return(EXIT_SUCCESS);
} /* end main */