WebSVN - shark - Blame - Rev 1368 - /demos/trunk/rtw/rtw.c

Rev	Author	Line No.	Line
1342	giacomo	1	#include "tmwtypes.h"
		2	#include "rtmodel.h"
		3	#include "rt_sim.h"
		4	#include "rt_logging.h"
		5	#include "rt_nonfinite.h"
		6
		7	#include "ext_work.h"
		8	#include "rt_nonfinite.h"
		9
		10	#include "kernel/kern.h"
		11
		12	/=========
		13	* Defines *
		14	=========/
		15
		16	#ifndef TRUE
		17	#define FALSE (0)
		18	#define TRUE (1)
		19	#endif
		20
		21	#ifndef EXIT_FAILURE
		22	#define EXIT_FAILURE 1
		23	#endif
		24	#ifndef EXIT_SUCCESS
		25	#define EXIT_SUCCESS 0
		26	#endif
		27
		28	#define QUOTE1(name) #name
		29	#define QUOTE(name) QUOTE1(name) /* need to expand name */
		30
		31	#define RUN_FOREVER -1.0
		32
		33	#define EXPAND_CONCAT(name1,name2) name1 ## name2
		34	#define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
		35	#define RT_MODEL CONCAT(MODEL,_rtModel)
		36
		37	extern RT_MODEL *MODEL(void);
		38
		39	extern void MdlInitializeSizes(void);
		40	extern void MdlInitializeSampleTimes(void);
		41	extern void MdlStart(void);
		42	extern void MdlOutputs(int_T tid);
		43	extern void MdlUpdate(int_T tid);
		44	extern void MdlTerminate(void);
		45
		46	real_T rtInf;
		47	real_T rtMinusInf;
		48	real_T rtNaN;
		49
		50	# define rt_CreateIntegrationData(S) \
		51	rtsiSetSolverName(rtmGetRTWSolverInfo(S),"FixedStepDiscrete");
		52	# define rt_UpdateContinuousStates(S) \
		53	rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S)));
		54
		55	/==================================
		56	* Global data local to this module *
		57	==================================/
1368	giacomo	58
		59	RT_MODEL *S;
		60	const char *status;
		61	real_T finaltime = -2.0;
		62	volatile int simulation_run;
