WebSVN - shark - Rev 1669 - /unsupported/trunk/imit2004/atcsim/atc.c

#include <kernel/kern.h> //include the SHARK kernel header
#include <drivers/glib.h> //include the graphics library
#include <drivers/keyb.h> //include the keyboard library
#include <semaphore.h> //include the semaphore library
#include <stdlib.h> //include the standard library
#include <math.h> //include the math library for random
#include <string.h>

#ifndef _ATCSIM_HEADER_
#include "atcsim.h"
#endif

#ifndef _SIM_AREA_HEADER_
#include "sim_area.h"
#endif

#ifndef _INPUT_
#include "input.h"
#endif

#ifndef _CONST_
#include "constants.h"
#endif

extern sem_t mut_planeinzone[MAX_ATCS][MAX_PLANES];
char *plane_cab_ptr1[MAX_PLANES];
aeroplane cabmsg_aeroplanes1[MAX_PLANES];
PID tid[MAX_ATCS];

extern int STARTSIM;
extern int get_i();
extern aeroplane aeroplanes[MAX_PLANES];

extern airport airports[NUM_OF_AIRPORTS];
extern atc atcs[MAX_ATCS];
extern mutex_t mut_number_of_planes;

aeroplane cabmsg_aeroplanes2[MAX_PLANES];
atcplanecab cabmsg_atc[MAX_PLANES];

int planeinzone[MAX_ATCS][MAX_PLANES];

void DoesCollide(int myid,int no_of_planes)
{
int id,id2;
int currx1,curry1,destx1,desty1,speed1,currx2,curry2,destx2,desty2,speed2,srcx2,srcy2,srcx1,srcy1;

int src_apid1,dest_apid2,tempx1,tempx2,tempy1,tempy2,resx,resy;
int gxmin,gymin,gxmax,gymax;

int cx1mmd,cx1pmd,cx2mmd,cx2pmd,cy1mmd,cy1pmd,cy2mmd,cy2pmd;
float pdistance,a1,a2,b1,b2,c1,c2,d1,d2,delta;
char *plane_cab_ptr2[MAX_PLANES];
// char *atc_cab_ptr1,*atc_cab_ptr2;

char temp[100];

int finalx1=-1,finalx2=-1,finaly1=-1,finaly2=-1;

for(id=0;id<no_of_planes;id++)
{
plane_cab_ptr2[id]=cab_getmes(plane_atc_cab_id[id]);
memcpy(&cabmsg_aeroplanes2[id],plane_cab_ptr2[id],sizeof(aeroplane));
cab_unget(plane_atc_cab_id[id],plane_cab_ptr2[id]);
}

//Getting the starting and ending co-ordinates of the 2 planes
for(id=0;id<no_of_planes-1;id++)
{
sem_wait(&mut_planeinzone[myid][id]);
if (planeinzone[myid][id]!=NOTMYZONE && cabmsg_aeroplanes2[planeinzone[myid][id]].is_flying == 1)
{
currx1=cabmsg_aeroplanes2[planeinzone[myid][id]].currx;
curry1=cabmsg_aeroplanes2[planeinzone[myid][id]].curry;

tempx1=cabmsg_aeroplanes2[planeinzone[myid][id]].xtemp;
tempy1=cabmsg_aeroplanes2[planeinzone[myid][id]].ytemp;

destx1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].dest_airport_id].x;
desty1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].dest_airport_id].y;

srcx1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id].x;
srcy1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id].y;

speed1=cabmsg_aeroplanes2[planeinzone[myid][id]].curr_speed;

src_apid1 = cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id;

sem_post(&mut_planeinzone[myid][id]);

cx1mmd = currx1-MIN_DIST/2;
if (cx1mmd < XMIN) cx1mmd = XMIN+10;

cx1pmd = currx1+MIN_DIST/2;
if (cx1pmd > XMAX) cx1pmd = XMAX-10;

cy1mmd = curry1-MIN_DIST/2;
if (cy1mmd < YMIN) cy1mmd = YMIN+10;

cy1pmd = curry1+MIN_DIST/2;
if (cy1pmd > YMAX) cy1pmd = YMAX-10;

for( id2 = id+1 ; id2 < no_of_planes; id2++ )
{
sem_wait(&mut_planeinzone[myid][id2]);
if (planeinzone[myid][id2]!=NOTMYZONE && cabmsg_aeroplanes2[planeinzone[myid][id2]].is_flying == 1)
{
dest_apid2 = cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id;

currx2=cabmsg_aeroplanes2[planeinzone[myid][id2]].currx;
curry2=cabmsg_aeroplanes2[planeinzone[myid][id2]].curry;

tempx2=cabmsg_aeroplanes2[planeinzone[myid][id2]].xtemp;
tempy2=cabmsg_aeroplanes2[planeinzone[myid][id2]].ytemp;

destx2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id].x;
desty2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id].y;

