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

Rev	Author	Line No.	Line
1664	pj	1	#include <kernel/kern.h> //include the SHARK kernel header
		2	#include <drivers/glib.h> //include the graphics library
		3	#include <drivers/keyb.h> //include the keyboard library
		4	#include <semaphore.h> //include the semaphore library
		5	#include <stdlib.h> //include the standard library
		6	#include <math.h> //include the math library for random
		7	#include <string.h>
		8
		9	#ifndef _ATCSIM_HEADER_
		10	#include "atcsim.h"
		11	#endif
		12
		13	#ifndef _SIM_AREA_HEADER_
		14	#include "sim_area.h"
		15	#endif
		16
		17	#ifndef _INPUT_
		18	#include "input.h"
		19	#endif
		20
		21	#ifndef _CONST_
		22	#include "constants.h"
		23	#endif
		24
		25	extern sem_t mut_planeinzone[MAX_ATCS][MAX_PLANES];
		26	char *plane_cab_ptr1[MAX_PLANES];
		27	aeroplane cabmsg_aeroplanes1[MAX_PLANES];
		28	PID tid[MAX_ATCS];
		29
		30	extern int STARTSIM;
		31	extern int get_i();
		32	extern aeroplane aeroplanes[MAX_PLANES];
		33
		34	extern airport airports[NUM_OF_AIRPORTS];
		35	extern atc atcs[MAX_ATCS];
		36	extern mutex_t mut_number_of_planes;
		37
		38	aeroplane cabmsg_aeroplanes2[MAX_PLANES];
		39	atcplanecab cabmsg_atc[MAX_PLANES];
		40
		41	int planeinzone[MAX_ATCS][MAX_PLANES];
		42
		43	void DoesCollide(int myid,int no_of_planes)
		44	{
		45	int id,id2;
		46	int currx1,curry1,destx1,desty1,speed1,currx2,curry2,destx2,desty2,speed2,srcx2,srcy2,srcx1,srcy1;
		47
		48	int src_apid1,dest_apid2,tempx1,tempx2,tempy1,tempy2,resx,resy;
		49	int gxmin,gymin,gxmax,gymax;
		50
		51	int cx1mmd,cx1pmd,cx2mmd,cx2pmd,cy1mmd,cy1pmd,cy2mmd,cy2pmd;
		52	float pdistance,a1,a2,b1,b2,c1,c2,d1,d2,delta;
		53	char *plane_cab_ptr2[MAX_PLANES];
		54	// char atc_cab_ptr1,atc_cab_ptr2;
		55
		56	char temp[100];
		57
		58	int finalx1=-1,finalx2=-1,finaly1=-1,finaly2=-1;
		59
		60
		61	for(id=0;id<no_of_planes;id++)
		62	{
		63	plane_cab_ptr2[id]=cab_getmes(plane_atc_cab_id[id]);
		64	memcpy(&cabmsg_aeroplanes2[id],plane_cab_ptr2[id],sizeof(aeroplane));
		65	cab_unget(plane_atc_cab_id[id],plane_cab_ptr2[id]);
		66	}
		67
		68	//Getting the starting and ending co-ordinates of the 2 planes
		69	for(id=0;id<no_of_planes-1;id++)
		70	{
		71	sem_wait(&mut_planeinzone[myid][id]);
		72	if (planeinzone[myid][id]!=NOTMYZONE && cabmsg_aeroplanes2[planeinzone[myid][id]].is_flying == 1)
		73	{
		74	currx1=cabmsg_aeroplanes2[planeinzone[myid][id]].currx;
		75	curry1=cabmsg_aeroplanes2[planeinzone[myid][id]].curry;
		76
		77	tempx1=cabmsg_aeroplanes2[planeinzone[myid][id]].xtemp;
		78	tempy1=cabmsg_aeroplanes2[planeinzone[myid][id]].ytemp;
		79
		80	destx1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].dest_airport_id].x;
		81	desty1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].dest_airport_id].y;
		82
		83	srcx1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id].x;
		84	srcy1=airports[cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id].y;
		85
		86	speed1=cabmsg_aeroplanes2[planeinzone[myid][id]].curr_speed;
		87
		88	src_apid1 = cabmsg_aeroplanes2[planeinzone[myid][id]].src_airport_id;
		89
		90	sem_post(&mut_planeinzone[myid][id]);
		91
		92	cx1mmd = currx1-MIN_DIST/2;
		93	if (cx1mmd < XMIN) cx1mmd = XMIN+10;
		94
		95	cx1pmd = currx1+MIN_DIST/2;
		96	if (cx1pmd > XMAX) cx1pmd = XMAX-10;
		97
		98	cy1mmd = curry1-MIN_DIST/2;
		99	if (cy1mmd < YMIN) cy1mmd = YMIN+10;
		100
		101	cy1pmd = curry1+MIN_DIST/2;
		102	if (cy1pmd > YMAX) cy1pmd = YMAX-10;
		103
		104	for( id2 = id+1 ; id2 < no_of_planes; id2++ )
		105	{
		106	sem_wait(&mut_planeinzone[myid][id2]);
		107	if (planeinzone[myid][id2]!=NOTMYZONE && cabmsg_aeroplanes2[planeinzone[myid][id2]].is_flying == 1)
		108	{
		109	dest_apid2 = cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id;
		110
		111	currx2=cabmsg_aeroplanes2[planeinzone[myid][id2]].currx;
		112	curry2=cabmsg_aeroplanes2[planeinzone[myid][id2]].curry;
		113
		114	tempx2=cabmsg_aeroplanes2[planeinzone[myid][id2]].xtemp;
		115	tempy2=cabmsg_aeroplanes2[planeinzone[myid][id2]].ytemp;
		116
		117	destx2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id].x;
		118	desty2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].dest_airport_id].y;
		119
		120	srcx2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].src_airport_id].x;
		121	srcy2=airports[cabmsg_aeroplanes2[planeinzone[myid][id2]].src_airport_id].y;
		122
		123	speed2=cabmsg_aeroplanes2[planeinzone[myid][id2]].curr_speed;
		124	sem_post(&mut_planeinzone[myid][id2]);
		125	cx2mmd = currx2-MIN_DIST/2;
		126	if (cx2mmd < XMIN) cx2mmd = XMIN+10;
		127
		128	cx2pmd = currx2+MIN_DIST/2;
		129	if (cx2pmd > XMAX) cx2pmd = XMAX-10;
		130
		131	cy2mmd = curry2-MIN_DIST/2;
		132	if (cy2mmd < YMIN) cy2mmd = YMIN+10;
		133
		134	cy2pmd = curry2+MIN_DIST/2;
		135	if (cy2pmd > YMAX) cy2pmd = YMAX-10;
		136
		137	pdistance=sqrt((currx2-currx1)(currx2-currx1) + (curry2-curry1)(curry2-curry1));
		138	if ( pdistance < MIN_DIST )
		139	{
		140	if((currx1-destx1)!=0) a1 = (curry1-desty1)/(currx1-destx1);
		141	else a1 = 0.0;
		142	b1=-1;c1=-curry1;
		143	if ((currx2-destx2)!=0) a2=(curry2-desty2)/(currx2-destx2);
		144	else a2=0.0;
		145	b2=-1;c2=-curry2;
		146	delta=a1b2-a2b1;//The determinant used for division
		147	resx=0;resy=0; //The result of solving the two equations.
		148	if(delta)
		149	{
		150	resx=(int)((c1b2-c2b1)/delta);
		151	resy=(int)((a1c2-a2c1)/delta);
		152
		153	gxmin = atcs[myid].x-GRID_SIZE/2;
		154	gxmax = atcs[myid].x+GRID_SIZE/2;
		155	gymin = atcs[myid].y-GRID_SIZE/2;
		156	gymax = atcs[myid].y+GRID_SIZE/2;
		157
		158	//Now to check if the lines really collide within the simulation area
		159	if ((resx > gxmin) && (resx < gxmax) && (resy > gymin) && (resy < gymax))
		160	{
		161	//There is definitely an intersection within the area but need not be a collision
		162	//So calculate the distance of the two planes from the colliding point
		163	//And based on their speeds get whether they reach there at the same time
		164	d1=sqrt((resx-currx1)(resx-currx1) + (resy-curry1)(resy-curry1));
		165	d2=sqrt((resx-currx2)(resx-currx2) + (resy-curry2)(resy-curry2));
		166	//These are the 2 distances of the 2 planes from the intersecting point.
		167	//if ((abs((d1SCALE_DIST/speed1)60-(d2SCALE_DIST/speed2)60))<5)
		168	if (floor(d1/speed1) == floor(d2/speed2))
		169	{
		170	//Condition for no collision
		171	if(((curry1<curry2)&&(tempy1<tempy2))\|\|((currx1<currx2)&&(tempx1<tempx2))){}
		172	else if(currx1<tempx1)//&&curry1>tempy1)
		173	finalx1=cx1pmd;
		174	else if(curry1<tempy1)//&&curry1>tempy1)
		175	finaly1=cy1pmd;
		176
		177	}
		178	}
		179	//else The planes collide outside our area;
		180	}
		181	else
		182	{
		183	if (srcx1 == tempx2 && srcy1 == tempy2)
		184	{
		185	finalx1=cx1pmd+30;
		186	//finaly1=cy1pmd;
		187	//finalx2=cx2mmd;
		188	//finaly2=cy2pmd;
		189	}
		190	else if (srcx1 == tempx2 && tempy1 > tempy2)
		191	{
		192	finalx1=cx1pmd;
		193	finaly1=cy1pmd;
		194	finalx2=cx2mmd;
		195	finaly2=cy2mmd;
		196	}
		197	else if (srcx1 == tempx2 && tempy1 < tempy2)
		198	{
		199	finalx1=cx1mmd;
		200	finaly1=cy1mmd;
		201	finalx2=cx2pmd;
		202	finaly2=cy2pmd;
		203	}
		204	else if (srcy1 == tempy2 && tempx1 > tempx2)
		205	{
		206	finalx1=cx1pmd;
		207	finaly1=cy1mmd;
		208	finalx2=cx2mmd;
		209	finaly2=cy2pmd;
		210	}
		211	else if (srcy1 == tempy2 && tempx1 < tempx2)
		212	{
		213	finalx1=cx1mmd;
		214	finaly1=cy1pmd;
		215	finalx2=cx2pmd;
		216	finaly2=cy2mmd;
		217	}
		218	else if ((srcx1-srcx2+tempx1-tempx2) && (tempy1 > tempy2))
		219	{
		220	finalx1=cx1pmd;
		221	finaly1=cy1pmd;
		222	finalx2=cx2mmd;
		223	finaly2=cy2mmd;
		224
		225	}
		226	else if ((srcx1-srcx2+tempx1-tempx2) && (tempy1 < tempy2))
		227	{
		228	finalx1=cx1mmd;
		229	finaly1=cy1mmd;
		230	finalx2=cx2pmd;
		231	finaly2=cy2pmd;
		232	}
		233	else if ((srcy1-srcy2+tempy1-tempy2) && (tempx1 > tempx2))
		234	{
		235	finalx1=cx1pmd;
		236	finaly1=cy1mmd;
		237	finalx2=cx2mmd;
		238	finaly2=cy2pmd;
		239	}
		240	else if ((srcy1-srcy2+tempy1-tempy2) && (tempx1 < tempx2))
		241	{
		242	finalx1=cx1mmd;
		243	finaly1=cy1pmd;
		244	finalx2=cx2pmd;
		245	finaly2=cy2mmd;
		246	}
		247
		248	}
		249	//keep mutex for the aeroplane
		250	sem_wait(&mut_planeinzone[myid][id]);
		251	if (finalx1!=-1)
		252	aeroplanes[planeinzone[myid][id]].xtemp=finalx1;
		253	if (finaly1!=-1)
		254	aeroplanes[planeinzone[myid][id]].ytemp=finaly1;
		255	sem_post(&mut_planeinzone[myid][id]);
		256	sem_wait(&mut_planeinzone[myid][id2]);
		257	if (finalx2!=-1)
		258	aeroplanes[planeinzone[myid][id2]].xtemp=finalx2;
		259	if (finaly2!=-1)
		260	aeroplanes[planeinzone[myid][id2]].ytemp=finaly2;
		261	sem_post(&mut_planeinzone[myid][id2]);
		262
		263	if ((finalx1 != -1 \|\| finaly1 != -1) \|\| (finalx2 != -1 \|\| finaly2 != -1))
		264	{
		265	// cabmsg_atc[id].written='Y';
		266	// cabmsg_atc[id2].written='Y';
		267	// cabmsg_atc[id].xtemp=cx1mmd;
		268	// cabmsg_atc[id].ytemp=cy1pmd;
		269	// cabmsg_atc[id2].xtemp=cx2pmd;
		270	// cabmsg_atc[id2].ytemp=cy2mmd;
		271
		272	// atc_cab_ptr1=cab_reserve(atc_plane_cab_id[id]);
		273	// memcpy(atc_cab_ptr1,&cabmsg_atc[id],sizeof(atcplanecab));
		274	// cab_putmes(atc_plane_cab_id[id],atc_cab_ptr1);
		275
		276	//atc_cab_ptr2=cab_reserve(atc_plane_cab_id[id2]);
		277	//memcpy(atc_cab_ptr2,&cabmsg_atc[id2],sizeof(atcplanecab));
		278	//cab_putmes(atc_plane_cab_id[id2],atc_cab_ptr2);
		279
		280	sprintf(temp,"ATC %d detected collision between %d and %d ",myid,id,id2);
		281	grx_text(temp,XMIN,YMIN-10,red,black);
		282	}
		283	}//end of if loop inside the second for
		284	}
		285	else
		286	{
		287	sem_post(&mut_planeinzone[myid][id2]);
		288	}
		289	}
		290	}
		291	else
		292	{
		293	sem_post(&mut_planeinzone[myid][id]);
		294	}
		295	}
		296	} // End of DoesCollide
		297
		298	TASK recv_reg(void *arg)
		299	{
		300	int atc_id = (int)(arg);
		301	int plane_id;
		302	int count=0;
		303	while(1)
		304	{
		305	for(plane_id=0;plane_id<get_i();++plane_id)
		306	{
		307	sem_wait(&mut_planeinzone[atc_id][plane_id]);
		308	if (planeinzone[atc_id][plane_id]==plane_id)
		309	++count;
		310	sem_post(&mut_planeinzone[atc_id][plane_id]);
		311	}
		312
		313	if(count>1)
		314	{
		315	DoesCollide(atc_id,plane_id); //check if all the planes in my zone collide
		316	}
		317	count=0;
		318	task_endcycle();
		319	}
		320	}
		321
		322	TASK manager_ATC(void *arg)
		323	{
		324	int plane_id,atc_id;
		325	while(1)
		326	{
		327	for (atc_id=0;atc_id<MAX_ATCS;atc_id++)
		328	{
		329	for(plane_id=0;plane_id<get_i();++plane_id)
		330	{
		331	//check if the plane is actually flying
		332	plane_cab_ptr1[plane_id]=cab_getmes(plane_atc_cab_id[plane_id]);
		333	memcpy(&cabmsg_aeroplanes1[plane_id],plane_cab_ptr1[plane_id],sizeof(aeroplane));
		334	cab_unget(plane_atc_cab_id[plane_id],plane_cab_ptr1[plane_id]);
		335
		336	if (cabmsg_aeroplanes1[plane_id].is_flying == 1) //this value will be read from the CAB
		337	{
		338	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
		339	{
		340	sem_wait(&mut_planeinzone[atc_id][plane_id]);
		341	planeinzone[atc_id][plane_id]=plane_id;
		342	sem_post(&mut_planeinzone[atc_id][plane_id]);
		343	task_activate(tid[atc_id]);
		344	}
		345	else
		346	{
		347	sem_wait(&mut_planeinzone[atc_id][plane_id]);
		348	planeinzone[atc_id][plane_id]=NOTMYZONE; //since plane is not flying
		349	sem_post(&mut_planeinzone[atc_id][plane_id]);
		350	task_disable(tid[atc_id]);
		351	}
		352	}
		353	else
		354	{
		355	sem_wait(&mut_planeinzone[atc_id][plane_id]);
		356	planeinzone[atc_id][plane_id]=NOTMYZONE;
		357	sem_post(&mut_planeinzone[atc_id][plane_id]);
		358	task_disable(tid[atc_id]);
		359	}
		360	}
		361	}
		362	task_endcycle();
		363	}
		364	}
		365
		366
		367	TASK display_ATC_info(void *arg) //this shud also display which planes are colliding
		368	{
		369	int i,j;
		370	char temp[100];
		371	int printed[MAX_ATCS];
		372	int no_of_planes_in_zone[MAX_ATCS];
		373
		374	char temp_p_in[3];
		375	char p_in_zone[100];
		376	strcpy(p_in_zone," ");
		377	char clear[30];
		378	sem_wait(&graphics_mutex);
		379	grx_text("ATC PLANES IN ZONE",XMAX+10,(int)(YMIN+YMAX/1.5),green,black);
		380	grx_text("----------------------",XMAX+10,(int)(10+YMIN+YMAX/1.5),green,black);
		381	for (i=0;i<MAX_ATCS;i++)
		382	{
		383	sprintf(temp," %d : ",i);
		384	grx_text(temp,XMAX+10,(int)((i*10)+YMIN+YMAX/1.5+20),green,black);
		385	printed[i]=0;
		386	no_of_planes_in_zone[i]=0;
		387	}
		388	sem_post(&graphics_mutex);
		389	while(1)
		390	{
		391	for (i=0;i<MAX_ATCS;i++)
		392	{
		393	for (j=0;j<MAX_PLANES;j++)
		394	{
		395	sem_wait(&mut_planeinzone[i][j]);
		396	if (cabmsg_aeroplanes1[j].is_flying==1 && planeinzone[i][j]==j)
		397	{
		398	sem_post(&mut_planeinzone[i][j]);
		399	sprintf(temp_p_in,"%d ",j);
		400	strcat(p_in_zone,temp_p_in);
		401	++no_of_planes_in_zone[i];
		402	}
		403	else
		404	{
		405	sem_post(&mut_planeinzone[i][j]);
		406	}
		407
		408	if (no_of_planes_in_zone[i] < printed[i])
		409	{
		410	strcpy(clear," ");
		411	sem_wait(&graphics_mutex);
		412	grx_text(clear,XMAX+65,(int)((i*10)+YMIN+YMAX/1.5+20),black,black);
		413	sem_post(&graphics_mutex);
		414	printed[i]=0;
		415	strcpy(p_in_zone," ");
		416	}
		417	else
		418	{
		419	printed[i]=no_of_planes_in_zone[i];
		420	sem_wait(&graphics_mutex);
		421	grx_text(p_in_zone,XMAX+65,(int)((i*10)+YMIN+(YMAX/1.5)+20),white,black);
		422	sem_post(&graphics_mutex);
		423	}
		424	}
		425	no_of_planes_in_zone[i]=0;
		426	strcpy(p_in_zone," ");
		427	}
		428	task_endcycle();
		429	}
		430	}
		431
		432	void init_atcs()
		433	{
		434	PID tid2,m_tid;
		435	int n_atcs;
		436	int i,j;
		437	SOFT_TASK_MODEL m;
		438	SOFT_TASK_MODEL manager_atc;
		439	SOFT_TASK_MODEL disp_atc;
		440
		441	for (i=0;i<MAX_ATCS;i++)
		442	for (j=0;j<MAX_PLANES;j++)
		443	{
		444	sem_wait(&mut_planeinzone[i][j]);
		445	planeinzone[i][j]=NOTMYZONE;
		446	sem_post(&mut_planeinzone[i][j]);
		447	}
		448
		449	soft_task_default_model(manager_atc);
		450	soft_task_def_level(manager_atc,1);
		451	soft_task_def_ctrl_jet (manager_atc);
		452	//soft_task_def_arg (manager_atc, (void *)n_atcs);
		453	soft_task_def_met (manager_atc, MANAGERATCWCET);
		454	soft_task_def_group (manager_atc, ATCGROUP); //modify this later
		455	soft_task_def_period (manager_atc, MANAGERATCPERIOD);
		456	soft_task_def_usemath (manager_atc);
		457	m_tid = task_create("MgrATC", manager_ATC, &manager_atc, NULL);
		458	if (m_tid == NIL)
		459	{
		460	grx_close();
		461	cprintf("Could not create task <MgrATC>");
		462	sys_end();
		463	}
		464	task_activate(m_tid); //manager task will activate and deactivate the other ATC tasks as planes go in their zone
		465
		466
		467
		468	for (n_atcs=0;n_atcs<MAX_ATCS;n_atcs++) //create all the ATC tasks but do not activate them
		469	//the manager thread will activate and deactivate these tasks
		470	{
		471	//activate the receive message TASK
		472	soft_task_default_model(m);
		473	soft_task_def_level(m,1);
		474	soft_task_def_ctrl_jet (m);
		475	soft_task_def_arg (m, (void *)n_atcs);
		476	soft_task_def_met (m, RECVMSGWCET);
		477	soft_task_def_group (m, ATCGROUP); //modify this later
		478	soft_task_def_period (m, RECVMSGPERIOD);
		479	soft_task_def_usemath (m);
		480	tid[n_atcs] = task_create("recv_reg", recv_reg, &m, NULL);
		481	if (tid[n_atcs] == NIL)
		482	{
		483	grx_close();
		484	cprintf("Could not create task <Recv_reg>");
		485	sys_end();
		486	}
		487	}
		488
		489	//activate the display ATC info TASK
		490	soft_task_default_model(disp_atc);
		491	soft_task_def_level(disp_atc,1);
		492	soft_task_def_ctrl_jet (disp_atc);
		493	soft_task_def_arg (disp_atc, (void *)n_atcs);
		494	soft_task_def_met (disp_atc, DISPATCWCET);
		495	soft_task_def_group (disp_atc, ATCGROUP); //modify this later
		496	soft_task_def_period (disp_atc, DISPATCPERIOD);
		497	soft_task_def_usemath (disp_atc);
		498	tid2 = task_create("DispATC", display_ATC_info, &disp_atc, NULL);
		499	if (tid2 == NIL)
		500	{
		501	grx_close();
		502	cprintf("Could not create task <DispATC>");
		503	sys_end();
		504	}
		505	task_activate(tid2);
		506	}

Subversion Repositories shark

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