/demos/trunk/rtw/rtw.c |
---|
0,0 → 1,585 |
#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; |
} |
/demos/trunk/rtw/sensors.c |
---|
0,0 → 1,93 |
#include "kernel/kern.h" |
#include "modules/cabs.h" |
#include "tmwtypes.h" |
#include "rtw.h" |
extern CAB output_cid[NOUTPUTCAB]; |
TASK SENSOR_body(void *output_port) { |
real_T *p, value; |
int out = (int)(output_port); |
if (out >= NOUTPUTCAB) return NULL; |
while(1) { |
/* Get CAB message */ |
p = (real_T *)cab_getmes(output_cid[out]); |
/* Set value */ |
value = *p; |
/* Release CAB message */ |
cab_unget(output_cid[out], p); |
cprintf("Value %d = %lf\n",out,value); |
task_endcycle(); |
} |
return NULL; |
} |
void activate_sensors() { |
HARD_TASK_MODEL SENSOR_task; |
PID DISTANCE_pid,ANGLE_pid,CURVE_pid,WR_pid,WL_pid; |
hard_task_default_model(SENSOR_task); |
hard_task_def_mit(SENSOR_task,60000); |
hard_task_def_wcet(SENSOR_task,4000); |
hard_task_def_usemath(SENSOR_task); |
hard_task_def_ctrl_jet(SENSOR_task); |
hard_task_def_arg(SENSOR_task,(void *)(0)); |
DISTANCE_pid = task_create("DISTANCE",SENSOR_body,&SENSOR_task,NULL); |
if (DISTANCE_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n"); |
sys_end(); |
} |
hard_task_def_arg(SENSOR_task,(void *)(1)); |
ANGLE_pid = task_create("ANGLE",SENSOR_body,&SENSOR_task,NULL); |
if (DISTANCE_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [DISTACE] Task\n"); |
sys_end(); |
} |
hard_task_def_arg(SENSOR_task,(void *)(2)); |
CURVE_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL); |
if (CURVE_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [CURVE] Task\n"); |
sys_end(); |
} |
hard_task_def_arg(SENSOR_task,(void *)(3)); |
WR_pid = task_create("WR",SENSOR_body,&SENSOR_task,NULL); |
if (WR_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [WR] Task\n"); |
sys_end(); |
} |
hard_task_def_arg(SENSOR_task,(void *)(4)); |
WL_pid = task_create("CURVE",SENSOR_body,&SENSOR_task,NULL); |
if (WR_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [WL] Task\n"); |
sys_end(); |
} |
task_activate(DISTANCE_pid); |
task_activate(ANGLE_pid); |
task_activate(CURVE_pid); |
task_activate(WR_pid); |
task_activate(WL_pid); |
} |
/demos/trunk/rtw/control.c |
---|
0,0 → 1,67 |
#include "kernel/kern.h" |
#include "modules/cabs.h" |
#include "tmwtypes.h" |
#include "rtw.h" |
extern CAB input_cid[NINPUTCAB]; |
TASK CONTROL_body(void *arg) { |
real_T *p; |
input_cid[0] = cab_create("INPUT0", sizeof(real_T), 2); |
input_cid[1] = cab_create("INPUT1", sizeof(real_T), 2); |
while(1) { |
/* Reserve a message */ |
p = (real_T *)cab_reserve(input_cid[0]); |
/* Save PAR1 data */ |
*p = 1000.0; |
/* Put CAB message */ |
cab_putmes(input_cid[0], p); |
/* Reserve a message */ |
p = (real_T *)cab_reserve(input_cid[1]); |
/* Save PAR2 data */ |
*p = 1000.0; |
/* Put CAB message */ |
cab_putmes(input_cid[1], p); |
task_endcycle(); |
} |
return NULL; |
} |
void activate_control() { |
HARD_TASK_MODEL CONTROL_task; |
PID CONTROL_pid; |
hard_task_default_model(CONTROL_task); |
hard_task_def_mit(CONTROL_task,10000); |
hard_task_def_wcet(CONTROL_task,500); |
hard_task_def_usemath(CONTROL_task); |
hard_task_def_ctrl_jet(CONTROL_task); |
CONTROL_pid = task_create("CONTROL",CONTROL_body,&CONTROL_task,NULL); |
if (CONTROL_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [CONTROL] Task\n"); |
sys_end(); |
} |
task_activate(CONTROL_pid); |
} |
/demos/trunk/rtw/rtw.h |
---|
0,0 → 1,10 |
#ifndef __RTW_H__ |
#define __RTW_H__ |
#define NINPUTCAB 10 |
#define NOUTPUTCAB 10 |
void activate_sensors(); |
void activate_control(); |
#endif |
/demos/trunk/rtw/percorso.h |
---|
0,0 → 1,48 |
#ifndef __PERCORSO_H__ |
#define __PERCORSO_H__ |
#define PATH_ELEM_ARC 1 |
#define PATH_ELEM_LINE 2 |
#define CLOCKWISE -1 |
#define COUNTERCLOCK 1 |
typedef struct point { |
double x,y; |
} POINT; |
typedef struct path_elem { |
int type; |
void *data; |
struct path_elem *next; |
} PATH_ELEM; |
typedef struct pd_arc { |
POINT c; |
double r; |
double alpha1; |
double alpha2; |
int dir; |
} PATH_DATA_ARC; |
typedef struct pd_line { |
POINT p1; |
POINT p2; |
double alpha; |
int dir; |
} PATH_DATA_LINE; |
PATH_ELEM *find_closest_elem(POINT *p, double radius); |
double get_distance_from_elem(POINT *p, PATH_ELEM *e); |
double get_angle_from_elem(POINT *p, PATH_ELEM *e); |
/* ritorna: |
1 : curva a sinistra |
-1 : curva a destra |
0 : dritto |
*/ |
int is_curve(PATH_ELEM *e); |
void Init_All_Path(); |
#endif |
/demos/trunk/rtw/makefile |
---|
0,0 → 1,36 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
MATLAB_RTW_DEMO_DIR = ./carrello0_grt_rtw/ |
DEFINE_MODEL = carrello0 |
DEFINE_NUMST = 2 |
DEFINE_NCSTATES = 7 |
DEFINE_TID01EQ = 1 |
DEFINE_RTWLIB += -lrtw |
DEFINE_SOLVER = ode4.o |
FILE_CODE = $(DEFINE_MODEL).o |
FILE_DATA = $(DEFINE_MODEL)_data.o |
PROGS = rtw |
RTW_CFG = "-I. -I$(MATLAB_RTW_DEMO_DIR) -DUSE_RTMODEL -DRT -DMODEL=$(DEFINE_MODEL) -DNUMST=$(DEFINE_NUMST) -DNCSTATES=$(DEFINE_NCSTATES) -DTID01EQ=$(DEFINE_TID01EQ)" |
RTW_OBJS = $(MATLAB_RTW_DEMO_DIR)/$(FILE_CODE) $(DEFINE_SOLVER) $(MODULES_rtwlib) |
ifeq ($(FILE_DATA),$(DEFINE_MODEL)_data.o) |
RTW_OBJS += $(MATLAB_RTW_DEMO_DIR)/$(FILE_DATA) |
endif |
include $(BASE)/config/example.mk |
rtw: |
make -f $(SUBMAKE) OTHERLIBS=$(DEFINE_RTWLIB) OTHERINCL=$(RTW_CFG) APP=rtw INIT= OTHEROBJS="initfile.o control.o sensors.o percorso.o rt_sim_shark.o $(RTW_OBJS)" SHARKOPT="__OLDCHAR__" |
/demos/trunk/rtw/percorso.c |
---|
0,0 → 1,288 |
#include <stdio.h> |
#include <math.h> |
#include "percorso.h" |
int nelem; |
PATH_ELEM * head; |
/* void print_elem(PATH_ELEM *elem) */ |
/* { */ |
/* if (elem->type == PATH_ELEM_ARC) { */ |
/* PATH_DATA_ARC *arc= (PATH_DATA_ARC *) elem->data; */ |
/* printf("Center: x =%3.3lf y =%3.3lf\n", arc->c.x, arc->c.y); */ |
/* printf("angles: a1=%3.3lf a2=%3.3lf\n", arc->alpha1, arc->alpha2); */ |
/* printf("Radius: r =%3.3lf\n", arc->r); */ |
/* } */ |
/* else if (elem->type == PATH_ELEM_LINE) { */ |
/* PATH_DATA_LINE *line = (PATH_DATA_LINE *) elem->data; */ |
/* printf("x1 = %3.3lf y1 = %3.3lf x2 = %3.3lf y2 = %3.3lf\n", */ |
/* line->p1.x, line->p1.y, line->p2.x, line->p2.y); */ |
/* } */ |
/* } */ |
/* void print_point(POINT *p) */ |
/* { */ |
/* printf("p.x = %3.3lf p.y = %3.3lf\n", p->x, p->y); */ |
/* } */ |
/*-----------------------------------------------------------------*/ |
inline double point_dist(POINT *p1, POINT *p2) |
{ |
return sqrt((p1->x - p2->x)*(p1->x - p2->x) + |
(p1->y - p2->y)*(p1->y - p2->y)); |
} |
double get_angle_from_arc(POINT *p1, PATH_DATA_ARC *arc) |
{ |
return acos((p1->x - arc->c.x) / point_dist(p1, &(arc->c))); |
} |
int inside_arc(PATH_DATA_ARC *arc, POINT *p) |
{ |
double alpha = get_angle_from_arc(p, arc); |
if (p->y < arc->c.y) alpha = 2*M_PI - alpha; |
if (alpha <= arc->alpha2 && alpha >= arc->alpha1) return 1; |
else return 0; |
} |
double get_distance_from_arc(POINT *p1, PATH_DATA_ARC *arc) |
{ |
return point_dist (p1,&(arc->c)) - arc->r; |
} |
double get_distance_from_line(POINT *p, PATH_DATA_LINE *l) |
{ |
return ((l->p1.y - l->p2.y) * p->x + |
(l->p2.x - l->p1.x) * p->y + |
(l->p1.x * l->p2.y - l->p2.x * l->p1.y)) / |
point_dist(&(l->p1), &(l->p2)); |
} |
inline double get_t(POINT *p, PATH_DATA_LINE *line) |
{ |
double t = ( (line->p2.x - line->p1.x) * (p->x - line->p1.x) + |
(line->p2.y - line->p1.y) * (p->y - line->p1.y)) / |
(point_dist(&(line->p1), &(line->p2)) * |
point_dist(&(line->p1), &(line->p2))); |
return t; |
} |
double get_angle_from_line(POINT *p, PATH_DATA_LINE *l) |
{ |
return atan2(l->p1.y - l->p2.y, l->p1.x - l->p2.x); |
} |
int includes_elem(POINT *p, PATH_ELEM *e, double radius) |
{ |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = (PATH_DATA_ARC *) e->data; |
if (get_distance_from_arc(p, arc) <= radius && inside_arc(arc,p)) |
return 1; |
else return 0; |
} |
else if (e->type==PATH_ELEM_LINE) { |
PATH_DATA_LINE *line = (PATH_DATA_LINE *) e->data; |
double t = get_t(p, line); |
double d = get_distance_from_line(p, line); |
if (t >= 0 && t <= 1 && d < radius) return 1; |
else return 0; |
} |
return 0; |
} |
void init_arc(PATH_DATA_ARC *arc, double cx, double cy, double r, |
double a1, double a2, int dir) |
{ |
arc->c.x = cx; |
arc->c.y = cy; |
arc->r = r; |
arc->alpha1 = a1; |
arc->alpha2 = a2; |
arc->dir = dir; |
} |
void init_line(PATH_DATA_LINE *l, double x1, double y1, double x2, |
double y2, int dir) |
{ |
//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", x1,y1,x2,y2); |
l->p1.x = x1; |
l->p2.x = x2; |
l->p1.y = y1; |
l->p2.y = y2; |
l->dir = dir; |
//printf("%3.3lf %3.3lf %3.3lf %3.3lf \n", l->p1.x,l->p1.y,l->p2.x,l->p2.y); |
} |
/*---------------------------------------------------------------*/ |
PATH_ELEM *find_closest_elem(POINT *p, double radius) |
{ |
PATH_ELEM *cur = head; |
int i = 0; |
for (i=0; i<nelem; i++) { |
//printf("Searching element %d \n", i); |
//print_elem(cur); |
if (includes_elem(p, cur, radius)) return cur; |
else cur = cur->next; |
} |
return 0; |
} |
double get_distance_from_elem(POINT *p, PATH_ELEM *e) |
{ |
double d = 0; |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = e->data; |
d = get_distance_from_arc(p, arc); |
} |
else if (e->type == PATH_ELEM_LINE) { |
PATH_DATA_LINE *line = e->data; |
d = get_distance_from_line(p, line); |
} |
return d; |
} |
double get_angle_from_elem(POINT *p, PATH_ELEM *e) |
{ |
double alpha = 0; |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = e->data; |
alpha = get_angle_from_arc(p, arc) - M_PI/2; |
} |
else if (e->type == PATH_ELEM_LINE) { |
PATH_DATA_LINE *line = e->data; |
alpha = get_angle_from_line(p, line); |
} |
return alpha; |
} |
/* ritorna: |
1 : curva a sinistra |
-1 : curva a destra |
0 : dritto */ |
int is_curve(PATH_ELEM *e) |
{ |
if (e->type == PATH_ELEM_ARC) { |
PATH_DATA_ARC *arc = e->data; |
return arc->dir; |
} |
else return 0; |
} |
/*---------------------------------------------------------------*/ |
PATH_ELEM elems[11]; |
PATH_DATA_ARC arcs[6]; |
PATH_DATA_LINE lines[5]; |
void Init_All_Path() |
{ |
init_line(&lines[0], 0,0,3,0, -1); |
elems[0].type = PATH_ELEM_LINE; |
elems[0].data = &lines[0]; |
elems[0].next = &elems[1]; |
init_arc(&arcs[0], 3,1, 1, -M_PI/2,0, 1); |
elems[1].type = PATH_ELEM_ARC; |
elems[1].data = &arcs[0]; |
elems[1].next = &elems[2]; |
init_line(&lines[1], 4,1,4,7, -1); |
elems[2].type = PATH_ELEM_LINE; |
elems[2].data = &lines[1]; |
elems[2].next = &elems[3]; |
init_arc(&arcs[1], 2,7, 2, 0, M_PI/2, 1); |
elems[3].type = PATH_ELEM_ARC; |
elems[3].data = &arcs[1]; |
elems[3].next = &elems[4]; |
init_line(&lines[2], 2,9,-1,9, 1); |
elems[4].type = PATH_ELEM_LINE; |
elems[4].data = &lines[2]; |
elems[4].next = &elems[5]; |
init_arc(&arcs[2], -2,8, 1, M_PI/2, M_PI, 1); |
elems[5].type = PATH_ELEM_ARC; |
elems[5].data = &arcs[2]; |
elems[5].next = &elems[6]; |
init_line(&lines[3], -3,8,-3,5, 1); |
elems[6].type = PATH_ELEM_LINE; |
elems[6].data = &lines[3]; |
elems[6].next = &elems[7]; |
init_arc(&arcs[3], -2,5, 1, M_PI, 3*M_PI/2, 1); |
elems[7].type = PATH_ELEM_ARC; |
elems[7].data = &arcs[3]; |
elems[7].next = &elems[8]; |
init_arc(&arcs[4], -2,3, 1, 0, M_PI/2, -1); |
elems[8].type = PATH_ELEM_ARC; |
elems[8].data = &arcs[4]; |
elems[8].next = &elems[9]; |
init_line(&lines[4], -1,3,-1,1, 1); |
elems[9].type = PATH_ELEM_LINE; |
elems[9].data = &lines[4]; |
elems[9].next = &elems[10]; |
init_arc(&arcs[5], 0,1, 1, M_PI, 3*M_PI/2, -1); |
elems[10].type = PATH_ELEM_ARC; |
elems[10].data = &arcs[5]; |
elems[10].next = &elems[0]; |
head = &elems[0]; |
nelem = 11; |
} |
/* int main(void) */ |
/* { */ |
/* double d; */ |
/* double alpha; */ |
/* POINT p; */ |
/* init_all(); */ |
/* p.x = -1.75; */ |
/* p.y = 4.25; */ |
/* PATH_ELEM *e = find_closest_elem(&p, head, 1); */ |
/* if (e != 0) { */ |
/* d = get_distance_from_elem(&p, e); */ |
/* alpha = get_angle_from_elem(&p, e); */ |
/* print_elem(e); */ |
/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */ |
/* printf("direzione %d\n", is_curve(e)); */ |
/* } */ |
/* else */ |
/* printf("Not found!\n"); */ |
/* p.x = 4.75; */ |
/* p.y = 7.1; */ |
/* e = find_closest_elem(&p, head, 1); */ |
/* if (e != 0) { */ |
/* d = get_distance_from_elem(&p, e); */ |
/* alpha = get_angle_from_elem(&p, e); */ |
/* print_elem(e); */ |
/* printf("distance : %3.3lf alpha %3.3lf\n", d, alpha); */ |
/* printf("direzione %d\n", is_curve(e)); */ |
/* } */ |
/* else */ |
/* printf("Not found!\n"); */ |
/* } */ |
/demos/trunk/rtw/actuators.c |
---|
0,0 → 1,62 |
#include "kernel/kern.h" |
#include "modules/cabs.h" |
#include "tmwtypes.h" |
#include "rtw.h" |
extern CAB output_cid[NOUTPUTCAB]; |
TASK ACTUATOR_body(void *output_port) { |
real_T *p, value; |
int out = (int)(output_port); |
if (out >= NOUTPUTCAB) return NULL; |
while(1) { |
/* Get CAB message */ |
p = (real_T *)cab_getmes(output_cid[out]); |
/* Set value */ |
value = *p; |
/* Release CAB message */ |
cab_unget(output_cid[out], p); |
cprintf("Value = %lf\n",value); |
task_endcycle(); |
} |
return NULL; |
} |
void activate_actuators() { |
HARD_TASK_MODEL ACTUATOR_task; |
PID ACTUATOR_pid; |
hard_task_default_model(ACTUATOR_task); |
hard_task_def_mit(ACTUATOR_task,20000); |
hard_task_def_wcet(ACTUATOR_task,4000); |
/* Set actuator port number */ |
hard_task_def_arg(ACTUATOR_task,(void *)(0)); |
hard_task_def_usemath(ACTUATOR_task); |
hard_task_def_ctrl_jet(ACTUATOR_task); |
ACTUATOR_pid = task_create("ACTUATOR0",ACTUATOR_body,&ACTUATOR_task,NULL); |
if (ACTUATOR_pid == NIL) { |
cprintf("Error: Cannot create RealTime Workshop [ACTUATOR0] Task\n"); |
sys_end(); |
} |
task_activate(ACTUATOR_pid); |
} |
/demos/trunk/rtw/rt_sim_shark.c |
---|
0,0 → 1,385 |
#include <math.h> |
#include "tmwtypes.h" |
#ifdef USE_RTMODEL |
# include "simstruc_types.h" |
#else |
# include "simstruc.h" |
#endif |
#include "rt_sim.h" |
#include "kernel/kern.h" |
#ifndef RT_MALLOC /* statically declare data */ |
/*==========* |
* Struct's * |
*==========*/ |
/* |
* TimingData |
*/ |
typedef struct TimingData_Tag { |
real_T period[NUMST]; /* Task periods in seconds */ |
real_T offset[NUMST]; /* Task offsets in seconds */ |
real_T clockTick[NUMST]; /* Flint task time tick counter */ |
int_T taskTick[NUMST]; /* Counter for determining task hits */ |
int_T nTaskTicks[NUMST]; /* Number base rate ticks for a task hit */ |
int_T firstDiscIdx; /* First discrete task index */ |
} TimingData; |
/*=========================* |
* Data local to this file * |
*=========================*/ |
static TimingData td; |
/*==================* |
* Visible routines * |
*==================*/ |
/* Function: rt_SimInitTimingEngine ============================================ |
* Abstract: |
* This function is for use with single tasking or multitasking |
* real-time systems. |
* |
* Initializes the timing engine for a fixed-step real-time system. |
* It is assumed that start time is 0.0. |
* |
* Returns: |
* NULL - success |
* non-NULL - error string |
*/ |
const char *rt_SimInitTimingEngine(int_T rtmNumSampTimes, |
real_T rtmStepSize, |
real_T *rtmSampleTimePtr, |
real_T *rtmOffsetTimePtr, |
int_T *rtmSampleHitPtr, |
int_T *rtmSampleTimeTaskIDPtr, |
real_T rtmTStart, |
SimTimeStep *rtmSimTimeStepPtr, |
void **rtmTimingDataPtr) |
{ |
int_T i; |
int *tsMap = rtmSampleTimeTaskIDPtr; |
real_T *period = rtmSampleTimePtr; |
real_T *offset = rtmOffsetTimePtr; |
int_T *sampleHit = rtmSampleHitPtr; |
real_T stepSize = rtmStepSize; |
if (rtmTStart != 0.0) { |
return("Start time must be zero for real-time systems"); |
} |
*rtmSimTimeStepPtr = MAJOR_TIME_STEP; |
*rtmTimingDataPtr = (void*)&td; |
for (i = 0; i < NUMST; i++) { |
tsMap[i] = i; |
td.period[i] = period[i]; |
td.offset[i] = offset[i]; |
td.nTaskTicks[i] = (int_T)floor(period[i]/stepSize + 0.5); |
if (td.period[i] == CONTINUOUS_SAMPLE_TIME || |
td.offset[i] == 0.0) { |
td.taskTick[i] = 0; |
td.clockTick[i] = 0.0; |
sampleHit[i] = 1; |
} else { |
td.taskTick[i] = (int_T)floor((td.period[i]-td.offset[i]) / |
stepSize+0.5); |
td.clockTick[i] = -1.0; |
sampleHit[i] = 0; |
} |
} |
/* Correct first sample time if continuous task */ |
td.period[0] = stepSize; |
td.nTaskTicks[0] = 1; |
/* Set first discrete task index */ |
#if NUMST == 1 |
td.firstDiscIdx = (int_T)(period[0] == CONTINUOUS_SAMPLE_TIME); |
#else |
td.firstDiscIdx = ((int_T)(period[0] == CONTINUOUS_SAMPLE_TIME) + |
(int_T)(period[1] == CONTINUOUS_SAMPLE_TIME)); |
#endif |
return(NULL); /* success */ |
} /* end rt_SimInitTimingEngine */ |
#if !defined(MULTITASKING) |
/*###########################################################################*/ |
/*########################### SINGLE TASKING ################################*/ |
/*###########################################################################*/ |
/* Function: rt_SimGetNextSampleHit ============================================ |
* Abstract: |
* For a single tasking real-time system, return time of next sample hit. |
*/ |
time_T rt_SimGetNextSampleHit(void) |
{ |
time_T timeOfNextHit; |
td.clockTick[0] += 1; |
timeOfNextHit = td.clockTick[0] * td.period[0]; |
# if NUMST > 1 |
{ |
int i; |
for (i = 1; i < NUMST; i++) { |
if (++td.taskTick[i] == td.nTaskTicks[i]) { |
td.taskTick[i] = 0; |
td.clockTick[i]++; |
} |
} |
} |
# endif |
return(timeOfNextHit); |
} /* end rt_SimGetNextSampleHit */ |
/* Function: rt_SimUpdateDiscreteTaskSampleHits ================================ |
* Abstract: |
* This function is for use with single tasking real-time systems. |
* |
* If the number of sample times is greater than one, then we need to |
* update the discrete task sample hits for the next time step. Note, |
* task 0 always has a hit since it's sample time is the fundamental |
* sample time. |
*/ |
void rt_SimUpdateDiscreteTaskSampleHits(int_T rtmNumSampTimes, |
void *rtmTimingData, |
int_T *rtmSampleHitPtr, |
real_T *rtmTPtr) |
{ |
int_T *sampleHit = rtmSampleHitPtr; |
int i; |
UNUSED_PARAMETER(rtmTimingData); |
UNUSED_PARAMETER(rtmNumSampTimes); |
for (i = td.firstDiscIdx; i < NUMST; i++) { |
int_T hit = (td.taskTick[i] == 0); |
if (hit) { |
rttiSetTaskTime(rtmTPtr, i, |
td.clockTick[i]*td.period[i] + td.offset[i]); |
} |
sampleHit[i] = hit; |
} |
} /* rt_SimUpdateDiscreteTaskSampleHits */ |
#else /* defined(MULTITASKING) */ |
/*###########################################################################*/ |
/*############################## MULTITASKING ###############################*/ |
/*###########################################################################*/ |
/* Function: rt_SimUpdateDiscreteEvents ======================================== |
* Abstract: |
* This function is for use with multitasking real-time systems. |
* |
* This function updates the status of the RT_MODEL sampleHits |
* flags and the perTaskSampleHits matrix which is used to determine |
* when special sample hits occur. |
* |
* The RT_MODEL contains a matrix, called perTaskSampleHits. |
* This matrix is used by the ssIsSpecialSampleHit macro. The row and |
* column indices are both task id's (equivalent to the root RT_MODEL |
* sample time indices). This is a upper triangle matrix. This routine |
* only updates the slower task hits (kept in column j) for row |
* i if we have a sample hit in row i. |
* |
* column j |
* tid 0 1 2 3 4 5 |
* ------------------------- |
* 0 | | X | X | X | X | X | |
* r ------------------------- |
* o 1 | | | X | X | X | X | This matrix(i,j) answers: |
* w ------------------------- If we are in task i, does |
* 2 | | | | X | X | X | slower task j have a hit now? |
* i ------------------------- |
* 3 | | | | | X | X | |
* ------------------------- |
* 4 | | | | | | X | X = 0 or 1 |
* ------------------------- |
* 5 | | | | | | | |
* ------------------------- |
* |
* How macros index this matrix: |
* |
* ssSetSampleHitInTask(S, j, i, X) => matrix(i,j) = X |
* |
* ssIsSpecialSampleHit(S, my_sti, promoted_sti, tid) => |
* (tid_for(promoted_sti) == tid && !minor_time_step && |
* matrix(tid,tid_for(my_sti)) |
* ) |
* |
* my_sti = My (the block's) original sample time index. |
* promoted_sti = The block's promoted sample time index resulting |
* from a transition via a ZOH from a fast to a |
* slow block or a transition via a unit delay from |
* a slow to a fast block. |
* |
* The perTaskSampleHits array, of dimension n*n, is accessed using |
* perTaskSampleHits[j + i*n] where n is the total number of sample |
* times, 0 <= i < n, and 0 <= j < n. The C language stores arrays in |
* row-major order, that is, row 0 followed by row 1, etc. |
* |
*/ |
time_T rt_SimUpdateDiscreteEvents(int_T rtmNumSampTimes, |
void *rtmTimingData, |
int_T *rtmSampleHitPtr, |
int_T *rtmPerTaskSampleHits) |
{ |
int i, j; |
int_T *sampleHit = rtmSampleHitPtr; |
UNUSED_PARAMETER(rtmTimingData); |
/* |
* Run this loop in reverse so that we do lower priority events first. |
*/ |
i = NUMST; |
while (--i >= 0) { |
if (td.taskTick[i] == 0) { |
/* |
* Got a sample hit, reset the counter, and update the clock |
* tick counter. |
*/ |
sampleHit[i] = 1; |
td.clockTick[i]++; |
/* |
* Record the state of all "slower" events |
*/ |
for (j = i + 1; j < NUMST; j++) { |
rttiSetSampleHitInTask(rtmPerTaskSampleHits, rtmNumSampTimes, |
j, i, sampleHit[j]); |
} |
} else { |
/* |
* no sample hit, increment the counter |
*/ |
sampleHit[i] = 0; |
} |
if (++td.taskTick[i] == td.nTaskTicks[i]) { /* update for next time */ |
td.taskTick[i] = 0; |
} |
} |
return(td.clockTick[0]*td.period[0]); |
} /* rt_SimUpdateDiscreteEvents */ |
/* Function: rt_SimUpdateDiscreteTaskTime ====================================== |
* Abstract: |
* This function is for use with multitasking systems. |
* |
* After a discrete task output and update has been performed, this |
* function must be called to update the discrete task time for next |
* time around. |
*/ |
void rt_SimUpdateDiscreteTaskTime(real_T *rtmTPtr, |
void *rtmTimingData, |
int tid) |
{ |
UNUSED_PARAMETER(rtmTimingData); |
rttiSetTaskTime(rtmTPtr, tid, |
td.clockTick[tid]*td.period[tid] + td.offset[tid]); |
} |
#endif /* MULTITASKING */ |
#else |
#include "mrt_sim.c" /* dynamically allocate data */ |
#endif /* RT_MALLOC */ |
/* |
******************************************************************************* |
* FUNCTIONS MAINTAINED FOR BACKWARDS COMPATIBILITY WITH THE SimStruct |
******************************************************************************* |
*/ |
#ifndef USE_RTMODEL |
const char *rt_InitTimingEngine(SimStruct *S) |
{ |
const char_T *retVal = rt_SimInitTimingEngine( |
ssGetNumSampleTimes(S), |
ssGetStepSize(S), |
ssGetSampleTimePtr(S), |
ssGetOffsetTimePtr(S), |
ssGetSampleHitPtr(S), |
ssGetSampleTimeTaskIDPtr(S), |
ssGetTStart(S), |
&ssGetSimTimeStep(S), |
&ssGetTimingData(S)); |
return(retVal); |
} |
# ifdef RT_MALLOC |
void rt_DestroyTimingEngine(SimStruct *S) |
{ |
rt_SimDestroyTimingEngine(ssGetTimingData(S)); |
} |
# endif |
# if !defined(MULTITASKING) |
void rt_UpdateDiscreteTaskSampleHits(SimStruct *S) |
{ |
rt_SimUpdateDiscreteTaskSampleHits( |
ssGetNumSampleTimes(S), |
ssGetTimingData(S), |
ssGetSampleHitPtr(S), |
ssGetTPtr(S)); |
} |
# ifndef RT_MALLOC |
time_T rt_GetNextSampleHit(void) |
{ |
return(rt_SimGetNextSampleHit()); |
} |
# else /* !RT_MALLOC */ |
time_T rt_GetNextSampleHit(SimStruct *S) |
{ |
return(rt_SimGetNextSampleHit(ssGetTimingData(S), |
ssGetNumSampleTimes(S))); |
} |
# endif |
# else /* MULTITASKING */ |
time_T rt_UpdateDiscreteEvents(SimStruct *S) |
{ |
return(rt_SimUpdateDiscreteEvents( |
ssGetNumSampleTimes(S), |
ssGetTimingData(S), |
ssGetSampleHitPtr(S), |
ssGetPerTaskSampleHitsPtr(S))); |
} |
void rt_UpdateDiscreteTaskTime(SimStruct *S, int tid) |
{ |
rt_SimUpdateDiscreteTaskTime(ssGetTPtr(S), ssGetTimingData(S), tid); |
} |
#endif |
#endif |
/* EOF: rt_sim.c */ |
/demos/trunk/rtw/initfile.c |
---|
0,0 → 1,120 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/* |
------------ |
CVS : $Id: initfile.c,v 1.1 2004-03-13 14:50:57 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.1 $ |
Last update: $Date: 2004-03-13 14:50:57 $ |
------------ |
System initialization file |
This file contains the 2 functions needed to initialize the system. |
These functions register the following levels: |
an EDF (Earliest Deadline First) level |
a RR (Round Robin) level |
a CBS (Costant Bandwidth Server) level |
a Dummy level |
It can accept these task models: |
HARD_TASK_MODEL (wcet+mit) at level 0 |
SOFT_TASK_MODEL (met, period) at level 1 |
NRT_TASK_MODEL at level 2 |
This file is similar to the configuration of kernel/init/hartik3.c |
TICK is set to 0 (one-shot timer is used) |
*/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS kparms = BASE_KEYB; |
HARTPORT_init(); |
keyb_def_ctrlC(kparms, NULL); |
keyb_def_map(kparms,itaMap); |
KEYB_init(&kparms); |
__call_main__(mb); |
return (void *)0; |
} |
/demos/trunk/static/static.c |
---|
0,0 → 1,262 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: static.c,v 1.5 2003-12-17 13:52:47 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.5 $ |
Last update: $Date: 2003-12-17 13:52:47 $ |
------------ |
**/ |
/* |
* Copyright (C) 2001 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "static.h" |
#include <ll/stdio.h> |
#include <ll/string.h> |
#include <kernel/model.h> |
#include <kernel/descr.h> |
#include <kernel/var.h> |
#include <kernel/func.h> |
#define STATIC_printf kern_printf |
//#define STATIC_printf printk |
/*+ Status used in the level +*/ |
#define STATIC_READY MODULE_STATUS_BASE /*+ - Ready status +*/ |
#define STATIC_IDLE MODULE_STATUS_BASE+4 /*+ to wait the deadline +*/ |
/*+ flags +*/ |
#define STATIC_FLAG_NORAISEEXC 2 |
/*+ the level redefinition for the Earliest Deadline First level +*/ |
typedef struct { |
level_des l; /*+ the standard level descriptor +*/ |
IQUEUE mytable; |
PID currenttask; |
struct timespec hp; |
struct timespec ref; |
} STATIC_level_des; |
static void STATIC_offset_activate(void *par) |
{ |
PID p = (PID) par; |
STATIC_level_des *lev; |
lev = (STATIC_level_des *)level_table[proc_table[p].task_level]; |
lev->currenttask = p; |
event_need_reschedule(); |
// STATIC_printf("(o p%d t%d)", p, (int)proc_table[p].timespec_priority.tv_sec); |
} |
static void STATIC_activateall(STATIC_level_des *lev) |
{ |
PID my_table_index; |
struct timespec x; |
STATIC_printf("(A "); |
for (my_table_index = iq_query_first(&lev->mytable); |
my_table_index != NIL; |
my_table_index = iq_query_next(my_table_index, &lev->mytable)) { |
ADDTIMESPEC(&lev->ref,iq_query_timespec(my_table_index, &lev->mytable),&x); |
kern_event_post(&x, STATIC_offset_activate,(void *)my_table_index); |
STATIC_printf("|p%d t%d ", |
my_table_index, |
(int)iq_query_timespec(my_table_index, &lev->mytable)->tv_sec); |
} |
STATIC_printf(")"); |
} |
static void STATIC_hyperperiod(void *par) |
{ |
STATIC_level_des *lev; |
struct timespec x; |
lev = (STATIC_level_des *)level_table[(LEVEL)par]; |
STATIC_printf("(hp %d)", (int)lev->ref.tv_sec); |
STATIC_activateall(lev); |
ADDTIMESPEC(&lev->ref, &lev->hp, &x); |
lev->ref = x; |
kern_event_post(&x, STATIC_hyperperiod, par); |
} |
/* The scheduler only gets the first task in the queue */ |
static PID STATIC_public_scheduler(LEVEL l) |
{ |
STATIC_level_des *lev = (STATIC_level_des *)(level_table[l]); |
return lev->currenttask; |
} |
static int STATIC_public_create(LEVEL l, PID p, TASK_MODEL *m) |
{ |
STATIC_level_des *lev = (STATIC_level_des *)(level_table[l]); |
/* if the STATIC_task_create is called, then the pclass must be a |
valid pclass. */ |
STATIC_TASK_MODEL *h = (STATIC_TASK_MODEL *)m; |
if (m->pclass != STATIC_PCLASS) return -1; |
if (m->level != 0 && m->level != l) return -1; |
iq_query_timespec(p, &lev->mytable)->tv_sec = h->offset.tv_sec; |
iq_query_timespec(p, &lev->mytable)->tv_nsec = h->offset.tv_nsec; |
iq_timespec_insert(p,&lev->mytable); |
return 0; /* OK, also if the task cannot be guaranteed... */ |
} |
static void STATIC_public_dispatch(LEVEL l, PID p, int nostop) |
{ |
} |
static void STATIC_public_epilogue(LEVEL l, PID p) |
{ |
} |
static void STATIC_public_activate(LEVEL l, PID p) |
{ |
} |
static void STATIC_public_unblock(LEVEL l, PID p) |
{ |
} |
static void STATIC_public_block(LEVEL l, PID p) |
{ |
} |
static int STATIC_public_message(LEVEL l, PID p, void *m) |
{ |
STATIC_level_des *lev = (STATIC_level_des *)(level_table[l]); |
lev->currenttask = NIL; |
jet_update_endcycle(); /* Update the Jet data... */ |
return 0; |
} |
static void STATIC_public_end(LEVEL l, PID p) |
{ |
STATIC_level_des *lev = (STATIC_level_des *)(level_table[l]); |
lev->currenttask = NIL; |
iq_extract(p,&lev->mytable); |
/* we finally put the task in the ready queue */ |
proc_table[p].status = FREE; |
iq_insertfirst(p,&freedesc); |
} |
/* Registration functions */ |
/*+ Registration function: |
int flags the init flags ... see STATIC.h +*/ |
LEVEL STATIC_register_level() |
{ |
LEVEL l; /* the level that we register */ |
STATIC_level_des *lev; /* for readableness only */ |
printk("STATIC_register_level\n"); |
/* request an entry in the level_table */ |
l = level_alloc_descriptor(sizeof(STATIC_level_des)); |
lev = (STATIC_level_des *)level_table[l]; |
printk(" lev=%d\n",(int)lev); |
/* fill the standard descriptor */ |
lev->l.public_scheduler = STATIC_public_scheduler; |
lev->l.public_create = STATIC_public_create; |
lev->l.public_end = STATIC_public_end; |
lev->l.public_dispatch = STATIC_public_dispatch; |
lev->l.public_epilogue = STATIC_public_epilogue; |
lev->l.public_activate = STATIC_public_activate; |
lev->l.public_unblock = STATIC_public_unblock; |
lev->l.public_block = STATIC_public_block; |
lev->l.public_message = STATIC_public_message; |
/* fill the STATIC descriptor part */ |
iq_init(&lev->mytable, &freedesc, 0); |
lev->currenttask = NIL; |
NULL_TIMESPEC(&lev->hp); |
NULL_TIMESPEC(&lev->ref); |
return l; |
} |
void STATIC_start(LEVEL l, struct timespec *h, struct timespec *o) |
{ |
STATIC_level_des *lev = (STATIC_level_des *)(level_table[l]); |
struct timespec x; |
kern_cli(); |
kern_gettime(&x); |
lev->hp = *h; |
ADDTIMESPEC(&x,o,&lev->ref); |
STATIC_printf("(ST: ref:%d.%d x:%d.%d)\n", |
(int)lev->ref.tv_sec, (int)lev->ref.tv_nsec, |
(int)x.tv_sec, (int)x.tv_nsec); |
kern_event_post(&x, STATIC_hyperperiod,(void *)l); |
kern_sti(); |
} |
/demos/trunk/static/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= test1st |
include $(BASE)/config/example.mk |
test1st: |
make -f $(SUBMAKE) APP=test1st INIT= OTHEROBJS="initfile.o static.o" OTHERINCL= SHARKOPT=__OLDCHAR__ |
/demos/trunk/static/static.h |
---|
0,0 → 1,117 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* Massimiliano Giorgi <massy@gandalf.sssup.it> |
* Luca Abeni <luca@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: static.h,v 1.2 2003-01-07 17:10:18 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.2 $ |
Last update: $Date: 2003-01-07 17:10:18 $ |
------------ |
Static scheduler demo (FIRST Project Hand-off) |
**/ |
/* |
* Copyright (C) 2001 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#ifndef __STATIC_H__ |
#define __STATIC_H__ |
#include <ll/ll.h> |
#include <kernel/config.h> |
#include <sys/types.h> |
#include <kernel/types.h> |
/*+ flags... +*/ |
/* no flags for the static scheduler */ |
#define STATIC_PCLASS 0x1000 |
#define STATIC_LEVELNAME "STATIC 1st Project" |
#define STATIC_LEVEL_CODE 167 |
#define STATIC_LEVEL_VERSION 1 |
typedef struct { |
TASK_MODEL t; |
struct timespec offset; |
} STATIC_TASK_MODEL; |
#define static_task_default_model(m,o) \ |
task_default_model((m).t,STATIC_PCLASS), \ |
((m).offset).tv_sec = (o).tv_sec, \ |
((m).offset).tv_nsec = (o).tv_nsec |
#define static_task_def_level(m,l) task_def_level((m).t,l) |
#define static_task_def_arg(m,a) task_def_arg((m).t,a) |
#define static_task_def_stack(m,s) task_def_stack((m).t,s) |
#define static_task_def_stackaddr(m,s) task_def_stackaddr((m).t,s) |
#define static_task_def_group(m,g) task_def_group((m).t,g) |
#define static_task_def_usemath(m) task_def_usemath((m).t) |
#define static_task_def_system(m) task_def_system((m).t) |
#define static_task_def_nokill(m) task_def_nokill((m).t) |
#define static_task_def_ctrl_jet(m) task_def_ctrl_jet((m).t) |
#define static_task_def_joinable(m) task_def_joinable((m).t) |
#define static_task_def_unjoinable(m) task_def_unjoinable((m).t) |
/*+ Registration function: |
int flags Options to be used in this level instance... |
+*/ |
LEVEL STATIC_register_level(void); |
void STATIC_start(LEVEL l, struct timespec *h, struct timespec *o); |
#endif |
/demos/trunk/static/test1st.c |
---|
0,0 → 1,129 |
/* |
* Project: HARTIK (HA-rd R-eal TI-me K-ernel) |
* |
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
* Gerardo Lamastra <gerardo@sssup.it> |
* |
* Authors : Paolo Gai <pj@hartik.sssup.it> |
* (see authors.txt for full list of hartik's authors) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://hartik.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: test1st.c,v 1.2 2002-11-11 07:55:13 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.2 $ |
Last update: $Date: 2002-11-11 07:55:13 $ |
------------ |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "kernel/kern.h" |
#include "static.h" |
#include <math.h> |
#include <string.h> |
int theend = 0; |
void *mytask(void *arg) |
{ |
struct timespec t; |
for (;;) |
{ |
sys_gettime(&t); |
cprintf("I am task %s at time %d.%d\n", (char *)arg, (int)t.tv_sec, (int)t.tv_nsec); |
theend++; |
if (theend > 10) sys_end(); |
task_endcycle(); |
} |
} |
int main(int argc, char **argv) |
{ |
PID p1; |
STATIC_TASK_MODEL m; |
struct timespec my_time, h, o; |
my_time.tv_nsec=0; |
my_time.tv_sec = 0; |
my_time.tv_nsec = 500000000; |
static_task_default_model(m, my_time); |
static_task_def_arg(m, "A"); |
p1 = task_create("TaskA",mytask,&m,NULL); |
if (p1 == -1) { |
perror("Could not create task <A> ..."); |
sys_end(); |
} |
my_time.tv_sec = 1; |
my_time.tv_nsec = 0; |
static_task_default_model(m, my_time); |
static_task_def_arg(m, "B"); |
p1 = task_create("TaskB",mytask,&m,NULL); |
if (p1 == -1) { |
perror("Could not create task <B> ..."); |
sys_end(); |
} |
my_time.tv_sec = 1; |
my_time.tv_nsec = 500000000; |
static_task_default_model(m, my_time); |
static_task_def_arg(m, "C"); |
p1 = task_create("TaskC",mytask,&m,NULL); |
if (p1 == -1) { |
perror("Could not create task <C> ..."); |
sys_end(); |
} |
h.tv_sec = 3; |
h.tv_nsec = 0; |
o.tv_sec = 1; |
o.tv_nsec = 0; |
STATIC_start(0, &h, &o); |
return 0; |
} |
/demos/trunk/static/initfile.c |
---|
0,0 → 1,81 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/* |
------------ |
CVS : $Id: initfile.c,v 1.1.1.1 2002-09-02 09:37:48 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.1.1.1 $ |
Last update: $Date: 2002-09-02 09:37:48 $ |
------------ |
*/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "kernel/kern.h" |
#include "static.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
STATIC_register_level(); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
__call_main__(mb); |
return (void *)0; |
} |
/demos/trunk/static/readme |
---|
0,0 → 1,16 |
STATIC Scheduling Module |
------------------------ |
This is the scheduling module created on the FIRST Project Meeting in |
Vasteras, July 2002. |
The module accept a new Task Model called STATIC_TASK_MODEL that |
contains the information about the starting time of a task. After |
creating all the tasks, the static scheduling starts calling the |
function static_start. |
For comments, bugfixes & co, write to pj@sssup.it. |
Bye |
PJ |
/demos/trunk/slsh/slsh.c |
---|
0,0 → 1,750 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* Massimiliano Giorgi <massy@gandalf.sssup.it> |
* Luca Abeni <luca@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: slsh.c,v 1.5 2003-12-17 13:52:47 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.5 $ |
Last update: $Date: 2003-12-17 13:52:47 $ |
------------ |
This file contains the scheduling module for Slot-Shifting. |
Read slsh.h for further details. |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "slsh.h" |
#include <ll/stdio.h> |
#include <ll/stdlib.h> |
#include <ll/string.h> |
#include <ll/math.h> /* for ceil(...) */ |
#include <ll/ll.h> /* for memcpy(...) */ |
#include <kernel/model.h> |
#include <kernel/descr.h> |
#include <kernel/var.h> |
#include <kernel/func.h> |
//#define eslsh_printf kern_printf |
#define slsh_printf printk |
/* Keeps information about static and guaranteed tasks */ |
typedef struct { |
int est; |
int dabs; |
int interval; |
} SLSH_task; |
/*+ Status used in the level +*/ |
#define SLSH_READY MODULE_STATUS_BASE |
#define SLSH_WAIT MODULE_STATUS_BASE + 1 |
#define SLSH_IDLE MODULE_STATUS_BASE + 2 |
#define SLSH_WCET_VIOLATED MODULE_STATUS_BASE + 3 |
/*+ defines +*/ |
#define MAX_INTERVALS 1000 /* 1000 intervals is max, for now */ |
/******+ the level redefinition for the SLOT SHIFT level +*******/ |
typedef struct { |
level_des l; /*+ the standard level descriptor+*/ |
/* task lists */ |
SLSH_task tasks[MAX_PROC]; /* est and dl's for static and guaranteed task */ |
IQUEUE idle_statics; /* finished static tasks */ |
IQUEUE unspecified; /* tasks with only a wcet */ |
/* the Intervals list */ |
SLSH_interval intervals[MAX_INTERVALS]; |
int current; /* current interval */ |
int last; /* last interval */ |
int slot; /* slot shifting time */ |
TIME slot_length; /* slothlength in real system time*/ |
int LCM; /* length (in slots) of ofline schedule */ |
int slot_event; /* save the event */ |
} SLSH_level_des; |
/* check if some tasks are ready, return 0 if ready, -1 otherwise */ |
static int SLSH_R(SLSH_task* tasks) |
{ |
int s; |
/* for all static tasks */ |
for(s = 0; tasks[s].est != -1; ++s) |
{ |
if(proc_table[s].status == SLSH_READY) |
return 0; |
} |
return -1; |
} |
/* check if unspecified exists, return 0 if it exists, -1 otherwise */ |
static int SLSH_T(IQUEUE *unspecified) |
{ |
if(!iq_isempty(unspecified)) |
return 0; |
else |
return -1; |
} |
/* return the sc in an interval */ |
static int SLSH_sc(SLSH_interval* intervals, int i) |
{ |
return intervals[i].sc; |
} |
/* return a static task from current interval or a guaranted task */ |
static PID SLSH_staticOrGuaranteed(SLSH_level_des* lev) |
{ |
int lowest_dl = 0; /* lowest dl found */ |
PID pid = 0; /* static or guaranteed task */ |
int t; |
/* Decide according to EDF, go through all static & guaranteed tasks */ |
for(t = 0; t < MAX_PROC; ++t) |
{ |
/* static tasks */ |
if(proc_table[t].pclass == STATIC_PCLASS) |
{ |
/* static task must belong to current interval */ |
if(lev->tasks[t].interval == lev->current) |
{ |
/* only ready tasks */ |
if(proc_table[t].status == SLSH_READY) |
{ |
/* a new lower dl was found */ |
if(lev->tasks[t].dabs < lowest_dl) |
{ |
lowest_dl = lev->tasks[t].dabs; |
pid = t; |
} |
} |
} |
} /* guaranteed tasks */ |
else if(proc_table[t].pclass == HARD_PCLASS) |
{ |
/* only ready tasks */ |
if(proc_table[t].status == SLSH_READY) |
{ |
/* a new lower dl was found */ |
if(lev->tasks[t].dabs < lowest_dl) |
{ |
lowest_dl = lev->tasks[t].dabs; |
pid = t; |
} |
} |
} |
}/* for all tasks */ |
return pid; |
} |
/* return a static task among the candidates, all ready statics */ |
static PID SLSH_candidates(SLSH_task* tasks) |
{ |
int lowest_dl = 0; |
PID pid = -1; |
int t; |
/* Use the EDL algorithm again to decide which task to run */ |
for(t = 0; t < MAX_PROC; ++t) |
{ |
/* only static tasks */ |
if(proc_table[t].pclass == STATIC_PCLASS) |
{ |
/* only ready tasks */ |
if(proc_table[t].status == SLSH_READY) |
{ |
/* a new lower dl was found */ |
if(tasks[t].dabs < lowest_dl) |
{ |
lowest_dl = tasks[t].dabs; |
pid = t; |
} |
}/* all ready tasks */ |
}/* all static tasks */ |
}/* for all tasks */ |
return pid; |
} |
/* decrease the sc in a interval by amount */ |
void SLSH_decSc(SLSH_interval* intervals, int i, int amount) |
{ |
intervals[i].sc -= amount; |
} |
void SLSH_incSc(SLSH_interval* intervals, int i, int amount) |
{ |
intervals[i].sc += amount; |
} |
/* swap the sc between intervals, also consider intervals with negative sc */ |
void SLSH_swapSc(SLSH_interval* intervals, int current, int task_interval) |
{ |
/* decrease the sc in the current interval */ |
SLSH_decSc(intervals, current, 1); |
/* update the other interval(s) */ |
if(intervals[task_interval].sc < 0) /* negative sc */ |
{ |
/* special case, increase next interval sc by 1 and also current interval (borrowing) */ |
if(task_interval == current + 1) |
{ |
SLSH_incSc(intervals, task_interval, 1); |
SLSH_incSc(intervals, current, 1); |
} |
else /* increase every interval sc that is negative between current and task_interval */ |
{ |
while(task_interval > current && intervals[task_interval].sc < 0) |
{ |
SLSH_incSc(intervals, task_interval, 1); |
task_interval--; |
} |
} |
} |
else /* ordinary swapping */ |
SLSH_incSc(intervals, task_interval, 1); |
} |
/* The scheduler, decides which task to run. */ |
static PID SLSH_public_scheduler(LEVEL l) |
{ |
SLSH_level_des* lev = (SLSH_level_des *)(level_table[l]); |
PID pid; |
/* The scheduler choses among static, guaranteed (hard aperiodic) and |
unspecified (soft aperiodic) tasks */ |
/* no ready tasks and no sc, execute idle task */ |
if(SLSH_R(lev->tasks) == 0 && SLSH_sc(lev->intervals, lev->current) == 0) |
return NIL; |
/* must execute a static from current intervall or a guaranteed task */ |
else if(SLSH_R(lev->tasks) > 0 && SLSH_sc(lev->intervals, lev->current) == 0) |
return SLSH_staticOrGuaranteed(lev); |
/* sc available... */ |
else if(SLSH_R(lev->tasks) > 0 && SLSH_sc(lev->intervals, lev->current) > 0) |
{ |
/* If unspecified exist, execute it according to FIFO order */ |
if(SLSH_T(&lev->unspecified) == 0) |
{ |
SLSH_decSc(lev->intervals, lev->current, 1); /* decrease sc by 1 */ |
return iq_getfirst(&lev->unspecified); |
} |
else /* No unspecified, execute task from candidates (statics) */ |
{ |
pid = SLSH_candidates(lev->tasks); |
/* sc needs to be swapped */ |
if(lev->tasks[pid].interval != lev->current) |
SLSH_swapSc(lev->intervals, lev->tasks[pid].interval, lev->current); |
return pid; |
} |
} |
kern_printf("(SLSH s)"); |
return NIL; |
} |
/* not used, slot-shifting handles all guarantees itself, it handles all bandwidth */ |
static int SLSH_public_guarantee(LEVEL l, bandwidth_t *freebandwidth) |
{ |
*freebandwidth = 0; |
return 1; |
} |
/* get the interval that x is in */ |
static int SLSH_getInterval(SLSH_interval* intervals, int x, int last) |
{ |
int i; |
/* search through the intervals */ |
for(i = 0; i <= last; ++i) |
{ |
/* I is in the interval where start is smaller or equal and end is bigger */ |
if(intervals[i].start <= x && x < intervals[i].end) |
return i; |
} |
return -1; |
} |
/* get the start of the interval I */ |
static int SLSH_intervalStart(SLSH_interval* intervals, int I) |
{ |
return intervals[I].start; |
} |
/* split interval I into two parts, slow because of copying. OBS!!! no check if there is |
enough space in the intervals array */ |
static void SLSH_splitInterval(SLSH_level_des* lev, int I, int dabs) |
{ |
SLSH_interval left_interval; |
int i; |
lev->last++; |
/* move every interval above and including I */ |
for(i = lev->last; i > I; --i) |
memcpy(&lev->intervals[i], &lev->intervals[i - 1], sizeof(SLSH_interval)); |
/* Left interval start, end and length */ |
left_interval.start = lev->intervals[I].start; |
left_interval.end = dabs; |
left_interval.length = left_interval.end - left_interval.start; |
/* Right interval (uses old interval struct) start and length end remains as the old value */ |
lev->intervals[I + 1].start = dabs; |
lev->intervals[I + 1].length = lev->intervals[I + 1].end - lev->intervals[I + 1].start; |
/* check if sc still exists in the right interval */ |
if(lev->intervals[I + 1].length - lev->intervals[I + 1].maxt > 0) |
{ |
lev->intervals[I + 1].sc = lev->intervals[I + 1].length - lev->intervals[I + 1].maxt; |
left_interval.sc = left_interval.length; /* the whole interval is free, for now... */ |
} |
else /* no sc in the right interval */ |
{ |
lev->intervals[I + 1].maxt = lev->intervals[I + 1].length; |
left_interval.sc = lev->intervals[I + 1].sc; /* all sc in left interval */ |
lev->intervals[I + 1].sc = 0; |
} |
/* insert the new interval */ |
memcpy(&lev->intervals[I], &left_interval, sizeof(SLSH_interval)); |
} |
/* Reduce the sc from back to front by the wcet amount, interval splitting may be neccesary */ |
static void SLSH_updateSc(SLSH_level_des* lev, HARD_TASK_MODEL* h) |
{ |
int dabs = ceil((lev->slot + h->drel)/lev->slot_length); /* absolute deadline of request */ |
int dabs_interval = SLSH_getInterval(lev->intervals, dabs, lev->last); /* interval where dabs is */ |
int C = ceil(h->wcet/lev->slot_length); /* amount of sc to reduce */ |
int sc = 0; |
int i; |
/* check if interval splitting is neccesary */ |
if(lev->intervals[dabs_interval].end != dabs) |
SLSH_splitInterval(lev, dabs_interval, dabs); |
/* decrease sc in all intervals that are neccesary from dabs_interval o current */ |
for(i = dabs_interval; i >= lev->current && C > 0; --i) |
{ |
if((sc = SLSH_sc(lev->intervals, i)) >= 0) /* only decrease where sc exists */ |
{ |
if(sc > C) /* the last sc dec */ |
{ |
SLSH_decSc(lev->intervals, i, C); |
C = 0; |
} |
else /* to little sc in this interval, decrease it to 0 */ |
{ |
C -= SLSH_sc(lev->intervals, i); |
SLSH_decSc(lev->intervals, i, SLSH_sc(lev->intervals, i)); |
} |
} |
}/* for all intervals */ |
} |
/* the guarantee algorithm for hard aperiodic requests */ |
static int SLSH_guarantee(SLSH_level_des* lev, HARD_TASK_MODEL* h) |
{ |
int total_sc = 0; |
int temp, i; |
int dabs = ceil((lev->slot + h->drel)/lev->slot_length); /* absolute deadline of request */ |
int dabs_interval = SLSH_getInterval(lev->intervals, dabs, lev->last); /* interval where dabs is */ |
/* check if the sc up until request deadline is >= request wcet */ |
/* 1. the sc of the current interal */ |
total_sc = SLSH_sc(lev->intervals, lev->current); |
/* 2. the sc for all whole intervals between current and the interval |
with the request deadline */ |
for(i = (lev->current) + 1; i < dabs_interval; ++i) |
{ |
if((temp = SLSH_sc(lev->intervals, i)) > 0) |
total_sc += temp; |
} |
/* 3. the min of sc or the execution need in the last interval */ |
total_sc += min(SLSH_sc(lev->intervals, dabs_interval), |
dabs - SLSH_intervalStart(lev->intervals, |
dabs_interval)); |
if(total_sc >= h->wcet) |
{ /* update the sc in the intervals from back to front */ |
SLSH_updateSc(lev, h); |
return 0; |
} |
else |
return -1; |
} |
/* check if task model is accepted and store nessecary parameters */ |
static int SLSH_public_create(LEVEL l, PID p, TASK_MODEL *m) |
{ |
SLSH_level_des *lev = (SLSH_level_des *)(level_table[l]); |
STATIC_TASK_MODEL* s; |
HARD_TASK_MODEL* h; |
SOFT_TASK_MODEL* u; |
/* Check the models */ |
switch(m->pclass) |
{ |
case STATIC_PCLASS: /* offline scheduled tasks */ |
break; |
case HARD_PCLASS: /* hard aperiodic tasks */ |
h = (HARD_TASK_MODEL *) m; |
if (h->drel == 0 || h->wcet == 0) /* must be set */ |
return -1; |
break; |
case SOFT_PCLASS: /* soft aperiodic tasks */ |
u = (SOFT_TASK_MODEL *) m; |
if(u->wcet == 0) /* must be set */ |
return -1; |
break; |
default: |
return -1; |
} |
/* if the SLSH_task_create is called, then the pclass must be a |
valid pclass. Slot-shifting accepts STATIC_TASK, HARD_TASK |
and SOFT_TASK models with some restrictions */ |
/* est, dl and wcet is saved in slotlengths */ |
switch(m->pclass) |
{ |
case STATIC_PCLASS: /* offline scheduled tasks */ |
s = (STATIC_TASK_MODEL *) m; |
lev->tasks[p].est = ceil(s->est/lev->slot_length); |
lev->tasks[p].dabs = ceil(s->dabs/lev->slot_length); |
lev->tasks[p].interval = s->interval; |
proc_table[p].avail_time = s->wcet; |
proc_table[p].wcet = s->wcet; |
break; |
case HARD_PCLASS: /* hard aperiodic tasks */ |
h = (HARD_TASK_MODEL *) m; |
if(SLSH_guarantee(lev, h) == 0) |
{ |
/* convert drel to dabs */ |
lev->tasks[p].dabs = ceil((lev->slot + h->drel)/lev->slot_length); |
proc_table[p].avail_time = h->wcet; |
proc_table[p].wcet = h->wcet; |
} |
else /* task not guaranteed */ |
return -1; |
break; |
case SOFT_PCLASS: |
u = (SOFT_TASK_MODEL *) m; |
proc_table[p].avail_time = u->wcet; |
proc_table[p].wcet = u->wcet; |
iq_insertlast(p, &lev->unspecified); /* respect FIFO order */ |
break; |
default: /* a task model not supported */ |
return -1; |
} |
/* enable wcet check in the kernel */ |
proc_table[p].control |= CONTROL_CAP; |
return 0; |
} |
/************* The slot end event handler *************/ |
static void SLSH_slot_end(void* p) |
{ |
SLSH_level_des* lev = (SLSH_level_des *) p; |
PID pid; |
int i; |
/* increase slot "time" by 1 */ |
if(lev->slot < lev->LCM) |
{ |
lev->slot++; |
/* check if new statics are ready */ |
for(i = 0; lev->tasks[i].interval != -1; ++i) |
{ |
if(lev->tasks[i].est <= lev->slot && proc_table[i].status == SLSH_WAIT) |
proc_table[i].status = SLSH_READY; |
} |
/* check if current (interval) needs updating */ |
if(lev->current < SLSH_getInterval(lev->intervals, lev->slot, lev->last)) |
lev->current++; |
} |
else /* restart from the beginning of the offline schedule */ |
{ |
lev->slot = 0; |
while((pid = iq_getfirst(&lev->idle_statics)) != NIL) |
{ |
if(lev->tasks[pid].est <= lev->slot) |
proc_table[pid].status = SLSH_READY; |
else |
proc_table[pid].status = SLSH_WAIT; |
} |
} |
/* call for a rescheduling, reset event flag and increase slot by 1 */ |
lev->slot_event = -1; |
kern_printf("*"); |
event_need_reschedule(); |
} |
/* when a task becomes executing (EXE status) */ |
static void SLSH_public_dispatch(LEVEL l, PID pid, int nostop) |
{ |
SLSH_level_des *lev = (SLSH_level_des *)(level_table[l]); |
struct timespec t; |
/* the task state is set EXE by the scheduler() |
we extract the task from the unspecified queue. |
NB: we can't assume that p is the first task in the queue!!! */ |
if(proc_table[pid].pclass == SOFT_PCLASS) |
iq_extract(pid, &lev->unspecified); |
/* also start the timer for one slot length */ |
lev->slot_event = kern_event_post(&TIME2TIMESPEC(lev->slot_length, t), |
SLSH_slot_end, (void*) lev); |
} |
/* called when task is moved from EXE status */ |
static void SLSH_public_epilogue(LEVEL l, PID pid) |
{ |
SLSH_level_des *lev = (SLSH_level_des *)(level_table[l]); |
/* check if the wcet is finished... */ |
if (proc_table[pid].avail_time <= 0) |
{ |
/* if it is, raise a XWCET_VIOLATION exception */ |
kern_raise(XWCET_VIOLATION, pid); |
proc_table[pid].status = SLSH_WCET_VIOLATED; |
} |
else /* the end of a slot. the task returns into the ready queue... */ |
{ |
if(proc_table[pid].pclass == SOFT_PCLASS) |
iq_insertfirst(pid,&lev->unspecified); |
proc_table[pid].status = SLSH_READY; |
} |
} |
/* when task go from SLEEP to SLSH_READY or SLSH_WAIT */ |
static void SLSH_public_activate(LEVEL l, PID pid) |
{ |
SLSH_level_des *lev = (SLSH_level_des *)(level_table[l]); |
WORD type = proc_table[pid].pclass; |
/* Test if we are trying to activate a non sleeping task */ |
/* Ignore this; the task is already active */ |
if (proc_table[pid].status != SLEEP && proc_table[pid].status != SLSH_WCET_VIOLATED) |
return; |
/* make task ready or waiting, dependong on slot (the time) for static tasks only*/ |
if(type == STATIC_PCLASS && lev->tasks[pid].est <= lev->slot) |
proc_table[pid].status = SLSH_READY; |
else |
proc_table[pid].status = SLSH_WAIT; |
if(type == HARD_PCLASS) |
proc_table[pid].status = SLSH_READY; |
/* insert unspecified tasks in QQUEUE and make it ready */ |
if(type == SOFT_PCLASS) |
{ |
iq_insertlast(pid ,&lev->unspecified); |
proc_table[pid].status = SLSH_READY; |
} |
} |
/* when a task i returned to module from a semaphore, mutex ... */ |
static void SLSH_public_unblock(LEVEL l, PID pid) |
{ |
SLSH_level_des *lev = (SLSH_level_des *)(level_table[l]); |
/* change staus of task */ |
proc_table[pid].status = SLSH_READY; |
if(proc_table[pid].pclass == SOFT_PCLASS) |
iq_insertfirst(pid ,&lev->unspecified); |
} |
/* when a semaphore, mutex ... taskes a task from module */ |
static void SLSH_public_block(LEVEL l, PID pid) |
{ |
/* Extract the running task from the level |
. we have already extract it from the ready queue at the dispatch time. |
. the capacity event have to be removed by the generic kernel |
. the wcet don't need modification... |
. the state of the task is set by the calling function |
. the deadline must remain... |
So, we do nothing!!! |
*/ |
} |
/* the task has finihed its wcet, kill task (dont kill static tasks) */ |
static void SLSH_public_end(LEVEL l, PID pid) |
{ |
SLSH_level_des *lev = (SLSH_level_des *)(level_table[l]); |
if(proc_table[pid].pclass == SOFT_PCLASS) |
{ |
if (proc_table[pid].status == SLSH_READY) |
iq_extract(pid, &lev->unspecified); |
} |
else if(proc_table[pid].pclass == HARD_PCLASS) |
{ |
if (proc_table[pid].status == SLSH_READY) |
lev->tasks[pid].dabs = 0; |
} |
/* static tasks: put them in idle QUEUE, reset status and avail_time */ |
else if(proc_table[pid].pclass == STATIC_PCLASS) |
{ |
proc_table[pid].avail_time = proc_table[pid].wcet; |
proc_table[pid].status = SLSH_IDLE; |
iq_priority_insert(pid, &lev->idle_statics); |
} |
proc_table[pid].status = FREE; |
} |
/* called when a task should sleep but not execute for awhile, mabe a mode change */ |
//static void SLSH_task_sleep(LEVEL l, PID pid) |
//{ |
// |
// /* the task has terminated his job before it consume the wcet. All OK! */ |
// proc_table[pid].status = SLEEP; |
// |
// /* we reset the capacity counters... only for static tasks */ |
// if (proc_table[pid].pclass == STATIC_PCLASS) |
// proc_table[pid].avail_time = proc_table[pid].wcet; |
// |
//} |
/** Guest Functions, slot shifing accepts no guests, so all generates exceptions **/ |
/******* Registration functions *******/ |
/*+ Registration function: */ |
LEVEL SLSH_register_level() |
{ |
LEVEL l; /* the level that we register */ |
SLSH_level_des *lev; /* for readableness only */ |
PID i; /* a counter */ |
kern_printf("SLSH_register_level\n"); |
/* request an entry in the level_table */ |
l = level_alloc_descriptor(sizeof(SLSH_level_des)); |
lev = (SLSH_level_des *)level_table[l]; |
printk(" lev=%d\n",(int)lev); |
/* fill the standard descriptor */ |
lev->l.public_scheduler = SLSH_public_scheduler; |
lev->l.public_guarantee = SLSH_public_guarantee; |
lev->l.public_create = SLSH_public_create; |
lev->l.public_end = SLSH_public_end; |
lev->l.public_dispatch = SLSH_public_dispatch; |
lev->l.public_epilogue = SLSH_public_epilogue; |
lev->l.public_activate = SLSH_public_activate; |
lev->l.public_unblock = SLSH_public_unblock; |
lev->l.public_block = SLSH_public_block; |
/* fill the SLSH descriptor part */ |
for(i = 0; i < MAX_PROC; i++) |
{ |
lev->tasks[i].est = -1; |
lev->tasks[i].dabs = 0; |
lev->tasks[i].interval = -1; |
} |
for(i = 0; i < MAX_INTERVALS; i++) |
{ |
lev->intervals[i].start = -1; |
lev->intervals[i].end = -1; |
lev->intervals[i].length = 0; |
lev->intervals[i].maxt = 0; |
lev->intervals[i].sc = 0; |
} |
lev->current = 0; |
lev->last = NIL; |
lev->slot = 0; |
lev->slot_length = 0; |
lev->slot_event = -1; |
return l; |
} |
void SLSH_set_interval(LEVEL l, int start, int end, int maxt) |
{ |
SLSH_level_des* lev = (SLSH_level_des *)(level_table[l]); |
static int i = -1; |
i++; |
lev->intervals[i].start = start; |
lev->intervals[i].end = end; |
lev->intervals[i].length = end - start; |
lev->intervals[i].maxt = maxt; |
lev->intervals[i].sc = lev->intervals[i].length - maxt; |
lev->last = i; |
} |
void SLSH_set_variables(LEVEL l, TIME length) |
{ |
SLSH_level_des* lev = (SLSH_level_des *)(level_table[l]); |
lev->slot_length = length; |
} |
/demos/trunk/slsh/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS=slshtest |
include $(BASE)/config/example.mk |
slshtest: |
make -f $(SUBMAKE) APP=slshtest INIT= OTHEROBJS="slshinit.o slsh.o" OTHERINCL= SHARKOPT=__OLDCHAR__ |
/demos/trunk/slsh/slsh.h |
---|
0,0 → 1,204 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* Massimiliano Giorgi <massy@gandalf.sssup.it> |
* Luca Abeni <luca@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: slsh.h,v 1.2 2003-01-07 17:10:18 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.2 $ |
Last update: $Date: 2003-01-07 17:10:18 $ |
------------ |
Author: Tomas Lennvall, Date: Feb 2000. |
This file contains the scheduling module for Slot shifting. |
Title: |
Slot Shifting |
Task Models Accepted: |
STATIC_TASK_MODEL - Periodic Hard tasks that are scheduled by |
an off-line scheduler, so that all guarantees regarding precedence, mutex |
deadline violation is taken care of. The tasks are in an executione schedule, |
that is the order in when they become ready. They have the following fields: |
est (earliest start time), wcet and absolute deadline. |
HARD_TASK_MODEL - Hard Tasks (Hard aperiodic requests) |
wcet field and drel field must be != 0. They are used to set the wcet |
and deadline of the tasks. |
periodicity field must be APERIODIC |
mit field is ignored. |
SOFT_TASK_MODEL - Soft Tasks (Unspecified tasks) |
wcet field must be != 0. periodicity filed must be APERIODIC |
period and met filed is ignored. |
Guest Models Accepted: |
NONE - Slot shifting handles all tasks by itself (at this moment). |
Description: |
This module schedules the offline scheduled tasks according to the slot- |
shifting paradigm, dividing time into slots of a fixed length and assigning |
tasks to execute in those slots. Slot-shifting also keeps track of the free |
bandwidth in the schedule by using disjoint intervals and sc (spare capacity). |
Each interval has a sc nr that represents the free bandwidth in that interval, |
the sc can be used by hard aperiodic tasks, static tasks from later interval or |
soft aperiodic tasks. Hard aperiodic tasks are guaranteed an incorporated in |
the schedule by reduction of sc before they execute. No guarantee is |
performed on the soft aperiodic tasks, they are run when no other task wants |
to execute and sc is available. |
Description: |
This module implements the Slot shifting algorithm, by Gerhard Fohler. Slot shifting |
schedules off-line scheduled tasks and also handles hard aperiodic requests by the |
guarantee alorithm. Slot shifting can also handle soft aperiodic tasks, |
called unspecified. That is tasks without a deadline. |
Exceptions raised: |
These exceptions are pclass-dependent... |
XDEADLINE_MISS |
If a task miss his deadline, the exception is raised. |
XWCET_VIOLATION |
If a task doesn't end the current cycle before if consume the wcet, |
an exception is raised, and the task is put in the EDF_WCET_VIOLATED |
state. To reactivate it, use EDF_task_activate via task_activate or |
manage directly the EDF data structure. Note that the exception is not |
handled properly, an XDEADLINE_MISS exeeption will also be raised at |
the period end... |
Restrictions & special features: |
- This level doesn't manage the main task. |
- At init time we can choose if the level have to activate |
. the wcet check |
(If a task require more time than declared, it is stopped and put in |
the state EDF_WCET_VIOLATED; a XWCET_VIOLATION exception is raised) |
. the task guarantee algorithm |
(when all task are created the system will check that the task_set |
will not use more than the available bandwidth) |
- The level use the priority and timespec_priority fields. |
- A function to return the used bandwidth of a level is provided. |
- The guest tasks don't provide the guest_endcycle function |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#ifndef __SLSH_H__ |
#define __SLSH_H__ |
#include <ll/ll.h> |
#include <kernel/config.h> |
#include <sys/types.h> |
#include <kernel/types.h> |
#define STATIC_PCLASS 0x0500 |
#define SLSH_LEVELNAME "Slot Shifting" |
#define SLSH_LEVEL_CODE 5 |
#define SLSH_LEVEL_VERSION 1 |
/* ----------------------------------------------------------------------- |
STATIC_TASK_MODEL: offline scheduled Tasks |
----------------------------------------------------------------------- */ |
/* Offline-scheduled tasks are hard periodic tasks that have been |
scheduled before runtime. All guarantees are made by the off- |
line scheduler so the tasks are already guaranteed. |
*/ |
typedef struct { |
TASK_MODEL t; |
TIME est; |
TIME wcet; |
TIME dabs; |
int interval; /* used in slot shifting */ |
} STATIC_TASK_MODEL; |
#define static_task_default_model(m) \ |
task_default_model((m).t,STATIC_PCLASS), \ |
(m).est = -1, \ |
(m).dabs = 0, \ |
(m).wcet = 0, \ |
(m).interval = -1; |
#define static_task_def_level(m,l) task_def_level((m).t,l) |
#define static_task_def_arg(m,a) task_def_arg((m).t,a) |
#define static_task_def_stack(m,s) task_def_stack((m).t,s) |
#define static_task_def_group(m,g) task_def_group((m).t,g) |
#define static_task_def_usemath(m) task_def_usemath((m).t) |
#define static_task_def_system(m) task_def_system((m).t) |
#define static_task_def_nokill(m) task_def_nokill((m).t) |
#define static_task_def_ctrl_jet(m) task_def_ctrl_jet((m).t) |
#define static_task_def_est(m,p) (m).est = (p) |
#define static_task_def_dabs(m,d) (m).dabs = (d) |
#define static_task_def_wcet(m,w) (m).wcet = (w) |
#define static_task_def_interval(m,i) (m).interval = (i) |
#define static_task_def_trace(m) task_def_trace((m).t) |
#define static_task_def_notrace(m) task_def_notrace((m).t) |
/*#define min(a, b) ((a) < (b) ? (a) : (b))*/ |
#define TIME2TIMESPEC(T, TS) \ |
( \ |
((TS).tv_sec = ((T)/1000000)), \ |
((TS).tv_nsec = (((T)%1000000) * 1000)), \ |
(TS) \ |
) |
/* define the interval struct */ |
typedef struct { |
int start; /* start of interval */ |
int end; /* end of interval */ |
int length; /* Length of interval */ |
int maxt; /* maximum execution time in interval */ |
int sc; /* spare capacity in interval */ |
} SLSH_interval; |
/*+ Registration function: */ |
LEVEL SLSH_register_level(); |
void SLSH_set_interval(LEVEL l, int start, int end, int maxt); |
void SLSH_set_variables(LEVEL l, TIME length); |
#endif |
/demos/trunk/slsh/slshtest.c |
---|
0,0 → 1,173 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* Massimiliano Giorgi <massy@gandalf.sssup.it> |
* Luca Abeni <luca@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: slshtest.c,v 1.1.1.1 2002-09-02 09:37:41 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.1.1.1 $ |
Last update: $Date: 2002-09-02 09:37:41 $ |
------------ |
Slot shifting test |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai and Tomas Lennvall |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include <kernel/config.h> |
#include "kernel/kern.h" |
#include "slsh.h" |
#include "drivers/keyb.h" |
/* a slot length of 100 ms */ |
#define SLOT_LENGTH 100000 |
TASK static1(void) |
{ |
int i = 0; |
kern_printf("Static1\n"); |
while(sys_gettime(NULL) < 10000) i++; |
return 0; |
} |
TASK static2(void) |
{ |
int i = 0; |
kern_printf("Static2\n"); |
while(sys_gettime(NULL) < 10000) i++; |
return 0; |
} |
TASK static3(void) |
{ |
kern_printf("Static3\n"); |
return 0; |
} |
void my_end(KEY_EVT *e) |
{ |
sys_end(); |
} |
int main(int argc, char** argv) |
{ |
STATIC_TASK_MODEL s; |
// HARD_TASK_MODEL h_aper; |
// SOFT_TASK_MODEL u; |
PID p1,p2,p3; |
struct timespec x; |
KEY_EVT emerg; |
kern_cli(); |
x.tv_sec=5; |
kern_event_post(&x,(void (*)(void *))sys_end,NULL); |
kern_sti(); |
//keyb_set_map(itaMap); |
emerg.ascii = 'x'; |
emerg.scan = KEY_X; |
emerg.flag = ALTL_BIT; |
keyb_hook(emerg,my_end); |
/* set som variables in the scheduling level */ |
SLSH_set_interval(0, 0, 8, 5); |
SLSH_set_interval(0, 8, 17, 7); |
SLSH_set_interval(0, 17, 20, 1); |
SLSH_set_variables(0, SLOT_LENGTH); |
static_task_default_model(s); |
static_task_def_group(s, 1); |
/* define time i ms */ |
/* static1 task */ |
static_task_def_est(s, 0); |
static_task_def_dabs(s, 800000); |
static_task_def_wcet(s, 500000); |
static_task_def_interval(s, 0); |
kern_printf("In main, before task creation\n"); |
p1 = task_create("Static 1", static1, &s, NULL); |
if(p1 == NIL) |
kern_printf("Cannot create: Static1!\n"); |
/* Static2 task */ |
static_task_def_est(s, 800000); |
static_task_def_dabs(s, 1700000); |
static_task_def_wcet(s, 700000); |
static_task_def_interval(s, 1); |
p2 = task_create("Static 2", static2, &s, NULL); |
if(p2 == NIL) |
kern_printf("Cannot create: Static2!\n"); |
/* Static3 task */ |
static_task_def_est(s, 1700000); |
static_task_def_dabs(s, 2000000); |
static_task_def_wcet(s, 100000); |
static_task_def_interval(s, 2); |
p3 = task_create("Static3", static3, &s, NULL); |
if(p3 == NIL) |
kern_printf("Cannot create: Static3!\n"); |
/* End task */ |
/*hard_task_default_model(h_aper); |
hard_task_def_wcet(h_aper, 100000); |
*/ |
kern_printf("After task creation\n"); |
group_activate(1); |
return 0; |
} |
/demos/trunk/slsh/readme.txt |
---|
0,0 → 1,6 |
This Example has been made by Tomas Lenvall. |
There is not a lot of documentation available, so if you have problems please |
send an e-mail to Tomas ( mailto:tlv@mdh.se ) |
Paolo |
/demos/trunk/slsh/slshinit.c |
---|
0,0 → 1,117 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: slshinit.c,v 1.1.1.1 2002-09-02 09:37:41 pj Exp $ |
File: $File$ |
Revision: $Revision: 1.1.1.1 $ |
Last update: $Date: 2002-09-02 09:37:41 $ |
------------ |
System initialization file |
The tick is set to TICK ms. |
This file contains the 2 functions needed to initialize the system. |
These functions register the following levels: |
a Slot Shifting level |
a Dummy level |
It can accept these task models: |
STATIC_TASK_MODEL |
HARD_TASK_MODEL(aperiodic) |
SOFT_TASK_MODEL |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "kernel/kern.h" |
#include "slsh.h" |
#include "modules/rr2.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "drivers/keyb.h" |
#include "modules/dummy.h" |
/*+ sysyem tick in us +*/ |
#define TICK 300 |
/* define RR tick in us*/ |
#define RRTICK 10000 |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
SLSH_register_level(); |
RR2_register_level(RRTICK, RR2_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
return TICK; |
} |
NRT_TASK_MODEL nrt; |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS k = BASE_KEYB; |
nrt_task_default_model(nrt); |
keyb_def_task(k,&nrt); |
HARTPORT_init(); |
KEYB_init(NULL); |
__call_main__(mb); |
return 0; |
} |
/demos/trunk/telesco/initfile.c |
---|
0,0 → 1,133 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Paolo Gai <pj@gandalf.sssup.it> |
* (see the web pages for full authors list) |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/* |
------------ |
CVS : $Id: initfile.c,v 1.1 2003-06-04 09:41:01 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.1 $ |
Last update: $Date: 2003-06-04 09:41:01 $ |
------------ |
System initialization file |
This file contains the 2 functions needed to initialize the system. |
These functions register the following levels: |
an EDF (Earliest Deadline First) level |
a RR (Round Robin) level |
a CBS (Costant Bandwidth Server) level |
a Dummy level |
It can accept these task models: |
HARD_TASK_MODEL (wcet+mit) at level 0 |
SOFT_TASK_MODEL (met, period) at level 1 |
NRT_TASK_MODEL at level 2 |
This file is similar to the configuration of kernel/init/hartik3.c |
TICK is set to 0 (one-shot timer is used) |
*/ |
/* |
* Copyright (C) 2000 Paolo Gai |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "drivers/keyb.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
int argc; |
char *argv[100]; |
void read_cfg_file(int argc, char **argv); |
int main(int argc, char **argv); |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
__compute_args__(mb, &argc, argv); |
read_cfg_file(argc, argv); |
//read_cfg_file(argc, argv); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
KEYB_PARMS kparms = BASE_KEYB; |
HARTPORT_init(); |
keyb_def_ctrlC(kparms, NULL); |
keyb_def_map(kparms,itaMap); |
KEYB_init(&kparms); |
//__call_main__(mb); |
return (void *)main(argc,argv); |
//return (void *)0; |
} |
/demos/trunk/telesco/tel.c |
---|
0,0 → 1,709 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Bera Marco mbera@libero.it |
* Varasio Gabriele varasio@odino.unipv.it |
* |
* Universita' degli studi di Pavia |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
/** |
------------ |
CVS : $Id: tel.c,v 1.1 2003-06-04 09:41:01 giacomo Exp $ |
File: $File$ |
Revision: $Revision: 1.1 $ |
Last update: $Date: 2003-06-04 09:41:01 $ |
------------ |
**/ |
/* |
* Copyright (C) 2000 Paolo Gai and Giorgio Buttazzo |
* |
* This program is free software; you can redistribute it and/or modify |
* it under the terms of the GNU General Public License as published by |
* the Free Software Foundation; either version 2 of the License, or |
* (at your option) any later version. |
* |
* This program is distributed in the hope that it will be useful, |
* but WITHOUT ANY WARRANTY; without even the implied warranty of |
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
* GNU General Public License for more details. |
* |
* You should have received a copy of the GNU General Public License |
* along with this program; if not, write to the Free Software |
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
* |
*/ |
/****************************************************************/ |
/* PERIODIC PROCESS TEST */ |
/****************************************************************/ |
#include <ll/i386/x-dos.h> |
#include <kernel/kern.h> |
#include <modules/cabs.h> |
#include <drivers/glib.h> |
#include <drivers/keyb.h> |
#include <math.h> |
#include <semaphore.h> |
#include <stdlib.h> |
#include "moon.h" |
#define N_MAX_TELESCOPI 5 |
#define X0 10 |
#define pi 3.14 |
#define ex 2.71 |
#define NCAB N_MAX_TELESCOPI+1 |
#define dim_msg 8 |
#define dim_x 49 |
#define dim_y 49 |
#define radius 9 |
#define Y_TELESCOPI 300 |
/* task periods */ |
#define PERIOD_T1 80000 |
#define PERIOD_T4 100000 |
#define PERIOD_T5 100000 |
#define WCET_T 5000 |
int period_t1=0; |
#define SOGLIA 8 |
char *cname[NCAB] = {"cab1", "cab2", "cab3", "cab4","cab5","cab6"}; |
// posizioni dei telescopi e rispettivi poli |
int x[5] = {100, 200, 300, 400,500}; |
//double poli[5]={-50,-30,-1, -20, -5}; |
double poli[5]; |
// guadagno proporzionale |
double kp=0.0; |
// flag per rilevare la pressione del tasto spazio |
int numero_telescopi=0; |
int flag=0; |
KEY_EVT move_tasto; |
// Immagini ausiliarie per il disegno della luna |
BYTE lunabkg[900]; |
BYTE lunadest[900]; |
/* A semaphore used to access Video Cards in mutual exclusion */ |
sem_t mutex; |
sem_t mutex_tasto; |
/***************************************************************/ |
// cab necessari allo scambio delle coordinate dell'oggetto e delle |
// immagini dei rispettivi telescopi |
CAB cid[NCAB]; |
// vettori per memorizzare le immagini |
BYTE videobuf[dim_x*dim_y]; |
BYTE videobuf1[dim_x*dim_y]; |
BYTE videobuf2[dim_x*dim_y]; |
BYTE videobuf3[dim_x*dim_y]; |
BYTE videobuf4[dim_x*dim_y]; |
BYTE videobuf5[dim_x*dim_y]; |
// Variabili per il disegno delle stelle sullo sfondo |
int num_p; |
int y_p[7]; |
int x_p[7]; |
int dim_p[7]; |
int col_p[7]; |
TASK media(void *arg) |
{ |
char *p; |
int i=0; |
int j=0; |
while(1) |
{ |
p = cab_getmes(cid[1]); |
memcpy(videobuf,p,dim_x*dim_y*sizeof(BYTE)); |
cab_unget(cid[1], p); |
p = cab_getmes(cid[2]); |
memcpy(videobuf1,p,dim_x*dim_y*sizeof(BYTE)); |
cab_unget(cid[2], p); |
p = cab_getmes(cid[3]); |
memcpy(videobuf2,p,dim_x*dim_y*sizeof(BYTE)); |
cab_unget(cid[3], p); |
p = cab_getmes(cid[4]); |
memcpy(videobuf4,p,dim_x*dim_y*sizeof(BYTE)); |
cab_unget(cid[4], p); |
p = cab_getmes(cid[5]); |
memcpy(videobuf5,p,dim_x*dim_y*sizeof(BYTE)); |
cab_unget(cid[5], p); |
// calcolo media |
for (i=0;i<dim_x;i++) |
{ |
for(j=0;j<dim_y;j++) |
{ |
videobuf3[i*dim_x+j]=(double)(videobuf[i*dim_x+j]+videobuf1[i*dim_x+j]+videobuf2[i*dim_x+j]+videobuf4[i*dim_x+j]+videobuf5[i*dim_x+j])/numero_telescopi; |
} |
} |
// visualizza immagine mediata |
sem_wait(&mutex); |
grx_putimage(570,10,570+dim_x-1,10+dim_y-1,videobuf3); |
sem_post(&mutex); |
task_endcycle(); |
} |
} |
TASK move(void *arg) |
{ |
//int i = (int)arg; |
char *p; |
int x_object=300; // posizione dell'oggetto da rilevare |
int y_object=200; |
// variabile contatore |
int z=0; |
char coord_x[dim_msg]; |
p = cab_reserve(cid[0]); |
sprintf(coord_x,"%d %d",x_object,y_object); |
memcpy(p,coord_x,dim_msg*sizeof(char)); |
cab_putmes(cid[0], p); |
//Normalizzazione |
for(z=0;z<900;z++) |
{ |
if (luna[z]>0) |
{ |
luna[z]=((luna[z])*16.0/255.0)+16; |
} |
} |
task_endcycle(); |
while (1) |
{ |
// Cancellazione Luna |
sem_wait(&mutex); |
grx_box(x_object-15,y_object-15,x_object+14,y_object+14,0); |
sem_post(&mutex); |
if (keyb_getcode(&move_tasto,NON_BLOCK)) |
{ |
if (move_tasto.ascii=='4') |
x_object=x_object-5; |
if (move_tasto.ascii=='6') |
x_object=x_object+5; |
if (move_tasto.ascii=='2') |
y_object=y_object+5; |
if (move_tasto.ascii=='8') |
y_object=y_object-5; |
if (move_tasto.ascii=='9'){ |
y_object=y_object-5; |
x_object=x_object+5; } |
if (move_tasto.ascii=='1'){ |
y_object=y_object+5; |
x_object=x_object-5; } |
if (move_tasto.ascii=='7'){ |
y_object=y_object-5; |
x_object=x_object-5; } |
if (move_tasto.ascii=='3'){ |
y_object=y_object+5; |
x_object=x_object+5;} |
if (move_tasto.ascii==' '){ |
sem_wait(&mutex_tasto); |
flag=1; |
if(numero_telescopi<N_MAX_TELESCOPI) |
{ |
numero_telescopi++; |
} |
sem_post(&mutex_tasto); |
} |
//CONTROLLI SULLA POSIZIONE |
if(y_object<(dim_y/2+70)) y_object=dim_y/2+70; |
if(y_object>(240-dim_y/2)) y_object=(240-dim_y/2); |
if(x_object<(dim_x/2+20 )) x_object=dim_x/2+20; |
if(x_object>(630-dim_x/2-10)) x_object=(630-dim_x/2-10); |
} |
p = cab_reserve(cid[0]); |
sprintf(coord_x,"%d %d",x_object,y_object); |
memcpy(p,coord_x,dim_msg*sizeof(char)); |
cab_putmes(cid[0], p); |
for(z=0;z<7;z++) |
{ |
if (sqrt(pow((x_object-x_p[z]),2)+pow((y_object-y_p[z]),2))<20+dim_p[z]+radius) |
grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]); |
} |
sem_wait(&mutex); |
// Gestione sfondo |
grx_getimage(x_object-15,y_object-15,x_object+14, y_object+14,lunabkg); |
for(z=0;z<900;z++) |
{ |
if( luna[z]==0 && lunabkg!=0 ) |
{ |
lunadest[z]=lunabkg[z]; |
} |
else |
{ |
lunadest[z]=luna[z]; |
} |
} |
grx_putimage(x_object-15,y_object-15,x_object+14, y_object+14,lunadest); |
sem_post(&mutex); |
task_endcycle(); |
} |
} |
TASK tele(void *arg) |
{ |
int i = (int)arg; |
int passi; |
char s[100]; |
int x_object=300; // posizione dell'oggetto da rilevare |
int y_object=200; |
int x_start=500; // posizione iniziale dei telescopi |
int y_start=190; |
int x_current=x_start; // posizione corrente dei telescopi |
int y_current=y_start; |
int x_current_d[9]; |
int y_current_d[9]; |
int y= Y_TELESCOPI; // coordinata y dei telescopi |
double alpha_new=atan((y-(double)y_current)/((x[i]-(double)x_current))); |
double alpha=alpha_new; |
double distance=0.0; |
double alpha_target=0.0; |
double tc=0.1; |
double u=0.0; |
double u_old=0.0; |
double errore=0.0; |
double delta_x=0; |
double delta_y=0; |
char *p; |
//int j=0; |
// indice matrice per aggiunta di rumore |
int k=0; |
int q=0; |
double polo=poli[i]; |
int val=0; |
BYTE videobuf[dim_x*dim_y]; |
passi = 0; |
//srand(i); |
//srand(i+sys_gettime(NULL)); |
alpha_target=atan((y-(double)y_object)/((double)x_object-x[i])); |
sem_wait(&mutex); |
grx_text("targ",4,340,12,0); |
sprintf(s,"%f",poli[i]); |
grx_text(s,x[i],y+60,12,0); |
grx_text("polo",4,y+60,12,0); |
grx_text("new",4,y+50,12,0); |
sprintf(s,"%1.6f",alpha_target); |
grx_text(s, x[i]-25,y+50,12,0); |
grx_rect(x[i]-40,y+25,x[i]+40,y+85,2); |
grx_rect(x[i]-40,y+90,x[i]+40,y+150,2); |
sem_post(&mutex); |
// Disegno telescopio |
grx_disc(x[i],y,19,i+1); |
grx_box( x[i]-19,y,x[i]+19,y+20,i+1); |
task_endcycle(); |
while (1) { |
// legge di controllo |
passi++; |
//sprintf(s,"%d",passi); |
//grx_text(s, 50,110,12,0); |
p = cab_getmes(cid[0]); |
sscanf(p,"%d %d",&val,&y_object); |
cab_unget(cid[0], p); |
x_object=val; |
alpha_target=atan((y-(double)y_object)/((double)x_object-x[i])); |
if (alpha_target <0) |
{ |
alpha_target=3.14+alpha_target; |
} |
errore=alpha_target-alpha_new; |
u=u_old+kp*tc*errore; |
//alpha_new=0.13*alpha+0.87*u; |
alpha_new=(exp(polo*tc))*alpha+(1-exp(polo*tc))*u; |
u_old=u; |
alpha=alpha_new; |
// implementazione dei limiti degli attuattori |
if (alpha_new > 3.14) |
alpha_new=3.14; |
if (alpha_new <0) |
alpha_new=0; |
distance=sqrt(((y-y_object)*(y-y_object))+((x_object-x[i])*(x_object-x[i]))); |
sem_wait(&mutex); |
// Cancello braccio telescopio |
if (passi>1) |
{ |
for (k=0; k<9 ; k++) |
{ |
grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],0); |
} |
} |
x_current=x[i]+distance*cos(alpha_new); |
y_current=300-distance*sin(alpha_new); |
// lettura immagine |
grx_getimage(x_current-(dim_x/2),y_current-(dim_y/2),x_current+(dim_x/2),y_current+(dim_y/2),videobuf); |
//aggiunta rumore |
sem_post(&mutex); |
for (k=0;k<dim_x;k++) |
{ |
for(q=0;q<dim_y;q++) |
{ |
int num=0; |
num=rand(); |
num=(num%10)+1; |
if (num>SOGLIA) |
{ |
videobuf[k*dim_x+q]=videobuf[k*dim_x+q]+1; |
} |
} |
} |
sem_wait(&mutex); |
grx_putimage(x[i]-25,y+92,x[i]-25+dim_x-1,y+92+dim_y-1,videobuf); |
sprintf(s,"%3.4f",(180*(alpha_target/pi))); |
grx_text(s, x[i]-25,y+40,12,0); |
sprintf(s,"%3.4f",180*(alpha_new/pi)); |
grx_text(s, x[i]-25,y+50,12,0); |
delta_x=x[i]+20*cos(alpha_new); |
delta_y=y-20*sin(alpha_new); |
for ( k=0; k<9 ; k++) { |
x_current_d[k]=x[i]+50*cos(alpha_new+(k-4)*0.01); |
y_current_d[k]=300-50*sin(alpha_new+(k-4)*0.01); } |
// Disegno braccio telescopio |
for(k=0 ; k<9 ; k++) |
grx_line(delta_x,delta_y,x_current_d[k],y_current_d[k],i+1); |
sem_post(&mutex); |
// scrive immagine nel cab |
p = cab_reserve(cid[i+1]); |
memcpy(p,videobuf,dim_x*dim_y*sizeof(BYTE)); |
cab_putmes(cid[i+1], p); |
task_endcycle(); |
} |
} |
/****************************************************************/ |
/* This is the exception handler. It is called when an exception |
is raised. |
It exits from the graphical mode, then it prints a message and |
shutdown the kernel using sys_abort() |
*/ |
void demo_exc_handler(int signo, siginfo_t *info, void *extra) |
{ |
struct timespec t; |
grx_close(); |
/* Default action for an kern exception is */ |
kern_cli(); |
ll_gettime(TIME_EXACT, &t), |
kern_printf("\nS.Ha.R.K. Exception raised!!!" |
"\nTime (s:ns) :%ld:%ld" |
"\nException number:%d (numbers in include/bits/errno.h)" |
"\nPID :%d\n", |
t.tv_sec, t.tv_nsec, info->si_value.sival_int, |
info->si_task); |
sys_abort(1); |
} |
/******************************************************************/ |
/* This function is called when Alt-X is pressed. |
It simply shutdown the system using sys_end. |
Note that the byebye() function is called only if we exit from |
the system using sys_end()!!!! |
*/ |
void my_end(KEY_EVT* e) |
{ |
sys_end(); |
} |
/******************************************************************/ |
/* This function is called when the system exit correctly after Alt-X. |
It exits from the graphic mode and then it prints a small greeting. |
Note that: |
- The function calls grx_exit, so it must be registered using |
RUNLEVEL_BEFORE_EXIT (RUNLEVEL_AFTER_EXIT does not work because |
at that point the kernel is already returned in real mode!!!) |
- When an exception is raised, the exception handler is called. |
Since the exception handler already exits from the graphic mode, |
this funcion has not to be called. For this reason: |
. we registered byebye using the flag NO_AT_ABORT |
. the exception handler exits using sys_abort; in that way byebye is |
NOT called |
*/ |
void byebye(void *arg) |
{ |
grx_close(); |
kern_printf("Bye Bye!\n"); |
} |
/****************************** MAIN ******************************/ |
int main(int argc, char **argv) |
{ |
PID pid1, pid4,pid5; |
KEY_EVT emerg; |
int i=0; |
int z=0; // contatore per disegno dei pianeti |
HARD_TASK_MODEL m1, m4,m5; |
//FILE *fp; |
struct sigaction action; |
cid[0] = cab_create(cname[0], dim_msg, 5); |
cid[1] = cab_create(cname[1], dim_x*dim_y, 3); |
cid[2] = cab_create(cname[2], dim_x*dim_y, 3); |
cid[3] = cab_create(cname[3], dim_x*dim_y, 3); |
cid[4] = cab_create(cname[4], dim_x*dim_y, 4); |
cid[5] = cab_create(cname[5], dim_x*dim_y, 3); |
cid[6] = cab_create(cname[6], dim_x*dim_y, 3); |
/*fp=fopen("file.txt","r"); |
fscanf(fp,"%d",a); |
fclose(fp); |
*/ |
/* Init the standard S.Ha.R.K. exception handler */ |
action.sa_flags = SA_SIGINFO; /* Set the signal action */ |
action.sa_sigaction = demo_exc_handler; |
action.sa_handler = 0; |
sigfillset(&action.sa_mask); /* we block all the other signals... */ |
if (sigaction(SIGHEXC, &action, NULL) == -1) { /* set the signal */ |
perror("Error initializing signals..."); |
sys_end(); |
} |
/* Set the closing function */ |
sys_atrunlevel(byebye, NULL, RUNLEVEL_BEFORE_EXIT|NO_AT_ABORT); |
/* Initializes the semaphore */ |
sem_init(&mutex,0,1); |
/* graphic card Initialization */ |
if (grx_init() < 1) { |
sys_abort(1); |
} |
if (grx_open(640, 480, 8) < 0) { |
kern_printf("GRX Err\n"); |
sys_abort(1); |
} |
kern_printf("Video card ok!\n"); |
/* set the keyboard handler to exit correctly */ |
emerg.ascii = 'x'; |
emerg.scan = KEY_X; |
emerg.flag = ALTL_BIT; |
keyb_hook(emerg,my_end); |
srand(i+sys_gettime(NULL)); |
emerg.ascii = 'x'; |
emerg.scan = KEY_X; |
emerg.flag = ALTR_BIT; |
keyb_hook(emerg,my_end); |
sem_wait(&mutex); |
/* a small banner */ |
grx_clear(0); |
grx_text("REALTIME TELESCOPES di Marco Bera e Gabriele Varasio",8,8,WHITE,0); |
grx_text("Premere Spazio per creare un telescopio",8,22,WHITE,0); |
grx_text("Premere Alt-X per uscire",8,32,WHITE,0); |
grx_text("Usare il tastierino numerico per muovere l'oggetto",8,42,WHITE,0); |
// DISEGNARE BORDI |
grx_rect(1,1,638,478,2); |
grx_rect(558,1,638,61,2); |
grx_rect(1,1,638,61,2); |
// Disegno sfondo |
for(z=0;z<7;z++) |
{ |
num_p=rand(); |
y_p[z]=100+((num_p%130)+1); |
x_p[z]=50+((num_p%540)+1); |
dim_p[z]=2+((num_p%5)+1); |
col_p[z]=((num_p%10)+1); |
grx_disc(x_p[z],y_p[z],dim_p[z],col_p[z]); |
} |
sem_post(&mutex); |
hard_task_default_model(m4); |
hard_task_def_ctrl_jet (m4); |
hard_task_def_arg (m4, (void *)3); |
hard_task_def_wcet (m4, 19000); |
hard_task_def_mit (m4, PERIOD_T4); |
hard_task_def_group (m4,1); |
pid4 = task_create("move", move, &m4, NULL); |
if (pid4 == NIL) { |
grx_close(); |
perror("Could not create task <move>"); |
sys_abort(1); |
} |
task_activate(pid4); |
hard_task_default_model(m5); |
hard_task_def_ctrl_jet (m5); |
hard_task_def_arg (m5, (void *)3); |
hard_task_def_wcet (m5, 19000); |
hard_task_def_mit (m5, PERIOD_T5); |
hard_task_def_group (m5,1); |
pid5 = task_create("media", media, &m5, NULL); |
if (pid5 == NIL) { |
grx_close(); |
perror("Could not create task <move>"); |
sys_abort(1); |
} |
task_activate(pid5); |
/* and finally we activate the three threads... */ |
do { |
int val_flag=0; |
sem_wait(&mutex_tasto); |
val_flag=flag; |
sem_post(&mutex_tasto); |
if ((val_flag==1) && (i < N_MAX_TELESCOPI)) |
{ |
flag=0; |
hard_task_default_model(m1); |
hard_task_def_ctrl_jet (m1); |
hard_task_def_arg (m1, (void *)i); |
hard_task_def_wcet (m1, WCET_T); |
// hard_task_def_mit (m1, PERIOD_T1); |
hard_task_def_mit (m1, period_t1); |
hard_task_def_group (m1,1); |
pid1 = task_create("tele1", tele, &m1, NULL); |
if (pid1 == NIL) |
{ |
grx_close(); |
perror("Could not create task <tele1>"); |
sys_abort(1); |
} |
task_activate(pid1); |
i++; |
} |
} while (1); |
/* |
now the task main ends, but the system does not shutdown because |
there are others. |
The demo will finish if a Alt-X key is pressed. |
*/ |
return 0; |
} |
/*********** lettura da file ********************/ |
void read_cfg_file(int argc, char **argv) |
{ |
int err; |
DOS_FILE *fp; |
char myfilebuf[1000]; |
int myfilebuf_length; |
if (2) |
{ |
fp = DOS_fopen("dati.cnf","r"); |
if (fp) |
{ |
/* read up to 1000 chars */ |
myfilebuf_length = DOS_fread(&myfilebuf,1,1000,fp); |
/* check for errors */ |
err = DOS_error(); |
cprintf("Read %d bytes...\n", myfilebuf_length); |
if (err) |
{ |
cprintf("Error %d reading file...Using default values\n", err); |
} |
else |
{ |
//geti(myfilebuf, &pos, &NMouses); // Number of Mouses |
sscanf(myfilebuf,"%lf %lf %lf %lf %lf %lf %d",&poli[0],&poli[1],&poli[2],&poli[3],&poli[4],&kp,&period_t1); |
} |
DOS_fclose(fp); |
return; |
} |
else { |
/* error!! */ |
err = DOS_error(); |
/* note that if you call DOS_error() here, it return 0!!! */ |
cprintf("Error %d opening myfile.txt...Using default values\n", err); |
} |
} |
else { |
cprintf("Wrong number of arguments...\n"); |
l1_exit(0); |
} |
} |
/****************************************************************/ |
/demos/trunk/telesco/dati.cnf |
---|
0,0 → 1,3 |
-1 -20 -30 -40 -50 0.5 100000 |
# 5 poli kp(guadagno) periodo telescopio |
/demos/trunk/telesco/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= tel |
include $(BASE)/config/example.mk |
tel: |
make -f $(SUBMAKE) APP=tel INIT= OTHEROBJS="initfile.o" OTHERINCL= SHARKOPT="__OLDCHAR__ __GRX__" |
/demos/trunk/telesco/moon.h |
---|
0,0 → 1,43 |
// Immagine luna |
BYTE luna[900]={ |
0, 0 , 0, 0, 0, 0, 0, 0, 4, 57, 212, 228, 204, 214, 235, 237, 208, 190, 179, 197, 229, 89, 4, 0, |
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 208, 213, 178, 155, 139, 129, 123, 169, 170, 173, 157, |
191, 209, 236, 215, 5, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 36, 103, 146, 116, 93, 143, 119, 96, |
134, 123, 117, 120, 113, 102, 93, 156, 210, 184, 66, 4, 0, 0, 0, 0, 0, 0, 0, 0, 2, 64, 83, 88, |
72, 156, 144, 127, 99, 87, 91, 156, 158, 124, 144, 120, 109, 119, 175, 167, 180, 119, 3, 0, |
0, 0, 0, 0, 0, 5, 56, 59, 63, 58, 138, 104, 71, 46, 55, 68, 79, 89, 172, 146, 141, 152, 110, 126, |
134, 179, 148, 145, 134, 4, 0, 0, 0, 0, 4, 32, 58, 50, 49, 56, 105, 58, 38, 42, 45, 69, 64, 93, |
144, 155, 157, 147, 95, 111, 122, 158, 162, 174, 171, 79, 4, 0, 0, 0, 4, 67, 49, 47, 54, 63, |
51, 55, 43, 38, 48, 62, 55, 86, 89, 145, 92, 60, 99, 72, 97, 130, 184, 169, 195, 171, 3, 0, 0, |
3, 77, 52, 37, 51, 72, 54, 63, 48, 46, 46, 48, 55, 54, 92, 95, 80, 55, 47, 47, 52, 56, 170, 187, |
196, 190, 177, 190, 6, 0, 45, 84, 38, 40, 79, 73, 56, 43, 41, 46, 45, 51, 74, 98, 89, 68, 89, |
46, 40, 48, 45, 38, 171, 176, 152, 165, 133, 157, 107, 3, 154, 42, 34, 42, 68, 66, 47, 52, 62, |
55, 60, 50, 56, 50, 106, 127, 74, 38, 39, 58, 38, 42, 116, 87, 128, 148, 69, 59, 170, 2, 100, |
41, 31, 38, 41, 53, 53, 62, 63, 64, 62, 65, 63, 58, 105, 71, 60, 57, 44, 43, 38, 36, 81, 58, 125, |
205, 61, 43, 130, 37, 88, 39, 31, 33, 43, 52, 76, 66, 97, 116, 101, 96, 90, 56, 76, 47, 43, 135, |
46, 87, 40, 30, 33, 37, 84, 152, 86, 43, 131, 81, 131, 36, 35, 33, 36, 53, 91, 73, 124, 145, |
132, 95, 80, 50, 45, 45, 35, 49, 70, 59, 32, 30, 30, 30, 39, 75, 158, 92, 133, 138, 151, 41, |
39, 38, 39, 82, 125, 59, 89, 98, 84, 64, 68, 40, 104, 107, 85, 78, 115, 75, 34, 43, 39, 36, 47, |
78, 98, 142, 104, 122, 155, 49, 32, 36, 32, 57, 71, 58, 85, 97, 69, 49, 53, 40, 88, 124, 133, |
169, 141, 94, 40, 40, 46, 53, 52, 81, 49, 77, 107, 122, 146, 79, 31, 38, 31, 37, 37, 51, 64, |
59, 84, 72, 64, 96, 88, 132, 153, 158, 154, 149, 140, 109, 77, 125, 120, 54, 51, 59, 123, 86, |
111, 122, 35, 44, 35, 34, 38, 63, 55, 55, 87, 76, 103, 133, 148, 146, 160, 151, 145, 163, 194, |
92, 104, 136, 109, 54, 46, 65, 123, 53, 160, 144, 56, 46, 41, 39, 39, 91, 41, 47, 70, 75, 57, |
112, 116, 140, 168, 179, 161, 164, 188, 123, 100, 145, 162, 87, 51, 111, 140, 5, 172, 148, |
134, 86, 72, 75, 48, 50, 40, 45, 53, 106, 60, 108, 137, 135, 160, 157, 152, 169, 179, 191, |
109, 70, 150, 122, 94, 104, 128, 4, 167, 144, 140, 103, 98, 116, 64, 56, 84, 62, 60, 59, 48, |
80, 150, 143, 151, 187, 161, 147, 178, 219, 79, 69, 163, 132, 87, 108, 128, 0, 83, 195, 195, |
125, 138, 67, 35, 106, 98, 90, 58, 44, 60, 131, 161, 152, 178, 157, 149, 164, 164, 177, 107, |
119, 129, 119, 145, 133, 104, 0, 5, 181, 233, 190, 150, 78, 43, 62, 63, 87, 50, 53, 46, 81, |
157, 187, 155, 181, 189, 145, 166, 211, 170, 137, 166, 189, 188, 164, 5, 0, 0, 5, 167, 175, |
193, 193, 108, 114, 63, 118, 139, 125, 130, 202, 209, 221, 213, 180, 172, 201, 170, 175, |
186, 177, 210, 236, 204, 14, 0, 0, 0, 3, 62, 166, 183, 166, 106, 139, 102, 129, 179, 206, 212, |
240, 232, 224, 210, 208, 196, 173, 179, 177, 186, 191, 227, 185, 100, 2, 0, 0, 0, 0, 5, 125, |
196, 143, 163, 169, 169, 152, 181, 212, 239, 241, 237, 234, 217, 180, 206, 188, 199, 188, |
194, 202, 164, 120, 4, 0, 0, 0, 0, 0, 0, 5, 123, 180, 145, 147, 184, 184, 197, 223, 232, 224, |
227, 227, 220, 190, 179, 192, 192, 197, 204, 165, 120, 4, 0, 0, 0, 0, 0, 0, 0, 0, 5, 74, 162, |
135, 153, 176, 178, 204, 235, 232, 235, 231, 212, 201, 190, 189, 196, 202, 189, 70, 4, 0, |
0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 182, 167, 170, 184, 208, 218, 215, 206, 209, 179, 205, 196, 188, |
184, 193, 12, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 103, 202, 197, 201, 208, 235, 217, 203, |
197, 188, 185, 217, 131, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 5, 89, 130, 218, |
195, 179, 165, 102, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0}; |
/demos/trunk/simcity/makefile |
---|
0,0 → 1,15 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= simcity |
include $(BASE)/config/example.mk |
simcity: |
make -f $(SUBMAKE) APP=simcity INIT= OTHEROBJS="initfile.o keyboard.o keyfunct.o tasks.o reading.o misc.o draw.o proc.o car.o" SHARKOPT="__OLDCHAR__ __GRX__" |
/demos/trunk/simcity/simcity.c |
---|
0,0 → 1,154 |
/************************** Simcity ************************/ |
// Main,initialization functions and global variables allocation |
#include <kernel/func.h> |
#include <string.h> |
#include <stdlib.h> |
#include <drivers/keyb.h> |
#include <drivers/glib.h> |
#include <kernel/kern.h> |
#include <semaphore.h> |
#include <math.h> |
#include "include/constant.h" |
#include "include/misc.h" |
#include "include/draw.h" |
#include "include/proc.h" |
#define rgb rgb16 |
/* graphic mutex... */ |
sem_t mutex; |
//kill flag mutexes |
sem_t kill_mutex[MAX_CAR]; |
// various sprites to use with grx_putimage().... |
BYTE vbuf[MAX_CAR][ROW*COL*2]; |
BYTE clrscr[800*600*2]; |
BYTE clrcam[(ROW+2)*(COL+2)*2]; |
BYTE gauge_img[ROW*COL*2]; |
BYTE brk_gauge[ROW*COL*2]; |
BYTE arrow[3][ROW*COL*2]; |
BYTE street[H*W*2]; |
//task chain pointers |
car_data *free_n,*free_o,*busy_n,*busy_o; |
// various sprites to plot |
extern DWORD macchine[ROW][COL][NCAR]; |
extern DWORD sprites[ROW][COL][NO_SPRITE]; |
extern DWORD strada[H][W]; |
//resolution to use |
WORD r=600; |
WORD c=800; |
BYTE bpp=16; |
// useful colors... |
DWORD white; |
DWORD black; |
DWORD red; |
DWORD gray; |
DWORD blue; |
DWORD green; |
DWORD border; |
DWORD tl_bg; |
//PID vectors |
PID p_table[MAX_CAR]; |
PID c_table[MAX_CAR]; |
PID g_table[MAX_CAR]; |
PID a_table[MAX_CAR]; |
char kill_flag[MAX_CAR]; |
float cosine[360],sine[360]; |
// data structures |
car_data car_data_array[MAX_CAR]; |
tl_data tl_data_array[MAX_TL]; |
starting_set starting_set_array[S_POINT]; |
void keyb_h(void); |
static void version( void ) |
{ |
cprintf("\n\nDemo presented by\n"); |
cprintf("Aguzzi Marco\n"); |
cprintf(" &\n"); |
cprintf("Ferrari Fabio\n"); |
} |
void my_close(void *arg) |
{ |
grx_close(); |
kern_printf("Shutting down SIMCITY\n"); |
} |
int main(int argc, char **argv) |
{ |
int i; |
char tl_name[4]; |
version(); |
sys_atrunlevel(my_close, NULL, RUNLEVEL_BEFORE_EXIT); |
//resetting kill flags |
for(i=0;i<MAX_CAR;i++) { |
p_table[i]=0; |
kill_flag[i]=0; |
} |
// graphic mode initialization |
#ifdef GRAPH |
if (grx_init() < 1) { |
kern_printf("Error initializing graphics\n"); |
sys_abort(1); |
} |
if (grx_open(c,r,bpp) < 0) { |
kern_printf("GRX Open Err\n"); |
sys_abort(1); |
} |
get_images(); |
#endif |
srand(sys_gettime(NULL)); |
//init the graphic mutex |
sem_init(&mutex,1,1); |
//init kill flag mutexes |
for(i=0;i<MAX_CAR;i++) sem_init(&(kill_mutex[i]),1,1); |
//fill sine & cosine lookup tables |
fill_table(); |
/*init keys*/ |
keyb_h(); |
set_start_point(); |
tl_init(); |
init_struct(); |
/* useful colors ... */ |
white = rgb(255,255,255); |
black = rgb(0,0,0); |
red = rgb(255,0,0); |
gray = rgb(210,210,210); |
blue = rgb(0,0,220); |
green = rgb(0,255,0); |
border= rgb(128,128,128); |
tl_bg= rgb(0,128,0); |
#ifdef GRAPH |
/* paint scenario*/ |
draw_scenario(); |
#endif |
#ifndef GRAPH |
cprintf("Main: max_tl:%d\n",MAX_TL); |
#endif |
//creating refresher,killer and traffic light processes... |
ref_create(); |
killer_create(); |
for(i=0;i<MAX_TL;i++) { |
sprintf(tl_name,"tl%d",i+1); |
#ifndef GRAPH |
cprintf("main:tname=%s",tl_name); |
#endif |
stl_create(tl_name,i); |
} |
return 0; |
} |
/demos/trunk/simcity/keyboard.c |
---|
0,0 → 1,49 |
/* ------------------ */ |
/* Keyboard handler */ |
/* ------------------ */ |
#include <drivers/keyb.h> |
#include "include/keyfunct.h" |
void keyb_h() { |
KEY_EVT k; |
keyb_set_map(itaMap); |
/* Exit keys: ALT-X, ENTER */ |
k.flag = ALTL_BIT; |
k.scan = KEY_X; |
k.ascii = 'x'; |
keyb_hook(k, endfun); |
k.flag = ALTR_BIT; |
k.scan = KEY_X; |
k.ascii = 'x'; |
keyb_hook(k, endfun); |
k.flag = 0; |
k.scan = KEY_ENT; |
k.ascii = 13; |
keyb_hook(k, endfun); |
/* Create HARD cars */ |
k.flag = 0; |
k.scan = KEY_C; |
k.ascii = 'c'; |
keyb_hook(k, h_car_create); |
/* kill hard cars*/ |
k.flag = 0; |
k.scan = KEY_K; |
k.ascii = 'k'; |
keyb_hook(k, h_car_kill); |
//refresh map |
k.flag=0; |
k.scan = KEY_R; |
k.ascii = 'r'; |
keyb_hook(k, refresh); |
//create soft cars |
k.flag=0; |
k.scan = KEY_S; |
k.ascii = 's'; |
keyb_hook(k, s_car_create); |
//toggle sensors |
k.flag=0; |
k.scan = KEY_D; |
k.ascii = 'd'; |
keyb_hook(k, sensor_switch); |
} |
/demos/trunk/simcity/include/simcity.h |
---|
0,0 → 1,77 |
/********************************simcity.h***************************/ |
// global variables and tasks bodies declaration |
#include <kernel/func.h> |
#include <semaphore.h> |
#ifndef CST |
#include "constant.h" |
#endif |
/*Global stuff*/ |
extern car_data car_data_array[MAX_CAR]; |
extern tl_data tl_data_array[MAX_TL]; |
//car starting points |
extern starting_set starting_set_array[S_POINT]; |
// PID vectors |
extern PID p_table[MAX_CAR]; |
extern PID c_table[MAX_CAR]; |
extern PID g_table[MAX_CAR]; |
extern PID a_table[MAX_CAR]; |
extern char kill_flag[MAX_CAR]; |
//graphic mutex |
extern sem_t mutex; |
//kill_flag mutexes |
extern sem_t kill_mutex[MAX_CAR]; |
//various sprites |
extern DWORD macchine[ROW][COL][NCAR]; |
extern DWORD strada[H][W]; |
extern BYTE street[H*W*2]; |
extern BYTE vbuf[MAX_CAR][ROW*COL*2]; |
extern BYTE clrcam[(ROW+2)*(COL+2)*2]; |
extern BYTE gauge_img[ROW*COL*2]; |
extern BYTE brk_gauge[ROW*COL*2]; |
extern BYTE arrow[3][ROW*COL*2]; |
extern DWORD sprites[ROW][COL][NO_SPRITE]; |
extern BYTE clrscr[800*600*2]; |
extern DWORD faces[2][74][47]; |
extern char sens; |
//sine & cosine look-up tables |
extern float cosine[360],sine[360]; |
//traffic light sprites |
extern DWORD y_sem[4][SH][SW]; |
extern DWORD r_sem[4][SH][SW]; |
extern DWORD g_sem[4][SH][SW]; |
//useful colors |
extern DWORD white; |
extern DWORD black; |
extern DWORD red; |
extern DWORD gray; |
extern DWORD blue; |
extern DWORD green; |
extern DWORD border; |
extern DWORD tl_bg; |
//task chain pointers |
extern car_data *free_n,*busy_n,*free_o,*busy_o; |
// car counter |
extern short maxc; |
/*function declaration */ |
/* task bodies*/ |
TASK car(car_data *); |
TASK traffic_light(tl_data *); |
TASK camera(int); |
TASK gauge_c(int); |
TASK blink_arrow(int); |
TASK refresher(void *); |
TASK killer(void *); |
/demos/trunk/simcity/include/keyfunct.h |
---|
0,0 → 1,10 |
/*******************************keyfunct.h****************************/ |
// keyboard handler functions |
#include <drivers/keyb.h> |
void h_car_create(KEY_EVT *); |
void s_car_create(KEY_EVT *); |
void h_car_kill(KEY_EVT *); |
void endfun(KEY_EVT *); |
void refresh(KEY_EVT *); |
void sensor_switch(KEY_EVT *); |
/demos/trunk/simcity/include/constant.h |
---|
0,0 → 1,99 |
/*************************const.h*******************************/ |
// constants and new types declaration |
#include <kernel/func.h> |
#define CST |
#define NO_SPRITE 8 |
#define GRAPH |
#define SENSOR |
#define MAX_CAR 10 |
#define MAX_TL 3 |
#define S_POINT 4 |
#define PERIOD_CAR 135000 //period for K62 350MHZ |
//#define PERIOD_CAR 87000 //period for PIII 733MHZ |
#define SPERIOD_CAR 13000 |
#define SCAR_WCET 4300 |
#define SECOND 1000000 |
#define TL_WCET 200 |
#define CAM_WCET 200 |
#define GAUGE_WCET 200 |
#define ARROW_WCET 100 |
//#define CAR_WCET 7700 //wcet for PIII 733MHZ |
#define CAR_WCET 12400 //wcet for K62 350MHZ |
#define ROW 30 |
#define COL 30 |
#define NCAR 24 |
#define W 640 |
#define H 480 |
#define SH 12 |
#define SW 12 |
#define MAX_DIST 50.0 |
#define NORM_DIST 15.0 |
#define COLL_DIST 25.0 |
#define MIN_SPEED 0.0 |
#define NORM_SPEED 4.0 |
#define MAX_SPEED 6.0 |
#define MAX_LOOK 50 |
#define LOOK_ANGLE 70 |
#define DIST_ANGLE 90 |
#define CORR_FACT 0.7 |
#define FRANTIC 0.0 |
#define POS_RAND |
#define OBL_START 1 |
#define DEG_TO_RAD(x) ((float)x)/180.0*3.14 |
#define RAD_TO_DEG(x) ((float)x)/3.14*180.0 |
#define RANDDIR (rand()%3-1) |
#define L 0 |
#define R 1 |
#define U 2 |
#define D 3 |
#define DRAW 1 |
#define CANCEL 0 |
#define VERTICAL 1 |
#define HORIZONTAL 0 |
#define STEER_LEFT 1 |
#define STEER_RIGHT -1 |
#define STRAIGHT 0 |
#define CAMX 670 |
#define CAMY 120 |
#define MAPX 8 |
#define MAPY 113 |
#define FREE 0 |
#define STOP 1 |
#define CAR 2 |
/*data types*/ |
typedef struct car_d { |
int number; |
float xpos,ypos; |
int xp,yp; |
float dist_obs,dist_sem; |
float speed,middle,front,rear,somma; |
int angle,sat; |
char collision,boom,correggi,incrocio; |
char dir,running; |
struct car_d *next; |
} car_data; |
typedef struct { |
int x,y; |
} point; |
typedef struct { |
char vpos,hpos,tl_name[8]; |
point l,r,u,d; |
PID pid; |
DWORD period; |
} tl_data; |
typedef struct { |
int xpos,ypos,angles; |
} starting_set; |
/demos/trunk/simcity/include/misc.h |
---|
0,0 → 1,18 |
//*****************************misc.h************************************ |
// miscellaneous useful functions |
#ifndef CST |
#include "constant.h" |
#endif |
int my_rint(float); |
void fill_table(); |
void tl_init(); |
void set_start_point(); |
int returnCarIndex(int); |
int find_col(int,int); |
int find_tl(int,int,int); |
int allinea(int); |
int module(int,int,int,int); |
int normalize(int); |
/demos/trunk/simcity/include/draw.h |
---|
0,0 → 1,14 |
//****************************draw.h******************************* |
// drawing functions |
#ifndef CST |
#include "constant.h" |
#endif |
void drawSprites(int,int,char,char); |
void drawStreet(int,int); |
void get_images(); |
void drawCar(int,int,int,char); |
void bold_rect(WORD,WORD,WORD,WORD,BYTE,BYTE,BYTE); |
void draw_scenario(); |
void draw_tl(int x,int y,DWORD img[SH][SW]); |
void drawFaces(int,int,short); |
/demos/trunk/simcity/include/states.h |
---|
0,0 → 1,21 |
enum stati { |
NORM, |
INC1, |
DX1, |
STR1, |
STR2, |
SX1, |
SX2, |
SX3 |
} ; |
enum sprite_type { |
EXPL, |
GAUGE, |
ARR_OFF, |
ARR_SX, |
ARR_DX, |
VECCHINA, |
V_SPLAT, |
GAUGE_ROTTO |
}; |
/demos/trunk/simcity/include/proc.h |
---|
0,0 → 1,23 |
//***************************proc.h****************************** |
// task management functions |
#ifndef CST |
#include "constant.h" |
#endif |
int add(); |
void del(int); |
void init_struct(); |
int del_o(); |
void gauge_create(char *,int); |
void arrow_create(char *,int); |
void killer_create(); |
void ref_create(); |
void cam_create(char *,int); |
void stl_create(char *,int); |
void h_create(char *,int); |
void s_create(char *,int); |
void killing(int); |
/demos/trunk/simcity/include/car.h |
---|
0,0 → 1,10 |
//*****************************car.h******************************** |
// car management functions |
#ifndef CST |
#include "constant.h" |
#endif |
char collision_sensor(car_data *); |
int sensore(car_data *); |
void ch_spd(car_data *,char); |
/demos/trunk/simcity/initfile.c |
---|
0,0 → 1,106 |
//****************************initfile.c******************************* |
#define PI |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#ifndef PI |
#include "modules/rrsoft.h" |
#endif |
#include "modules/dummy.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/cabs.h" |
#include "modules/pi.h" |
#include "modules/pc.h" |
#include "modules/srp.h" |
#include "modules/npp.h" |
#include "modules/nop.h" |
#include <drivers/keyb.h> |
//#include "ll/i386/x-dos.h" |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
//#define RRTICK 10000 |
#define RRTICK 2000 |
void getCars(void); |
void getRoad(void); |
void getSem(void); |
void getSprites(void); |
void getFaces(void); |
void tl_pos(); |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 0); |
// RRSOFT_register_level(RRTICK, RR_MAIN_NO, mb, RRSOFT_ONLY_HARD|RRSOFT_ONLY_SOFT); |
// RRSOFT_register_level(RRTICK, RR_MAIN_NO, mb, RRSOFT_ONLY_SOFT); //cbs |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
CABS_register_module(); |
PI_register_module(); |
NOP_register_module(); |
getFaces(); |
getCars(); |
getRoad(); |
getSem(); |
getSprites(); |
tl_pos(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
HARTPORT_init(); |
KEYB_init(NULL); |
__call_main__(mb); |
return (void *)0; |
} |
#ifdef PI |
void app_mutex_init(mutex_t *m) |
{ |
PI_mutexattr_t attr; |
PI_mutexattr_default(attr); |
mutex_init(m, &attr); |
} |
#else |
void app_mutex_init(mutex_t *m) |
{ |
NOP_mutexattr_t attr; |
NOP_mutexattr_default(attr); |
mutex_init(m, &attr); |
} |
#endif |
/demos/trunk/simcity/car.c |
---|
0,0 → 1,175 |
#include <drivers/glib.h> |
#include <stdlib.h> |
#include <math.h> |
#include "include/simcity.h" |
#include "include/misc.h" |
#include "include/car.h" |
#include "include/draw.h" |
char collision_sensor(car_data *cd) { |
float i,dx,dx1,dy,dy1,deltax,deltay; |
int colore,colore1; |
int speed_done=0; |
char col=FREE; |
dx=cd->xpos+2.0*cosine[normalize(cd->angle+90)]; |
dy=cd->ypos-2.0*sine[normalize(cd->angle+90)]; |
dx1=cd->xpos-8.0*cosine[normalize(cd->angle+90)]; |
dy1=cd->ypos+8.0*sine[normalize(cd->angle+90)]; |
for (i=0.0;i<MAX_LOOK;i+=0.1) { |
deltax=i*cosine[normalize(cd->angle)]; |
deltay=i*sine[normalize(cd->angle)]; |
sem_wait(&mutex); |
colore=grx_getpixel(my_rint(dx+deltax),my_rint(dy-deltay)); |
colore1=grx_getpixel(my_rint(dx1+deltax),my_rint(dy1-deltay)); |
sem_post(&mutex); |
if (((colore==white)||(colore1==white) || ((colore==red)||(colore1==red)))) { |
if((colore==white)||(colore1==white)) |
col=STOP; |
else |
col=CAR; |
cd->dist_obs=i; |
cd->collision=1; |
speed_done=1; |
} |
if (!speed_done) { |
if (sens) { |
sem_wait(&mutex); |
grx_plot(my_rint(dx+deltax),my_rint(dy-deltay),gray); |
grx_plot(my_rint(dx1+deltax),my_rint(dy1-deltay),gray); |
sem_post(&mutex); |
} |
} |
} |
if (!speed_done) { |
cd->collision=0; |
cd->dist_obs=MAX_LOOK; |
} |
return col; |
} |
int sensore(car_data *cd) { |
int col_front,col_rear,col_middle; |
int done_front=0,done_rear=0,done_middle=0; |
float front,rear,middle,store_middle; |
int angolo; |
float i; |
float x1,y1,x05,y05; |
float xs,ys,xs1,ys1; |
float coordx,coordy,distx,disty; |
cd->front=cd->rear=cd->middle=front=rear=middle=store_middle=NORM_DIST; |
x1=cd->xpos-10.0*cosine[normalize(cd->angle)]; // coordinata sensore dietro |
y1=cd->ypos+10.0*sine[normalize(cd->angle)]; |
x05=cd->xpos-5.0*cosine[normalize(cd->angle)]; // coordinata sensore mezzo |
y05=cd->ypos+5.0*sine[normalize(cd->angle)]; |
for (i=0.0;i<MAX_DIST;i+=0.1) { |
coordx=i*cosine[normalize(cd->angle-LOOK_ANGLE)]; |
coordy=i*sine[normalize(cd->angle-LOOK_ANGLE)]; |
distx=i*cosine[normalize(cd->angle-DIST_ANGLE)]; |
disty=i*sine[normalize(cd->angle-DIST_ANGLE)]; |
col_front=grx_getpixel(my_rint(cd->xpos+coordx),my_rint(cd->ypos-coordy)); |
col_rear=grx_getpixel(my_rint(x1+coordx),my_rint(y1-coordy)); |
col_middle=grx_getpixel(my_rint(x05+distx),my_rint(y05-disty)); |
if ((col_front==border) && !done_front) { |
done_front=1; |
cd->front=front=i; |
} |
if ((col_middle==border) && !done_middle) { |
done_middle=1; |
middle=cd->middle=i; |
} |
if ((col_rear==border) && !done_rear) { |
done_rear=1; |
cd->rear=rear=i; |
} |
if (sens) { |
sem_wait(&mutex); |
if (!done_front) { |
grx_plot(my_rint(cd->xpos+coordx),my_rint(cd->ypos-coordy),gray); |
} |
if (!done_rear) { |
grx_plot(my_rint(x1+coordx),my_rint(y1-coordy),blue); |
} |
if (!done_middle) { |
grx_plot(my_rint(x05+distx),my_rint(y05-disty),green); |
} |
sem_post(&mutex); |
} |
} |
if (!done_front) { |
front=cd->front=MAX_DIST; |
} |
if (!done_rear) { |
rear=cd->rear=MAX_DIST; |
} |
if (!done_middle) { |
cd->middle=middle=MAX_DIST; |
} |
xs=cd->xpos+front*cosine[normalize(cd->angle-LOOK_ANGLE)]; |
xs1=x1+rear*cosine[normalize(cd->angle-LOOK_ANGLE)]; |
ys=cd->ypos-front*sine[normalize(cd->angle-LOOK_ANGLE)]; |
ys1=y1-rear*sine[normalize(cd->angle-LOOK_ANGLE)]; |
if (xs==xs1) { |
angolo=90; |
} else { |
angolo=my_rint(RAD_TO_DEG(atan((-ys1+ys)/(xs1-xs)))); |
} |
if (angolo<0) angolo+=360; |
if (angolo>=360) angolo-=360; |
if ((ys-ys1<0) && (xs1-xs>0)) { |
angolo-=180; |
} |
if ((ys-ys1>=0) && (xs1-xs>0)) { |
angolo+=180; |
} |
if ((xs1-xs==0) && (ys-ys1>0)) { |
angolo=270; |
} |
if (abs(middle-NORM_DIST)>FRANTIC) { |
cd->correggi=1; |
} else { |
cd->somma=0; |
cd->correggi=0; |
} |
if (cd->correggi) { |
if (middle>NORM_DIST) { |
cd->somma-=CORR_FACT; |
} |
if (middle<NORM_DIST) { |
cd->somma+=CORR_FACT; |
} |
} |
angolo+=cd->somma; |
return angolo; |
} |
void ch_spd(car_data *cd,char ob_type) { |
float set_spd=0.0; |
float bf; |
if(cd->collision) { |
if(ob_type==STOP) |
bf=0.001; |
else |
bf=0.7; |
set_spd=cd->speed-(MAX_LOOK-cd->dist_obs)*bf; |
if(set_spd<0) |
set_spd=0; |
} |
else { |
set_spd=cd->speed+0.4; |
if(set_spd>MAX_SPEED) |
set_spd=MAX_SPEED; |
} |
cd->speed=set_spd; |
} |
/demos/trunk/simcity/tasks.c |
---|
0,0 → 1,409 |
//*******************************tasks.c************************************ |
// task bodies |
#include <drivers/glib.h> |
#include <stdlib.h> |
#include <string.h> |
#include <kernel/func.h> |
#include <drivers/glib.h> |
#include <semaphore.h> |
#include <math.h> |
#include "include/simcity.h" |
#include "include/states.h" |
#include "include/draw.h" |
#include "include/misc.h" |
#include "include/car.h" |
#include "include/proc.h" |
#define rgb rgb16 |
/*extern starting_set starting_set_array[S_POINT]; |
extern short maxc; */ |
TASK killer(void *arg) { |
short i,j=0; |
DWORD col[3]; |
char flag=0; |
col[0]=red; |
col[1]=green; |
col[2]=white; |
while(1) { |
if(maxc>0) { |
for(i=0;i<MAX_CAR;i++) { |
sem_wait(&(kill_mutex[i])); |
if(car_data_array[i].boom && car_data_array[i].running) flag=1; |
else flag=0; |
sem_post(&(kill_mutex[i])); |
if(flag) { |
del(i); |
killing(i); |
} |
} |
} |
j=(j+1)%3; |
sem_wait(&mutex); |
grx_text("Killer active!",CAMX,CAMY+MAX_CAR*46,col[j],black); |
sem_post(&mutex); |
task_endcycle(); |
} |
} |
TASK refresher(void *arg) { |
int i; |
tl_data *d; |
while(1) { |
sem_wait(&mutex); |
grx_putimage(MAPX,MAPY,MAPX+W-1,MAPY+H-1,street); |
for(i=0;i<MAX_TL;i++) { |
d=&(tl_data_array[i]); |
if((d->vpos=='g')) { |
draw_tl(d->u.x,d->u.y,g_sem[U]); |
draw_tl(d->d.x,d->d.y,g_sem[D]); |
draw_tl(d->l.x,d->l.y,r_sem[L]); |
draw_tl(d->r.x,d->r.y,r_sem[R]); |
} else if(d->vpos=='y') { |
draw_tl(d->u.x,d->u.y,y_sem[U]); |
draw_tl(d->d.x,d->d.y,y_sem[D]); |
draw_tl(d->l.x,d->l.y,r_sem[L]); |
draw_tl(d->r.x,d->r.y,r_sem[R]); |
} else if(d->hpos=='g') { |
draw_tl(d->u.x,d->u.y,r_sem[U]); |
draw_tl(d->d.x,d->d.y,r_sem[D]); |
draw_tl(d->l.x,d->l.y,g_sem[L]); |
draw_tl(d->r.x,d->r.y,g_sem[R]); |
} else if(d->hpos=='y') { |
draw_tl(d->u.x,d->u.y,r_sem[U]); |
draw_tl(d->d.x,d->d.y,r_sem[D]); |
draw_tl(d->l.x,d->l.y,y_sem[L]); |
draw_tl(d->r.x,d->r.y,y_sem[R]); |
} |
} |
sem_post(&mutex); |
task_endcycle(); |
} |
} |
TASK blink_arrow(int number) { |
char ld=0,prev=0,dir=0; |
short c; |
BYTE *arrow_img; |
c=number*46; |
while(1) { |
if(kill_flag[number]) { |
return 0; |
} |
if(!car_data_array[number].boom) { |
dir=car_data_array[number].dir+1; |
if(dir==prev) { |
ld=(ld+1)%2; |
if(ld==1) |
arrow_img=arrow[(int)dir]; |
else |
arrow_img=arrow[1]; |
sem_wait(&mutex); |
grx_putimage(CAMX+80,CAMY-1+c,CAMX+80+29,CAMY-1+c+29,arrow_img); |
sem_post(&mutex); |
} else { |
prev=dir; |
ld=0; |
sem_wait(&mutex); |
grx_putimage(CAMX+80,CAMY-1+c,CAMX+80+29,CAMY-1+c+29,arrow[(int)dir]); |
sem_post(&mutex); |
} |
} |
task_endcycle(); |
} |
} |
TASK gauge_c(int number) { |
int c; |
float angle,ro; |
int x,y; |
char vel[5]; |
c=number*46; |
while(1) { |
if(kill_flag[number]) { |
return 0; |
} |
if(!car_data_array[number].boom) { |
angle=180.0*car_data_array[number].speed/MAX_SPEED; |
sprintf(vel,"%.1f",car_data_array[number].speed); |
ro=9.0; |
y=my_rint(ro*sine[my_rint(angle)]); |
x=my_rint(ro*cosine[my_rint(angle)]); |
sem_wait(&mutex); |
grx_putimage(CAMX,CAMY+c,CAMX+29,CAMY+29+c,gauge_img); |
grx_line(CAMX+14,CAMY-1+c+25,CAMX+14-x,CAMY-1+c+25-y,blue); |
grx_text(vel,CAMX,CAMY+c+30,white,black); |
sem_post(&mutex); |
} else { |
sem_wait(&mutex); |
grx_putimage(CAMX,CAMY+c,CAMX+29,CAMY+29+c,brk_gauge); |
sem_post(&mutex); |
} |
task_endcycle(); |
} |
} |
TASK camera(int number) { |
int c; |
c=number*46; |
grx_rect(CAMX+40-1,CAMY-1+c,CAMX+40+30,CAMY+30+c,green); |
while(1) { |
if(kill_flag[number]) { |
return 0; |
} |
sem_wait(&mutex); |
grx_putimage(CAMX+40,CAMY+c,CAMX+40+29,CAMY+c+29,vbuf[number]); |
sem_post(&mutex); |
task_endcycle(); |
} |
} |
TASK traffic_light(tl_data *d) { |
char hpos,vpos,flag; |
flag=0; |
vpos=d->vpos; |
if(vpos=='g') { |
hpos='r'; |
#ifdef GRAPH |
sem_wait(&mutex); |
draw_tl(d->l.x,d->l.y,r_sem[L]); |
draw_tl(d->r.x,d->r.y,r_sem[R]); |
draw_tl(d->u.x,d->u.y,g_sem[U]); |
draw_tl(d->d.x,d->d.y,g_sem[D]); |
sem_post(&mutex); |
#endif |
} else { |
vpos='r'; |
hpos='g'; |
#ifdef GRAPH |
sem_wait(&mutex); |
draw_tl(d->l.x,d->l.y,g_sem[L]); |
draw_tl(d->r.x,d->r.y,g_sem[R]); |
draw_tl(d->u.x,d->u.y,r_sem[U]); |
draw_tl(d->d.x,d->d.y,r_sem[D]); |
sem_post(&mutex); |
#endif |
} |
task_endcycle(); |
while(1) { |
if(vpos=='g') { |
vpos='y'; |
flag=1; |
#ifdef GRAPH |
sem_wait(&mutex); |
draw_tl(d->u.x,d->u.y,y_sem[U]); |
draw_tl(d->d.x,d->d.y,y_sem[D]); |
sem_post(&mutex); |
#endif |
} |
if((vpos=='y')&&(!flag)) { |
vpos='r'; |
hpos='g'; |
flag=1; |
#ifdef GRAPH |
sem_wait(&mutex); |
draw_tl(d->u.x,d->u.y,r_sem[U]); |
draw_tl(d->d.x,d->d.y,r_sem[D]); |
draw_tl(d->l.x,d->l.y,g_sem[L]); |
draw_tl(d->r.x,d->r.y,g_sem[R]); |
sem_post(&mutex); |
#endif |
} |
if((vpos=='r')&&(!flag)) { |
if(hpos=='g') { |
hpos='y'; |
flag=1; |
#ifdef GRAPH |
sem_wait(&mutex); |
draw_tl(d->l.x,d->l.y,y_sem[L]); |
draw_tl(d->r.x,d->r.y,y_sem[R]); |
sem_post(&mutex); |
#endif |
} |
if((hpos=='y')&&(!flag)) { |
hpos='r'; |
vpos='g'; |
#ifdef GRAPH |
sem_wait(&mutex); |
draw_tl(d->l.x,d->l.y,r_sem[L]); |
draw_tl(d->r.x,d->r.y,r_sem[R]); |
draw_tl(d->u.x,d->u.y,g_sem[U]); |
draw_tl(d->d.x,d->d.y,g_sem[D]); |
sem_post(&mutex); |
#endif |
} |
} |
flag=0; |
d->vpos=vpos; |
d->hpos=hpos; |
#ifndef GRAPH |
sem_wait(&mutex); |
cprintf("semaforo %s: verticale:%c orizz.:%c\n",d->tl_name,vpos,hpos); |
sem_post(&mutex); |
#endif |
task_endcycle(); |
} |
} |
TASK car(car_data *cd) { |
char ob_type=FREE; |
int start_point,sat_angle=0; |
int tl=0; |
short stato=NORM,dir_flag=0; |
sem_wait(&(kill_mutex[cd->number])); |
while(kill_flag[cd->number]){ |
cd->running=0; |
sem_post(&(kill_mutex[cd->number])); |
return 0; |
} |
sem_post(&(kill_mutex[cd->number])); |
while(1) { |
sem_wait(&(kill_mutex[cd->number])); |
while(kill_flag[cd->number]){ |
cd->running=0; |
sem_post(&(kill_mutex[cd->number])); |
return 0; |
} |
sem_post(&(kill_mutex[cd->number])); |
if(cd->xp>630||cd->yp>590||cd->xp<18||cd->yp<MAPY+3) { |
start_point=rand()%S_POINT; |
if(start_point==2) |
start_point++; |
cd->xpos=cd->xp=starting_set_array[start_point].xpos; |
cd->ypos=cd->yp=starting_set_array[start_point].ypos; |
cd->angle=starting_set_array[start_point].angles; |
} |
if (!cd->boom) { |
// Control State Machine |
if(stato==NORM) { |
cd->dir=STRAIGHT; |
ob_type=collision_sensor(cd); |
ch_spd(cd,ob_type); |
if(cd->dist_obs<30 && ob_type==STOP) { |
stato=INC1; |
} |
if(ob_type==FREE || cd->dist_obs>=27 || cd->speed>=2.0) { |
cd->angle=sensore(cd); |
cd->angle=normalize(cd->angle); |
} |
} |
if(stato==INC1) { |
tl=find_tl(cd->angle,cd->xp,cd->yp); |
if(!dir_flag) |
cd->dir=rand()%3-1; |
if(find_col(cd->angle,tl)=='g') { |
switch(cd->dir) { |
case STEER_RIGHT: |
stato=DX1; |
break; |
case STRAIGHT: |
case STEER_LEFT: |
stato=STR1; |
break; |
default: |
stato=DX1; |
} |
} else { |
cd->speed=0.0; |
dir_flag=1; |
} |
} |
if(stato==DX1) { |
dir_flag=0; |
cd->speed=4.0; |
cd->angle=sensore(cd); |
if(ob_type!=STOP) |
stato=NORM; |
else { |
ob_type=collision_sensor(cd); |
if(ob_type==CAR) |
ch_spd(cd,ob_type); |
} |
} |
if(stato==STR1) { |
dir_flag=0; |
cd->angle=allinea(cd->angle); |
sensore(cd); |
cd->speed=4.0; |
ob_type=collision_sensor(cd); |
if(ob_type==CAR) |
ch_spd(cd,ob_type); |
if(cd->middle==MAX_DIST) { |
if(cd->dir==STRAIGHT) |
stato=STR2; |
else { |
sat_angle=cd->angle+90; |
stato=SX1; |
} |
} |
} |
if(stato==STR2) { |
cd->speed=4.0; |
sensore(cd); |
ob_type=collision_sensor(cd); |
if(ob_type==CAR) |
ch_spd(cd,ob_type); |
if(abs(cd->front-cd->rear)<0.1 && ((cd->middle-NORM_DIST)<7.0)) |
stato=NORM; |
} |
if(stato==SX1) { |
cd->speed=4.0; |
sensore(cd); |
ob_type=collision_sensor(cd); |
if(ob_type==CAR) |
ch_spd(cd,ob_type); |
else { |
cd->angle+=7; |
if(cd->angle>=sat_angle) { |
cd->angle=sat_angle; |
} |
} |
if(abs(cd->front-cd->rear)<0.1 && ((cd->middle-NORM_DIST)<7.0)) |
stato=NORM; |
} |
} |
if (cd->dist_obs<COLL_DIST && ob_type==CAR) { |
cd->boom=1; |
cd->speed=0.0; |
} |
sem_wait(&mutex); |
grx_getimage(cd->xp-15,cd->yp-15,cd->xp+14,cd->yp+14,vbuf[cd->number]); |
if (cd->boom==0) { |
drawCar(cd->xp,cd->yp,cd->angle,DRAW); |
} |
if (cd->boom==1) { |
drawSprites(cd->xp,cd->yp,0,DRAW); |
} |
sem_post(&mutex); |
task_endcycle(); |
sem_wait(&mutex); |
if (cd->boom==0) { |
grx_putimage(cd->xp-15,cd->yp-15,cd->xp+14,cd->yp+14,vbuf[cd->number]); |
} else { |
drawSprites(cd->xp,cd->yp,0,CANCEL); |
} |
sem_post(&mutex); |
sem_wait(&(kill_mutex[cd->number])); |
while(kill_flag[cd->number]){ |
cd->running=0; |
sem_post(&(kill_mutex[cd->number])); |
return 0; |
} |
sem_post(&(kill_mutex[cd->number])); |
cd->xpos+=cd->speed*cos(DEG_TO_RAD(cd->angle)); |
cd->ypos-=cd->speed*sin(DEG_TO_RAD(cd->angle)); |
cd->xp=my_rint(cd->xpos); |
cd->yp=my_rint(cd->ypos); |
} |
} |
/demos/trunk/simcity/keyfunct.c |
---|
0,0 → 1,85 |
/**************** Functions called by keyboard handler*********/ |
#include <drivers/keyb.h> |
#include <drivers/glib.h> |
#include <drivers/glib.h> |
#include <stdlib.h> |
#include <string.h> |
#include "include/simcity.h" |
#include "include/proc.h" |
short maxc=0; |
char sens=0; |
extern starting_set starting_set_array[S_POINT]; |
void h_car_create(KEY_EVT *k) |
{ |
char name[10]; |
int num; |
if((maxc<MAX_CAR)) { |
num=add(); |
if(num>=0) { |
sprintf(name,"car%d",num); |
h_create(name,num); |
sprintf(name,"camera%d",num); |
cam_create(name,num); |
sprintf(name,"speed%d",num); |
gauge_create(name,num); |
sprintf(name,"arrow%d",num); |
arrow_create(name,num); |
} |
} |
} |
void s_car_create(KEY_EVT *k) |
{ |
char name[10]; |
int num; |
if((maxc<MAX_CAR)) { |
num=add(); |
if(num>=0) { |
sprintf(name,"car%d",num); |
h_create(name,num); |
sprintf(name,"camera%d",num); |
cam_create(name,num); |
sprintf(name,"speed%d",num); |
gauge_create(name,num); |
sprintf(name,"arrow%d",num); |
arrow_create(name,num); |
} |
} |
} |
void h_car_kill(KEY_EVT *k) { |
int num; |
if(maxc>0) { |
num=del_o(); |
if(num>=0) { |
killing(num); |
} |
} |
} |
void endfun(KEY_EVT *k) |
{ |
grx_close(); |
cprintf("Brk command pressed! Ending...\n"); |
sys_end(); |
} |
void refresh(KEY_EVT *k){ |
sem_wait(&mutex); |
//heavy and obsolete..... |
//draw_scenario(); |
grx_putimage(MAPX,MAPY,MAPX+W-1,MAPY+H-1,street); |
sem_post(&mutex); |
} |
void sensor_switch(KEY_EVT *k) { |
if(sens==0) sens=1; |
else sens=0; |
} |
/demos/trunk/simcity/misc.c |
---|
0,0 → 1,215 |
#include <math.h> |
#include "include/misc.h" |
#include "include/simcity.h" |
int my_rint(float num) { |
return (int)floor(num+0.5); |
} |
void fill_table(void) { |
int i; |
for (i=0;i<360;i++) { |
sine[i]=sin(DEG_TO_RAD(i)); |
cosine[i]=cos(DEG_TO_RAD(i)); |
} |
} |
void tl_init() { |
//MAPX,MAPY |
char col[2]; |
short i; |
col[0]='r'; |
col[1]='g'; |
for(i=0;i<MAX_TL;i++) { |
tl_data_array[i].vpos=col[i%2]; |
tl_data_array[i].period=(i+3)*SECOND; |
tl_data_array[i].l.x+=MAPX; |
tl_data_array[i].l.y+=MAPY; |
tl_data_array[i].r.x+=MAPX; |
tl_data_array[i].r.y+=MAPY; |
tl_data_array[i].u.x+=MAPX; |
tl_data_array[i].u.y+=MAPY; |
tl_data_array[i].d.x+=MAPX; |
tl_data_array[i].d.y+=MAPY; |
} |
} |
void set_start_point() { |
//MAPX,MAPY |
starting_set_array[0].xpos=607+MAPX; |
starting_set_array[0].ypos=20+MAPY; |
starting_set_array[0].angles=180; |
starting_set_array[1].xpos=12+MAPX; |
starting_set_array[1].ypos=62+MAPY; |
starting_set_array[1].angles=0; |
starting_set_array[2].xpos=260+MAPX; |
starting_set_array[2].ypos=460+MAPY; |
starting_set_array[2].angles=90; |
starting_set_array[3].xpos=605+MAPX; |
starting_set_array[3].ypos=205+MAPY; |
starting_set_array[3].angles=180; |
} |
int returnCarIndex(int a) { |
int indice=-1; |
int angle; |
angle=normalize(a); |
if (((angle>=0) && (angle<=8)) || ((angle>353) && (angle<360))) { // 0 (+8 ; -7) |
indice=0; |
} |
if ((angle<=23) && (angle>8)) { // 15 |
indice=1; |
} |
if ((angle<=38) && (angle>23)) { // 30 |
indice=2; |
} |
if ((angle<=53) && (angle>38)) { // 45 |
indice=3; |
} |
if ((angle<=68) && (angle>53)) { // 60 |
indice=4; |
} |
if ((angle<=83) && (angle>68)) { // 75 |
indice=5; |
} |
if ((angle<=98) && (angle>83)) { // 90 |
indice=6; |
} |
if ((angle<=113) && (angle>98)) { // 105 |
indice=7; |
} |
if ((angle<=128) && (angle>113)) { // 120 |
indice=8; |
} |
if ((angle<=143) && (angle>128)) { // 135 |
indice=9; |
} |
if ((angle<=158) && (angle>143)) { // 150 |
indice=10; |
} |
if ((angle<=173) && (angle>158)) { // 165 |
indice=11; |
} |
if ((angle<=188) && (angle>173)) { // 180 |
indice=12; |
} |
if ((angle<=203) && (angle>188)) { // 195 |
indice=13; |
} |
if ((angle<=218) && (angle>203)) { // 210 |
indice=14; |
} |
if ((angle<=233) && (angle>218)) { // 225 |
indice=15; |
} |
if ((angle<=248) && (angle>233)) { // 240 |
indice=16; |
} |
if ((angle<=263) && (angle>248)) { // 255 |
indice=17; |
} |
if ((angle<=278) && (angle>263)) { // 270 |
indice=18; |
} |
if ((angle<=293) && (angle>278)) { // 285 |
indice=19; |
} |
if ((angle<=308) && (angle>293)) { // 300 |
indice=20; |
} |
if ((angle<=323) && (angle>308)) { // 315 |
indice=21; |
} |
if ((angle<=338) && (angle>323)) { // 330 |
indice=22; |
} |
if ((angle<=353) && (angle>338)) { // 345 |
indice=23; |
} |
if (angle==360) { |
indice=0; |
} |
return indice; |
} |
int normalize(int angle) { |
if(angle<0) |
return ((angle+360)); |
if(angle>=360) |
return ((angle-360)); |
return angle; |
} |
int module(int x1,int x2,int y1,int y2) { |
int x,y; |
x=x1-x2; |
y=y1-y2; |
x*=x; |
y*=y; |
return x+y; |
} |
int allinea(int angle) { |
int old_angle; |
old_angle=normalize(angle); |
if(old_angle<45) |
old_angle=0; |
else if(old_angle<135) |
old_angle=90; |
else if(old_angle<225) |
old_angle=180; |
else |
old_angle=270; |
return old_angle; |
} |
int find_tl(int angle,int xp,int yp) { |
int a,min_tl,old_min,mins,i; |
a=allinea(angle); |
min_tl=old_min=10000; |
for(mins=i=0;i<MAX_TL;i++) { |
switch(a) { |
case 0: |
min_tl=module( xp,tl_data_array[i].l.x, yp,tl_data_array[i].l.y); |
break; |
case 90: |
min_tl=module( xp,tl_data_array[i].d.x, yp,tl_data_array[i].d.y); |
break; |
case 180: |
min_tl=module( xp,tl_data_array[i].r.x, yp,tl_data_array[i].r.y); |
break; |
case 270: |
min_tl=module( xp,tl_data_array[i].u.x, yp,tl_data_array[i].u.y); |
break; |
default: |
break; |
} |
if(min_tl<old_min) { |
old_min=min_tl; |
mins=i; |
} |
} |
return mins; |
} |
int find_col(int angle,int tl) { |
int min_tl=0,a; |
a=allinea(angle); |
switch(a) { |
case 0: |
case 180: |
min_tl=tl_data_array[tl].hpos; |
break; |
case 90: |
case 270: |
min_tl=tl_data_array[tl].vpos; |
break; |
} |
return min_tl; |
} |
/demos/trunk/simcity/sem/green_l.raw |
---|
0,0 → 1,0 |