Rev 1403 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#include "tmwtypes.h"
#include "rtmodel.h"
#include "rt_sim.h"
#include "rt_logging.h"
#include "rt_nonfinite.h"
#include "ext_work.h"
#include "rt_nonfinite.h"
#include "kernel/kern.h"
/*=========*
* Defines *
*=========*/
#ifndef TRUE
#define FALSE (0)
#define TRUE (1)
#endif
#ifndef EXIT_FAILURE
#define EXIT_FAILURE 1
#endif
#ifndef EXIT_SUCCESS
#define EXIT_SUCCESS 0
#endif
#define QUOTE1(name) #name
#define QUOTE(name) QUOTE1(name) /* need to expand name */
#define RUN_FOREVER -1.0
#define EXPAND_CONCAT(name1,name2) name1 ## name2
#define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
#define RT_MODEL CONCAT(MODEL,_rtModel)
extern RT_MODEL
*MODEL
(void);
extern void MdlInitializeSizes
(void);
extern void MdlInitializeSampleTimes
(void);
extern void MdlStart
(void);
extern void MdlOutputs
(int_T tid
);
extern void MdlUpdate
(int_T tid
);
extern void MdlTerminate
(void);
real_T rtInf
;
real_T rtMinusInf
;
real_T rtNaN
;
#if NCSTATES > 0
extern void rt_ODECreateIntegrationData
(RTWSolverInfo
*si
);
extern void rt_ODEUpdateContinuousStates
(RTWSolverInfo
*si
);
# define rt_CreateIntegrationData(S) \
rt_ODECreateIntegrationData(rtmGetRTWSolverInfo(S));
# define rt_UpdateContinuousStates(S) \
rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(S));
# else
# define rt_CreateIntegrationData(S) \
rtsiSetSolverName(rtmGetRTWSolverInfo(S),"FixedStepDiscrete");
# define rt_UpdateContinuousStates(S) \
rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S)));
#endif
/*==================================*
* Global data local to this module *
*==================================*/
RT_MODEL
*S
;
const char *status
;
real_T finaltime
= -2.0;
volatile int simulation_run
;
static struct {
int_T stopExecutionFlag
;
int_T isrOverrun
;
int_T overrunFlags
[NUMST
];
const char_T
*errmsg
;
} GBLbuf
;
/* Function: rt_InitInfAndNaN ==================================================
* Abstract:
* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
* generated code. NaN is initialized as non-signaling. Assumes IEEE.
*/
void rt_InitInfAndNaN
(int_T realSize
)
{
short one
= 1;
enum {
LittleEndian
,
BigEndian
} machByteOrder
= (*((char *) &one
) == 1) ? LittleEndian
: BigEndian
;
switch (realSize
) {
case 4:
switch (machByteOrder
) {
case LittleEndian
: {
typedef struct {
uint32_T fraction
: 23;
uint32_T exponent
: 8;
uint32_T sign
: 1;
} LittleEndianIEEEDouble
;
(*(LittleEndianIEEEDouble
*)&rtInf
).
sign = 0;
(*(LittleEndianIEEEDouble
*)&rtInf
).
exponent = 0xFF;
(*(LittleEndianIEEEDouble
*)&rtInf
).
fraction = 0;
rtMinusInf
= rtInf
;
rtNaN
= rtInf
;
(*(LittleEndianIEEEDouble
*)&rtMinusInf
).
sign = 1;
(*(LittleEndianIEEEDouble
*)&rtNaN
).
fraction = 0x7FFFFF;
}
break;
case BigEndian
: {
typedef struct {
uint32_T sign
: 1;
uint32_T exponent
: 8;
uint32_T fraction
: 23;
} BigEndianIEEEDouble
;
(*(BigEndianIEEEDouble
*)&rtInf
).
sign = 0;
(*(BigEndianIEEEDouble
*)&rtInf
).
exponent = 0xFF;
(*(BigEndianIEEEDouble
*)&rtInf
).
fraction = 0;
rtMinusInf
= rtInf
;
rtNaN
= rtInf
;
(*(BigEndianIEEEDouble
*)&rtMinusInf
).
sign = 1;
(*(BigEndianIEEEDouble
*)&rtNaN
).
fraction = 0x7FFFFF;
}
break;
}
break;
case 8:
switch (machByteOrder
) {
case LittleEndian
: {
typedef struct {
struct {
uint32_T fraction2
;
} wordH
;
struct {
uint32_T fraction1
: 20;
uint32_T exponent
: 11;
uint32_T sign
: 1;
} wordL
;
} LittleEndianIEEEDouble
;
(*(LittleEndianIEEEDouble
*)&rtInf
).
wordL.
sign = 0;
(*(LittleEndianIEEEDouble
*)&rtInf
).
wordL.
exponent = 0x7FF;
(*(LittleEndianIEEEDouble
*)&rtInf
).
wordL.
fraction1 = 0;
(*(LittleEndianIEEEDouble
*)&rtInf
).
wordH.
fraction2 = 0;
rtMinusInf
= rtInf
;
(*(LittleEndianIEEEDouble
*)&rtMinusInf
).
wordL.
sign = 1;
(*(LittleEndianIEEEDouble
*)&rtNaN
).
wordL.
sign = 0;
(*(LittleEndianIEEEDouble
*)&rtNaN
).
wordL.
exponent = 0x7FF;
(*(LittleEndianIEEEDouble
*)&rtNaN
).
wordL.
fraction1 = 0xFFFFF;
(*(LittleEndianIEEEDouble
*)&rtNaN
).
wordH.
fraction2 = 0xFFFFFFFF;
}
break;
case BigEndian
: {
typedef struct {
struct {
uint32_T sign
: 1;
uint32_T exponent
: 11;
uint32_T fraction1
: 20;
} wordL
;
struct {
uint32_T fraction2
;
} wordH
;
} BigEndianIEEEDouble
;
(*(BigEndianIEEEDouble
*)&rtInf
).
wordL.
sign = 0;
(*(BigEndianIEEEDouble
*)&rtInf
).
wordL.
exponent = 0x7FF;
(*(BigEndianIEEEDouble
*)&rtInf
).
wordL.
fraction1 = 0;
(*(BigEndianIEEEDouble
*)&rtInf
).
wordH.
fraction2 = 0;
rtMinusInf
= rtInf
;
(*(BigEndianIEEEDouble
*)&rtMinusInf
).
wordL.
sign = 1;
(*(BigEndianIEEEDouble
*)&rtNaN
).
wordL.
sign = 0;
(*(BigEndianIEEEDouble
*)&rtNaN
).
wordL.
exponent = 0x7FF;
(*(BigEndianIEEEDouble
*)&rtNaN
).
wordL.
fraction1 = 0xFFFFF;
(*(BigEndianIEEEDouble
*)&rtNaN
).
wordH.
fraction2 = 0xFFFFFFFF;
}
break;
}
break;
default:
cprintf
("Error: Unable to initialize rtInf, rtMinusInf and rtNaN\n");
sys_end
();
break;
}
} /* end rt_InitInfAndNaN */
/* Function: rtOneStep ========================================================
*
* Abstract:
* Perform one step of the model. This function is modeled such that
* it could be called from an interrupt service routine (ISR) with minor
* modifications.
*/
static void rt_OneStep
(RT_MODEL
*S
)
{
real_T tnext
;
/***********************************************
* Check and see if base step time is too fast *
***********************************************/
if (GBLbuf.
isrOverrun++) {
GBLbuf.
stopExecutionFlag = 1;
return;
}
/***********************************************
* Check and see if error status has been set *
***********************************************/
if (rtmGetErrorStatus
(S
) != NULL
) {
GBLbuf.
stopExecutionFlag = 1;
return;
}
/* enable interrupts here */
rtExtModeOneStep
(rtmGetRTWExtModeInfo
(S
),
(boolean_T
*)&rtmGetStopRequested
(S
));
tnext
= rt_SimGetNextSampleHit
();
rtsiSetSolverStopTime
(rtmGetRTWSolverInfo
(S
),tnext
);
MdlOutputs
(0);
MdlUpdate
(0);
rt_SimUpdateDiscreteTaskSampleHits
(rtmGetNumSampleTimes
(S
),
rtmGetTimingData
(S
),
rtmGetSampleHitPtr
(S
),
rtmGetTPtr
(S
));
if (rtmGetSampleTime
(S
,0) == CONTINUOUS_SAMPLE_TIME
) {
rt_UpdateContinuousStates
(S
);
}
GBLbuf.
isrOverrun--;
rtExtModeCheckEndTrigger
();
} /* end rtOneStep */
void Init_RealTime_Workshop
() {
/****************************
* Initialize global memory *
****************************/
(void)memset(&GBLbuf
, 0, sizeof(GBLbuf
));
/************************
* Initialize the model *
************************/
rt_InitInfAndNaN
(sizeof(real_T
));
S
= MODEL
();
if (rtmGetErrorStatus
(S
) != NULL
) {
cprintf
("Error: Model registration: %s\n",
rtmGetErrorStatus
(S
));
sys_end
();
}
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
) {
cprintf
("Error: Failed to initialize sample time engine: %s\n", status
);
sys_end
();
}
rt_CreateIntegrationData
(S
);
rtExtModeCheckInit
();
rtExtModeWaitForStartMsg
(rtmGetRTWExtModeInfo
(S
),
(boolean_T
*)&rtmGetStopRequested
(S
));
cprintf
("\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
) {
cprintf
("\n**May run forever. Model stop time set to infinity.**\n");
}
}
void Close_RealTime_Workshop
() {
/********************
* Cleanup and exit *
********************/
rtExtModeShutdown
();
if (GBLbuf.
errmsg) {
cprintf
("Error: %s\n",GBLbuf.
errmsg);
sys_end
();
}
if (GBLbuf.
isrOverrun) {
cprintf
("Error: %s: ISR overrun - base sampling rate is too fast\n",QUOTE
(MODEL
));
sys_end
();
}
if (rtmGetErrorStatus
(S
) != NULL
) {
cprintf
("Error: %s\n", rtmGetErrorStatus
(S
));
sys_end
();
}
MdlTerminate
();
}
TASK RTW_body
(void *arg
) {
simulation_run
= 1;
while (!GBLbuf.
stopExecutionFlag &&
(rtmGetTFinal
(S
) == RUN_FOREVER
||
rtmGetTFinal
(S
)-rtmGetT
(S
) > rtmGetT
(S
)*DBL_EPSILON
)) {
rtExtModePauseIfNeeded
(rtmGetRTWExtModeInfo
(S
),
(boolean_T
*)&rtmGetStopRequested
(S
));
cprintf
(".(%dus out = %lf)",(int)(rtmGetT
(S
)*1000000),*(real_T
*)(rtmGetY
(S
)));
if (rtmGetStopRequested
(S
)) break;
rt_OneStep
(S
);
task_endcycle
();
}
if (!GBLbuf.
stopExecutionFlag && !rtmGetStopRequested
(S
)) {
/* Execute model last time step */
rt_OneStep
(S
);
}
simulation_run
= 0;
return NULL
;
}
TASK INPUT_body
(void *arg
) {
int input_ports
= rtmGetNumU
(S
);
real_T
*input
;
cprintf
("Number of inputs = %d\n",input_ports
);
input
= (real_T
*)rtmGetU
(S
);
while(1) {
input
[0] = 1;
input
[1] = 1;
task_endcycle
();
}
return NULL
;
}
int main
() {
HARD_TASK_MODEL RTW_task
,INPUT_task
;
PID RTW_pid
,INPUT_pid
;
Init_RealTime_Workshop
();
hard_task_default_model
(RTW_task
);
hard_task_def_mit
(RTW_task
,rtmGetStepSize
(S
) * 1000000);
hard_task_def_wcet
(RTW_task
,10000);
hard_task_def_usemath
(RTW_task
);
hard_task_def_ctrl_jet
(RTW_task
);
RTW_pid
= task_create
("RTW",RTW_body
,&RTW_task
,NULL
);
if (RTW_pid
== NIL
) {
cprintf
("Error: Cannot create RealTime Workshop Task\n");
sys_end
();
}
hard_task_default_model
(INPUT_task
);
hard_task_def_mit
(INPUT_task
,100000);
hard_task_def_wcet
(INPUT_task
,10000);
hard_task_def_usemath
(INPUT_task
);
hard_task_def_ctrl_jet
(INPUT_task
);
INPUT_pid
= task_create
("INPUT",INPUT_body
,&INPUT_task
,NULL
);
if (INPUT_pid
== NIL
) {
cprintf
("Error: Cannot create RealTime Workshop Task\n");
sys_end
();
}
task_activate
(INPUT_pid
);
task_activate
(RTW_pid
);
while(simulation_run
);
Close_RealTime_Workshop
();
sys_end
();
return 0;
}