srcx2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].src_airport_id].x;
srcy2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].src_airport_id].y;

speed2=cabmsg_aeroplanes2[planeinzone[myid][id2]].curr_speed;
sem_post(&mut_planeinzone[myid][id2]);
cx2mmd = currx2-MIN_DIST/2;
if (cx2mmd < XMIN) cx2mmd = XMIN+10;

cx2pmd = currx2+MIN_DIST/2;
if (cx2pmd > XMAX) cx2pmd = XMAX-10;

cy2mmd = curry2-MIN_DIST/2;
if (cy2mmd < YMIN) cy2mmd = YMIN+10;

cy2pmd = curry2+MIN_DIST/2;
if (cy2pmd > YMAX) cy2pmd = YMAX-10;

pdistance=sqrt((currx2-currx1)*(currx2-currx1) + (curry2-curry1)*(curry2-curry1));
if ( pdistance < MIN_DIST )
{
if((currx1-destx1)!=0) a1 = (curry1-desty1)/(currx1-destx1);
else a1 = 0.0;
b1=-1;c1=-curry1;
if ((currx2-destx2)!=0) a2=(curry2-desty2)/(currx2-destx2);
else a2=0.0;
b2=-1;c2=-curry2;
delta=a1*b2-a2*b1;//The determinant used for division
resx=0;resy=0; //The result of solving the two equations.
if(delta)
{
resx=(int)((c1*b2-c2*b1)/delta);
resy=(int)((a1*c2-a2*c1)/delta);

gxmin = atcs[myid].x-GRID_SIZE/2;
gxmax = atcs[myid].x+GRID_SIZE/2;
gymin = atcs[myid].y-GRID_SIZE/2;
gymax = atcs[myid].y+GRID_SIZE/2;

//Now to check if the lines really collide within the simulation area
if ((resx > gxmin) && (resx < gxmax) && (resy > gymin) && (resy < gymax))
{
//There is definitely an intersection within the area but need not be a collision
//So calculate the distance of the two planes from the colliding point
//And based on their speeds get whether they reach there at the same time
d1=sqrt((resx-currx1)*(resx-currx1) + (resy-curry1)*(resy-curry1));
d2=sqrt((resx-currx2)*(resx-currx2) + (resy-curry2)*(resy-curry2));
//These are the 2 distances of the 2 planes from the intersecting point.
//if ((abs((d1*SCALE_DIST/speed1)*60-(d2*SCALE_DIST/speed2)*60))<5)
if (floor(d1/speed1) == floor(d2/speed2))
{
//Condition for no collision
if(((curry1<curry2)&&(tempy1<tempy2))||((currx1<currx2)&&(tempx1<tempx2))){}
else if(currx1<tempx1)//&&curry1>tempy1)
finalx1=cx1pmd;
else if(curry1<tempy1)//&&curry1>tempy1)
finaly1=cy1pmd;

}
}
//else The planes collide outside our area;
}
else
{
if (srcx1 == tempx2 && srcy1 == tempy2)
{
finalx1=cx1pmd+30;
//finaly1=cy1pmd;
//finalx2=cx2mmd;
//finaly2=cy2pmd;
}
else if (srcx1 == tempx2 && tempy1 > tempy2)
{
finalx1=cx1pmd;
finaly1=cy1pmd;
finalx2=cx2mmd;
finaly2=cy2mmd;
}
else if (srcx1 == tempx2 && tempy1 < tempy2)
{
finalx1=cx1mmd;
finaly1=cy1mmd;
finalx2=cx2pmd;
finaly2=cy2pmd;
}
else if (srcy1 == tempy2 && tempx1 > tempx2)
{
finalx1=cx1pmd;
finaly1=cy1mmd;
finalx2=cx2mmd;
finaly2=cy2pmd;
}
else if (srcy1 == tempy2 && tempx1 < tempx2)
{
finalx1=cx1mmd;
finaly1=cy1pmd;
finalx2=cx2pmd;
finaly2=cy2mmd;
}
else if ((srcx1-srcx2+tempx1-tempx2) && (tempy1 > tempy2))
{
finalx1=cx1pmd;
finaly1=cy1pmd;
finalx2=cx2mmd;
finaly2=cy2mmd;

}
else if ((srcx1-srcx2+tempx1-tempx2) && (tempy1 < tempy2))
{
finalx1=cx1mmd;
finaly1=cy1mmd;
finalx2=cx2pmd;
finaly2=cy2pmd;
}
else if ((srcy1-srcy2+tempy1-tempy2) && (tempx1 > tempx2))
{
finalx1=cx1pmd;
finaly1=cy1mmd;
finalx2=cx2mmd;
finaly2=cy2pmd;
}
else if ((srcy1-srcy2+tempy1-tempy2) && (tempx1 < tempx2))
{
finalx1=cx1mmd;
finaly1=cy1pmd;
finalx2=cx2pmd;
finaly2=cy2mmd;
}

}
//keep mutex for the aeroplane
sem_wait(&mut_planeinzone[myid][id]);
if (finalx1!=-1)
aeroplanes[planeinzone[myid][id]].xtemp=finalx1;
if (finaly1!=-1)
aeroplanes[planeinzone[myid][id]].ytemp=finaly1;
sem_post(&mut_planeinzone[myid][id]);
sem_wait(&mut_planeinzone[myid][id2]);
if (finalx2!=-1)
aeroplanes[planeinzone[myid][id2]].xtemp=finalx2;
if (finaly2!=-1)
aeroplanes[planeinzone[myid][id2]].ytemp=finaly2;
sem_post(&mut_planeinzone[myid][id2]);

if ((finalx1 != -1 || finaly1 != -1) || (finalx2 != -1 || finaly2 != -1))
{
// cabmsg_atc[id].written='Y';
// cabmsg_atc[id2].written='Y';
// cabmsg_atc[id].xtemp=cx1mmd;
// cabmsg_atc[id].ytemp=cy1pmd;
// cabmsg_atc[id2].xtemp=cx2pmd;
// cabmsg_atc[id2].ytemp=cy2mmd;

// atc_cab_ptr1=cab_reserve(atc_plane_cab_id[id]);
// memcpy(atc_cab_ptr1,&cabmsg_atc[id],sizeof(atcplanecab));
// cab_putmes(atc_plane_cab_id[id],atc_cab_ptr1);

//atc_cab_ptr2=cab_reserve(atc_plane_cab_id[id2]);
//memcpy(atc_cab_ptr2,&cabmsg_atc[id2],sizeof(atcplanecab));
//cab_putmes(atc_plane_cab_id[id2],atc_cab_ptr2);

sprintf(temp,"ATC %d detected collision between %d and %d ",myid,id,id2);
grx_text(temp,XMIN,YMIN-10,red,black);
}
}//end of if loop inside the second for
}
else
{
sem_post(&mut_planeinzone[myid][id2]);
}
}
}
else
{
sem_post(&mut_planeinzone[myid][id]);
}
}
} // End of DoesCollide

