8,7 → 8,10 |
#include "rt_nonfinite.h" |
|
#include "kernel/kern.h" |
#include "modules/cabs.h" |
|
#include "rtw.h" |
|
/*=========* |
* Defines * |
*=========*/ |
47,6 → 50,9 |
real_T rtMinusInf; |
real_T rtNaN; |
|
CAB input_cid[NINPUTCAB]; |
CAB output_cid[NOUTPUTCAB]; |
|
#ifdef EXT_MODE |
# define rtExtModeSingleTaskUpload(S) \ |
{ \ |
389,20 → 395,26 |
|
} |
|
TASK INPUT_body(void *arg) { |
TASK INPUT_body(void *input_port) { |
|
int input_ports = rtmGetNumU(S); |
real_T *input; |
real_T *input, *p; |
int in = (int)(input_port); |
|
cprintf("Number of Inputs = %d\n",input_ports); |
if (in >= NINPUTCAB) return NULL; |
|
input = (real_T *)rtmGetU(S); |
|
while(1) { |
|
input[0] = 1000.0; |
input[1] = 1000.0; |
/* Get CAB message */ |
p = (real_T *)cab_getmes(input_cid[in]); |
|
/* Set INPUT port */ |
input[in] = *p; |
|
/* Release CAB message */ |
cab_unget(input_cid[in], p); |
|
task_endcycle(); |
|
} |
411,22 → 423,29 |
|
} |
|
TASK OUTPUT_body(void *arg) { |
TASK OUTPUT_body(void *output_port) { |
|
real_T *output, *p; |
int out = (int)(output_port); |
char cname[20]; |
|
if (out >= NOUTPUTCAB) return NULL; |
|
sprintf(cname,"OUTPUT%d",out); |
output_cid[out] = cab_create(cname, sizeof(real_T), 2); |
|
int output_ports = rtmGetNumY(S),i; |
real_T *output; |
|
cprintf("Number of Outputs = %d\n",output_ports); |
|
output = (real_T *)rtmGetY(S); |
|
while(1) { |
|
/* Reserve a message */ |
p = (real_T *)cab_reserve(output_cid[out]); |
|
cprintf("T=%6lf ",rtmGetT(S)); |
|
for (i=0;i<output_ports;i++) |
cprintf("O[%d]=%6lf ",i,output[i]); |
cprintf("\n"); |
/* Save data */ |
*p = output[out]; |
|
/* Put CAB message */ |
cab_putmes(output_cid[out], p); |
|
task_endcycle(); |
|
441,6 → 460,8 |
HARD_TASK_MODEL RTW_task,INPUT_task,OUTPUT_task; |
PID RTW_pid,INPUT_pid,OUTPUT_pid; |
|
int i; |
|
int RTW_period; |
int RTW_wcet; |
|
452,14 → 473,17 |
|
Init_RealTime_Workshop(); |
|
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); |
|
INPUT_period = (int)(10000.0); |
INPUT_wcet = (int)(10000.0 * 0.20); |
INPUT_wcet = (int)(10000.0 * 0.10); |
|
OUTPUT_period = (int)(20000.0); |
OUTPUT_wcet = (int)(20000.0 * 0.25); |
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); |
481,12 → 505,14 |
hard_task_default_model(INPUT_task); |
hard_task_def_mit(INPUT_task,INPUT_period); |
hard_task_def_wcet(INPUT_task,INPUT_wcet); |
/* Set input port number */ |
hard_task_def_arg(INPUT_task,(void *)(0)); |
hard_task_def_usemath(INPUT_task); |
hard_task_def_ctrl_jet(INPUT_task); |
|
INPUT_pid = task_create("INPUT",INPUT_body,&INPUT_task,NULL); |
INPUT_pid = task_create("INPUT0",INPUT_body,&INPUT_task,NULL); |
if (INPUT_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [INPUT] Task\n"); |
cprintf("Error: Cannot create RealTime Workshop [INPUT0] Task\n"); |
sys_end(); |
} |
|
493,12 → 519,14 |
hard_task_default_model(OUTPUT_task); |
hard_task_def_mit(OUTPUT_task,OUTPUT_period); |
hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet); |
/* Set output port number */ |
hard_task_def_arg(OUTPUT_task,(void *)(0)); |
hard_task_def_usemath(OUTPUT_task); |
hard_task_def_ctrl_jet(OUTPUT_task); |
|
OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL); |
OUTPUT_pid = task_create("OUTPUT0",OUTPUT_body,&OUTPUT_task,NULL); |
if (OUTPUT_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n"); |
cprintf("Error: Cannot create RealTime Workshop [OUTPUT0] Task\n"); |
sys_end(); |
} |
|
506,6 → 534,9 |
task_activate(RTW_pid); |
task_activate(OUTPUT_pid); |
|
activate_sensors(); |
activate_actuators(); |
|
while(simulation_run); |
|
Close_RealTime_Workshop(); |