1342	giacomo	63	static struct {
		64	int_T stopExecutionFlag;
		65	int_T isrOverrun;
		66	int_T overrunFlags[NUMST];
		67	const char_T *errmsg;
		68	} GBLbuf;
		69
		70	/* Function: rt_InitInfAndNaN ==================================================
		71	* Abstract:
		72	* Initialize the rtInf, rtMinusInf, and rtNaN needed by the
		73	* generated code. NaN is initialized as non-signaling. Assumes IEEE.
		74	*/
		75	void rt_InitInfAndNaN(int_T realSize)
		76	{
		77	short one = 1;
		78	enum {
		79	LittleEndian,
		80	BigEndian
		81	} machByteOrder = (((char ) &one) == 1) ? LittleEndian : BigEndian;
		82
		83	switch (realSize) {
		84	case 4:
		85	switch (machByteOrder) {
		86	case LittleEndian: {
		87	typedef struct {
		88	uint32_T fraction : 23;
		89	uint32_T exponent : 8;
		90	uint32_T sign : 1;
		91	} LittleEndianIEEEDouble;
		92
		93	((LittleEndianIEEEDouble)&rtInf).sign = 0;
		94	((LittleEndianIEEEDouble)&rtInf).exponent = 0xFF;
		95	((LittleEndianIEEEDouble)&rtInf).fraction = 0;
		96	rtMinusInf = rtInf;
		97	rtNaN = rtInf;
		98	((LittleEndianIEEEDouble)&rtMinusInf).sign = 1;
		99	((LittleEndianIEEEDouble)&rtNaN).fraction = 0x7FFFFF;
		100	}
		101	break;
		102	case BigEndian: {
		103	typedef struct {
		104	uint32_T sign : 1;
		105	uint32_T exponent : 8;
		106	uint32_T fraction : 23;
		107	} BigEndianIEEEDouble;
		108
		109	((BigEndianIEEEDouble)&rtInf).sign = 0;
		110	((BigEndianIEEEDouble)&rtInf).exponent = 0xFF;
		111	((BigEndianIEEEDouble)&rtInf).fraction = 0;
		112	rtMinusInf = rtInf;
		113	rtNaN = rtInf;
		114	((BigEndianIEEEDouble)&rtMinusInf).sign = 1;
		115	((BigEndianIEEEDouble)&rtNaN).fraction = 0x7FFFFF;
		116	}
		117	break;
		118	}
		119	break;
		120
		121	case 8:
		122	switch (machByteOrder) {
		123	case LittleEndian: {
		124	typedef struct {
		125	struct {
		126	uint32_T fraction2;
		127	} wordH;
		128	struct {
		129	uint32_T fraction1 : 20;
		130	uint32_T exponent : 11;
		131	uint32_T sign : 1;
		132	} wordL;
		133	} LittleEndianIEEEDouble;
		134
		135	((LittleEndianIEEEDouble)&rtInf).wordL.sign = 0;
		136	((LittleEndianIEEEDouble)&rtInf).wordL.exponent = 0x7FF;
		137	((LittleEndianIEEEDouble)&rtInf).wordL.fraction1 = 0;
		138	((LittleEndianIEEEDouble)&rtInf).wordH.fraction2 = 0;
		139
		140	rtMinusInf = rtInf;
		141	((LittleEndianIEEEDouble)&rtMinusInf).wordL.sign = 1;
		142	((LittleEndianIEEEDouble)&rtNaN).wordL.sign = 0;
		143	((LittleEndianIEEEDouble)&rtNaN).wordL.exponent = 0x7FF;
		144	((LittleEndianIEEEDouble)&rtNaN).wordL.fraction1 = 0xFFFFF;
		145	((LittleEndianIEEEDouble)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
		146	}
		147	break;
		148	case BigEndian: {
		149	typedef struct {
		150	struct {
		151	uint32_T sign : 1;
		152	uint32_T exponent : 11;
		153	uint32_T fraction1 : 20;
		154	} wordL;
		155	struct {
		156	uint32_T fraction2;
		157	} wordH;
		158	} BigEndianIEEEDouble;
		159
		160	((BigEndianIEEEDouble)&rtInf).wordL.sign = 0;
		161	((BigEndianIEEEDouble)&rtInf).wordL.exponent = 0x7FF;
		162	((BigEndianIEEEDouble)&rtInf).wordL.fraction1 = 0;
		163	((BigEndianIEEEDouble)&rtInf).wordH.fraction2 = 0;
		164
		165	rtMinusInf = rtInf;
		166	((BigEndianIEEEDouble)&rtMinusInf).wordL.sign = 1;
		167	((BigEndianIEEEDouble)&rtNaN).wordL.sign = 0;
		168	((BigEndianIEEEDouble)&rtNaN).wordL.exponent = 0x7FF;
		169	((BigEndianIEEEDouble)&rtNaN).wordL.fraction1 = 0xFFFFF;
		170	((BigEndianIEEEDouble)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
		171	}
		172	break;
		173	}
		174	break;
		175	default:
		176	cprintf("Unable to initialize rtInf, rtMinusInf and rtNaN\n");
		177	sys_end();
		178	break;
		179	}
		180
		181	} /* end rt_InitInfAndNaN */
		182
		183	/* Function: rtOneStep ========================================================
		184	*
		185	* Abstract:
		186	* Perform one step of the model. This function is modeled such that
		187	* it could be called from an interrupt service routine (ISR) with minor
		188	* modifications.
		189	*/
		190	static void rt_OneStep(RT_MODEL *S)
		191	{
		192	real_T tnext;
		193
		194	/***********************************************
		195	* Check and see if base step time is too fast *
		196	***********************************************/
		197
		198	if (GBLbuf.isrOverrun++) {
		199	GBLbuf.stopExecutionFlag = 1;
		200	return;
		201	}
		202
		203	/***********************************************
		204	* Check and see if error status has been set *
		205	***********************************************/
		206
		207	if (rtmGetErrorStatus(S) != NULL) {
		208	GBLbuf.stopExecutionFlag = 1;
		209	return;
		210	}
		211
		212	/* enable interrupts here */
		213
		214	rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
		215	(boolean_T *)&rtmGetStopRequested(S));
		216
		217	tnext = rt_SimGetNextSampleHit();
		218	rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
		219
		220	MdlOutputs(0);
		221
		222	MdlUpdate(0);
		223	rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(S),
		224	rtmGetTimingData(S),
		225	rtmGetSampleHitPtr(S),
		226	rtmGetTPtr(S));
		227
		228	if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
		229	rt_UpdateContinuousStates(S);
		230	}
		231
		232	GBLbuf.isrOverrun--;
		233
		234	rtExtModeCheckEndTrigger();
		235
		236	} /* end rtOneStep */
		237