TASK recv_reg(void *arg)
{
int atc_id = (int)(arg);
int plane_id;
int count=0;
while(1)
{
for(plane_id=0;plane_id<get_i();++plane_id)
{
sem_wait(&mut_planeinzone[atc_id][plane_id]);
if (planeinzone[atc_id][plane_id]==plane_id)
++count;
sem_post(&mut_planeinzone[atc_id][plane_id]);
}

if(count>1)
{
DoesCollide(atc_id,plane_id); //check if all the planes in my zone collide
}
count=0;
task_endcycle();
}
}

TASK manager_ATC(void *arg)
{
int plane_id,atc_id;
while(1)
{
for (atc_id=0;atc_id<MAX_ATCS;atc_id++)
{
for(plane_id=0;plane_id<get_i();++plane_id)
{
//check if the plane is actually flying
plane_cab_ptr1[plane_id]=cab_getmes(plane_atc_cab_id[plane_id]);
memcpy(&cabmsg_aeroplanes1[plane_id],plane_cab_ptr1[plane_id],sizeof(aeroplane));
cab_unget(plane_atc_cab_id[plane_id],plane_cab_ptr1[plane_id]);

if (cabmsg_aeroplanes1[plane_id].is_flying == 1) //this value will be read from the CAB
{
if (cabmsg_aeroplanes1[plane_id].zone_id == atc_id) //this zone id will be compared by the manager ATC thread and that will activate the ATC of that zone
{
sem_wait(&mut_planeinzone[atc_id][plane_id]);
planeinzone[atc_id][plane_id]=plane_id;
sem_post(&mut_planeinzone[atc_id][plane_id]);
task_activate(tid[atc_id]);
}
else
{
sem_wait(&mut_planeinzone[atc_id][plane_id]);
planeinzone[atc_id][plane_id]=NOTMYZONE; //since plane is not flying
sem_post(&mut_planeinzone[atc_id][plane_id]);
task_disable(tid[atc_id]);
}
}
else
{
sem_wait(&mut_planeinzone[atc_id][plane_id]);
planeinzone[atc_id][plane_id]=NOTMYZONE;
sem_post(&mut_planeinzone[atc_id][plane_id]);
task_disable(tid[atc_id]);
}
}
}
task_endcycle();
}
}

