Rev 1625 |
Go to most recent revision |
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"
#include "modules/cabs.h"
#include "percorso.h"
#include "rtw.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
;
CAB input_cid
[NINPUTCAB
];
CAB output_cid
[NOUTPUTCAB
];
#ifdef EXT_MODE
# define rtExtModeSingleTaskUpload(S) \
{ \
int stIdx; \
rtExtModeUploadCheckTrigger(); \
for (stIdx=0; stIdx<NUMST; stIdx++) { \
if (rtmIsSampleHit(S, stIdx, 0 /*unused*/)) { \
rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx)); \
} \
} \
}
#else
# define rtExtModeSingleTaskUpload(S) /* Do nothing */
#endif
#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);
rtExtModeSingleTaskUpload
(S
);
if (GBLbuf.
errmsg != NULL
) {
GBLbuf.
stopExecutionFlag = 1;
return;
}
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
) {
real_T
*input
, *p
;
input
= (real_T
*)rtmGetU
(S
);
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
));
if (rtmGetStopRequested
(S
)) break;
/* Get PAR0 */
if (input_cid
[0] != -1) {
p
= (real_T
*)cab_getmes
(input_cid
[0]);
input
[0] = *p
;
cab_unget
(input_cid
[0], p
);
}
/* Get PAR1 */
if (input_cid
[1] != -1) {
p
= (real_T
*)cab_getmes
(input_cid
[1]);
input
[1] = *p
;
cab_unget
(input_cid
[1], p
);
}
rt_OneStep
(S
);
task_endcycle
();
}
if (!GBLbuf.
stopExecutionFlag && !rtmGetStopRequested
(S
)) {
/* Execute model last time step */
rt_OneStep
(S
);
}
simulation_run
= 0;
return NULL
;
}
TASK OUTPUT_body
(void *arg
) {
real_T
*output
, *p
;
real_T th
;
POINT pos
;
PATH_ELEM
*e
;
real_T distance
= 0;
real_T angle
= 0;
real_T curve
= 0;
output_cid
[0] = cab_create
("DISTANCE", sizeof(real_T
), 2);
output_cid
[1] = cab_create
("ANGLE", sizeof(real_T
), 2);
output_cid
[2] = cab_create
("CURVE", sizeof(real_T
), 2);
output
= (real_T
*)rtmGetY
(S
);
while(1) {
pos.
x = output
[0];
pos.
y = output
[1];
th
= output
[2];
e
= find_closest_elem
(&pos
, 1.0);
if (e
!= 0) {
distance
= get_distance_from_elem
(&pos
, e
);
angle
= th
- get_angle_from_elem
(&pos
,e
);
curve
= is_curve
(e
);
}
/* Send Distance */
p
= (real_T
*)cab_reserve
(output_cid
[0]);
*p
= distance
;
cab_putmes
(output_cid
[0], p
);
/* Send Angle */
p
= (real_T
*)cab_reserve
(output_cid
[1]);
*p
= angle
;
cab_putmes
(output_cid
[1], p
);
/* Send Curve */
p
= (real_T
*)cab_reserve
(output_cid
[2]);
*p
= curve
;
cab_putmes
(output_cid
[2], p
);
task_endcycle
();
}
return NULL
;
}
TASK OUTPUT_w_body
(void *arg
) {
real_T
*output
, *p
;
real_T wr
= 0;
real_T wl
= 0;
output_cid
[0] = cab_create
("WR", sizeof(real_T
), 2);
output_cid
[1] = cab_create
("WL", sizeof(real_T
), 2);
output
= (real_T
*)rtmGetY
(S
);
while(1) {
wr
= output
[3];
wl
= output
[4];
/* Send wr */
p
= (real_T
*)cab_reserve
(output_cid
[3]);
*p
= wr
;
cab_putmes
(output_cid
[3], p
);
/* Send wl */
p
= (real_T
*)cab_reserve
(output_cid
[4]);
*p
= wl
;
cab_putmes
(output_cid
[4], p
);
task_endcycle
();
}
return NULL
;
}
void Init_Rtw
() {
HARD_TASK_MODEL RTW_task
,OUTPUT_task
;
PID RTW_pid
,OUTPUT_pid
,OUTPUT_w_pid
;
int i
;
int RTW_period
;
int RTW_wcet
;
int OUTPUT_period
;
int OUTPUT_wcet
;
Init_RealTime_Workshop
();
Init_All_Path
();
for (i
=0;i
<NINPUTCAB
;i
++) input_cid
[i
] = -1;
for (i
=0;i
<NOUTPUTCAB
;i
++) output_cid
[i
] = -1;
RTW_period
=(int)(rtmGetStepSize
(S
) * 1000000.0);
RTW_wcet
= (int)(rtmGetStepSize
(S
) * 1000000.0 * 0.45);
OUTPUT_period
= (int)(10000.0);
OUTPUT_wcet
= (int)(10000.0 * 0.10);
cprintf
("Task Setup\n");
cprintf
("RTW (P %8d W %8d)\n",RTW_period
,RTW_wcet
);
cprintf
("OUTPUT (P %8d W %8d)\n",OUTPUT_period
,OUTPUT_wcet
);
hard_task_default_model
(RTW_task
);
hard_task_def_mit
(RTW_task
,RTW_period
);
hard_task_def_wcet
(RTW_task
,RTW_wcet
);
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 [RTW] Task\n");
sys_end
();
}
hard_task_default_model
(OUTPUT_task
);
hard_task_def_mit
(OUTPUT_task
,OUTPUT_period
);
hard_task_def_wcet
(OUTPUT_task
,OUTPUT_wcet
);
hard_task_def_usemath
(OUTPUT_task
);
hard_task_def_ctrl_jet
(OUTPUT_task
);
OUTPUT_pid
= task_create
("OUTPUT",OUTPUT_body
,&OUTPUT_task
,NULL
);
if (OUTPUT_pid
== NIL
) {
cprintf
("Error: Cannot create RealTime Workshop [OUTPUT] Task\n");
sys_end
();
}
hard_task_def_mit
(OUTPUT_task
,OUTPUT_period
);
hard_task_def_wcet
(OUTPUT_task
,OUTPUT_wcet
);
OUTPUT_w_pid
= task_create
("OUTPUTW",OUTPUT_w_body
,&OUTPUT_task
,NULL
);
if (OUTPUT_pid
== NIL
) {
cprintf
("Error: Cannot create RealTime Workshop [OUTPUTW] Task\n");
sys_end
();
}
task_activate
(RTW_pid
);
task_activate
(OUTPUT_pid
);
task_activate
(OUTPUT_w_pid
);
}
int main
() {
Init_Rtw
();
activate_sensors
();
activate_control
();
while(simulation_run
);
Close_RealTime_Workshop
();
sys_end
();
return 0;
}