1368	giacomo	238	void Init_RealTime_Workshop() {
1342	giacomo	239
1368	giacomo	240	/****************************
1342	giacomo	241	* Initialize global memory *
		242	****************************/
1368	giacomo	243
1342	giacomo	244	(void)memset(&GBLbuf, 0, sizeof(GBLbuf));
		245
1368	giacomo	246	/************************
1342	giacomo	247	* Initialize the model *
		248	************************/
1368	giacomo	249
1342	giacomo	250	rt_InitInfAndNaN(sizeof(real_T));
		251
		252	S = MODEL();
		253	if (rtmGetErrorStatus(S) != NULL) {
		254	cprintf("Error during model registration: %s\n",
		255	rtmGetErrorStatus(S));
		256	sys_end();
		257	}
1368	giacomo	258
1342	giacomo	259	if (finaltime >= 0.0 \|\| finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
		260
		261	MdlInitializeSizes();
		262	MdlInitializeSampleTimes();
		263
		264	status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
		265	rtmGetStepSize(S),
		266	rtmGetSampleTimePtr(S),
		267	rtmGetOffsetTimePtr(S),
		268	rtmGetSampleHitPtr(S),
		269	rtmGetSampleTimeTaskIDPtr(S),
		270	rtmGetTStart(S),
		271	&rtmGetSimTimeStep(S),
		272	&rtmGetTimingData(S));
		273
		274	if (status != NULL) {
		275	cprintf("Failed to initialize sample time engine: %s\n", status);
		276	sys_end();
		277	}
1368	giacomo	278
1342	giacomo	279	rt_CreateIntegrationData(S);
		280
		281	rtExtModeCheckInit();
		282	rtExtModeWaitForStartMsg(rtmGetRTWExtModeInfo(S),
		283	(boolean_T *)&rtmGetStopRequested(S));
		284
1368	giacomo	285	cprintf("\n Starting the model \n");
1342	giacomo	286
		287	MdlStart();
		288	if (rtmGetErrorStatus(S) != NULL) {
		289	GBLbuf.stopExecutionFlag = 1;
		290	}
		291
		292	/*************************************************************************
		293	* Execute the model. You may attach rtOneStep to an ISR, if so replace *
		294	* the call to rtOneStep (below) with a call to a background task *
		295	* application. *
		296	*************************************************************************/
		297
		298	if (rtmGetTFinal(S) == RUN_FOREVER) {
		299	cprintf("\nMay run forever. Model stop time set to infinity.\n");
		300	}
		301
1368	giacomo	302	}
1342	giacomo	303
1368	giacomo	304	void Close_RealTime_Workshop() {
1342	giacomo	305
		306	/********************
		307	* Cleanup and exit *
		308	********************/
		309
		310	rtExtModeShutdown();
		311
		312	if (GBLbuf.errmsg) {
		313	cprintf("%s\n",GBLbuf.errmsg);
		314	sys_end();
		315	}
		316
		317	if (GBLbuf.isrOverrun) {
		318	cprintf("%s: ISR overrun - base sampling rate is too fast\n",QUOTE(MODEL));
		319	sys_end();
		320	}
		321
		322	if (rtmGetErrorStatus(S) != NULL) {
		323	cprintf("%s\n", rtmGetErrorStatus(S));
		324	sys_end();
		325	}
		326
1368	giacomo	327	MdlTerminate();
1344	giacomo	328
1368	giacomo	329	}
		330
		331	TASK RTW_body(void *arg) {
		332
		333	simulation_run = 1;
		334
		335	while (!GBLbuf.stopExecutionFlag &&
		336	(rtmGetTFinal(S) == RUN_FOREVER \|\|
		337	rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
		338
		339	rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
		340	(boolean_T *)&rtmGetStopRequested(S));
		341
		342	cprintf(".(%dus)",(int)(rtmGetT(S)*1000000));
		343
		344	if (rtmGetStopRequested(S)) break;
		345	rt_OneStep(S);
		346
		347	task_endcycle();
		348
		349	}
		350
		351	if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
		352	/* Execute model last time step */
		353	rt_OneStep(S);
		354	}
		355
		356	simulation_run = 0;
		357
		358	return NULL;
		359
		360	}
		361
		362	int main() {
		363
		364	HARD_TASK_MODEL RTW_task;
		365	PID RTW_pid;
		366
		367	Init_RealTime_Workshop();
		368
		369	hard_task_default_model(RTW_task);
		370	hard_task_def_mit(RTW_task,rtmGetStepSize(S) * 1000000);
		371	hard_task_def_wcet(RTW_task,10000);
		372	hard_task_def_usemath(RTW_task);
		373	hard_task_def_ctrl_jet(RTW_task);
		374
		375	RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL);
		376	if (RTW_pid == NIL) {
		377	cprintf("Error creating RealTime Workshop Task\n");
		378	sys_end();
		379	}
		380
		381	task_activate(RTW_pid);
		382
		383	while(simulation_run);
		384
		385	Close_RealTime_Workshop();
		386
		387	sys_end();
		388
1342	giacomo	389	return 0;
		390
1368	giacomo	391	}

Subversion Repositories shark

(root)/demos/trunk/rtw/rtw.c @ 1369 - Rev 1368