TASK display_ATC_info(void *arg) //this shud also display which planes are colliding
{
int i,j;
char temp[100];
int printed[MAX_ATCS];
int no_of_planes_in_zone[MAX_ATCS];

char temp_p_in[3];
char p_in_zone[100];
strcpy(p_in_zone," ");
char clear[30];
sem_wait(&graphics_mutex);
grx_text("ATC PLANES IN ZONE",XMAX+10,(int)(YMIN+YMAX/1.5),green,black);
grx_text("----------------------",XMAX+10,(int)(10+YMIN+YMAX/1.5),green,black);
for (i=0;i<MAX_ATCS;i++)
{
sprintf(temp," %d : ",i);
grx_text(temp,XMAX+10,(int)((i*10)+YMIN+YMAX/1.5+20),green,black);
printed[i]=0;
no_of_planes_in_zone[i]=0;
}
sem_post(&graphics_mutex);
while(1)
{
for (i=0;i<MAX_ATCS;i++)
{
for (j=0;j<MAX_PLANES;j++)
{
sem_wait(&mut_planeinzone[i][j]);
if (cabmsg_aeroplanes1[j].is_flying==1 && planeinzone[i][j]==j)
{
sem_post(&mut_planeinzone[i][j]);
sprintf(temp_p_in,"%d ",j);
strcat(p_in_zone,temp_p_in);
++no_of_planes_in_zone[i];
}
else
{
sem_post(&mut_planeinzone[i][j]);
}

if (no_of_planes_in_zone[i] < printed[i])
{
strcpy(clear," ");
sem_wait(&graphics_mutex);
grx_text(clear,XMAX+65,(int)((i*10)+YMIN+YMAX/1.5+20),black,black);
sem_post(&graphics_mutex);
printed[i]=0;
strcpy(p_in_zone," ");
}
else
{
printed[i]=no_of_planes_in_zone[i];
sem_wait(&graphics_mutex);
grx_text(p_in_zone,XMAX+65,(int)((i*10)+YMIN+(YMAX/1.5)+20),white,black);
sem_post(&graphics_mutex);
}
}
no_of_planes_in_zone[i]=0;
strcpy(p_in_zone," ");
}
task_endcycle();
}
}

void init_atcs()
{
PID tid2,m_tid;
int n_atcs;
int i,j;
SOFT_TASK_MODEL m;
SOFT_TASK_MODEL manager_atc;
SOFT_TASK_MODEL disp_atc;

for (i=0;i<MAX_ATCS;i++)
for (j=0;j<MAX_PLANES;j++)
{
sem_wait(&mut_planeinzone[i][j]);
planeinzone[i][j]=NOTMYZONE;
sem_post(&mut_planeinzone[i][j]);
}

soft_task_default_model(manager_atc);
soft_task_def_level(manager_atc,1);
soft_task_def_ctrl_jet (manager_atc);
//soft_task_def_arg (manager_atc, (void *)n_atcs);
soft_task_def_met (manager_atc, MANAGERATCWCET);
soft_task_def_group (manager_atc, ATCGROUP); //modify this later
soft_task_def_period (manager_atc, MANAGERATCPERIOD);
soft_task_def_usemath (manager_atc);
m_tid = task_create("MgrATC", manager_ATC, &manager_atc, NULL);
if (m_tid == NIL)
{
grx_close();
cprintf("Could not create task <MgrATC>");
sys_end();
}
task_activate(m_tid); //manager task will activate and deactivate the other ATC tasks as planes go in their zone

for (n_atcs=0;n_atcs<MAX_ATCS;n_atcs++) //create all the ATC tasks but do not activate them
//the manager thread will activate and deactivate these tasks
{
//activate the receive message TASK
soft_task_default_model(m);
soft_task_def_level(m,1);
soft_task_def_ctrl_jet (m);
soft_task_def_arg (m, (void *)n_atcs);
soft_task_def_met (m, RECVMSGWCET);
soft_task_def_group (m, ATCGROUP); //modify this later
soft_task_def_period (m, RECVMSGPERIOD);
soft_task_def_usemath (m);
tid[n_atcs] = task_create("recv_reg", recv_reg, &m, NULL);
if (tid[n_atcs] == NIL)
{
grx_close();
cprintf("Could not create task <Recv_reg>");
sys_end();
}
}

//activate the display ATC info TASK
soft_task_default_model(disp_atc);
soft_task_def_level(disp_atc,1);
soft_task_def_ctrl_jet (disp_atc);
soft_task_def_arg (disp_atc, (void *)n_atcs);
soft_task_def_met (disp_atc, DISPATCWCET);
soft_task_def_group (disp_atc, ATCGROUP); //modify this later
soft_task_def_period (disp_atc, DISPATCPERIOD);
soft_task_def_usemath (disp_atc);
tid2 = task_create("DispATC", display_ATC_info, &disp_atc, NULL);
if (tid2 == NIL)
{
grx_close();
cprintf("Could not create task <DispATC>");
sys_end();
}
task_activate(tid2);
}

Subversion Repositories shark

(root)/unsupported/trunk/imit2004/atcsim/atc.c - Rev 1669