WebSVN - shark - Rev 1624 - /advdemos/branches/advdemos/rtw/rtw.c

#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;

}

Subversion Repositories shark

(root)/advdemos/branches/advdemos/rtw/rtw.c @ 1650 - Rev 1624