www.pudn.com > sound-source-location.rar > location.c, change:2014-07-30,size:7366b


#include "location.h" 
 
 
void location(uint64_t timedecA,uint64_t timedecB,uint64_t timedecC) 
{ 
	uint8_t i,j; 
	float time0,time1,a,m,n,x,y,tmpx,tmpy,tmpz,sincita,coscita; 
	uint32_t d1,d2; 
	uint64_t dec[3],tmp; 
	dec[0]=timedecA; 
	dec[1]=timedecB; 
	dec[2]=timedecC; 
 
	for(i=0;i<3;i++) 
	{ 
		for(j=3;j>0;j--) 
		{ 
			if(dec[j-1]>dec[i]) 
			{ 
				tmp=dec[j-1]; 
				dec[j-1]=dec[i]; 
				dec[i]=tmp; 
			} 
		} 
	} 
	time0=(dec[0]-dec[2])/(9); 
	time1=(dec[1]-dec[2])/(9); 
	 
	d1=time0*340; 
	d2=time1*340; 
	m=300;//AD 
	n=300;//AB 
	tmpx=4*d1*d1*m*m+4*n*n-n*n*m*m; 
	tmpy=-4*d1*m*m*(n*n-d1*d1)+4*d2*n*n*(m*m-d2*d2); 
	tmpz=m*m*pow((n*n-d2*d2),2)+m*m*pow((n*n-d1*d1),2); 
	a=(-tmpy-sqrt(pow(y,2)-4*tmpx*tmpz))/(2*tmpx); 
	sincita=(m*m-d2*d2+2*a*d2)/(2*a*m); 
	coscita=(n*n-d1*d1+2*a*d1)/(2*a*n); 
	x=a*coscita; 
	y=a*sincita; 
	 
	//a=(sqrt(- pow(d1,6)*pow(n,4) + pow(d1,6)*pow(n,2) - pow(d1,4)*pow(d2,2)*pow(m,2)*pow(n,2) + 6*pow(d1,4)*pow(m,2)*pow(n,4) - 2*pow(d1,4)*pow(m,2)*pow(n,3) - 2*pow(d1,3)*pow(d2,3)*m*n + 2*pow(d1,3)*d2*pow(m,2)*pow(n,3) - pow(d1,2)*pow(d2,4)*pow(m,2)*pow(n,2) + 2*pow(d1,2)*pow(d2,2)*pow(m,4)*pow(n,2) + 2*pow(d1,2)*pow(d2,2)*pow(m,2)*pow(n,4) - 8*pow(d1,2)*pow(m,4)*pow(n,4) - pow(d1,2)*pow(m,2)*pow(n,6) + 2*d1*pow(d2,3)*pow(m,3)*pow(n,2) - 2*d1*d2*pow(m,4)*pow(n,4) - pow(d2,6)*pow(m,4) + pow(d2,6)*pow(m,2) + 6*pow(d2,4)*pow(m,4)*pow(n,2) - 2*pow(d2,4)*pow(m,3)*pow(n,2) - pow(d2,2)*pow(m,6)*pow(n,2) - 8*pow(d2,2)*pow(m,4)*pow(n,4) + 4*pow(m,6)*pow(n,4) + 4*pow(m,4)*pow(n,6)) + pow(d2,3)*m - pow(d1,3)*n + d1*pow(m,2)*pow(n,2) - d2*pow(m,2)*pow(n,2))/(2*(pow(d1,2)*pow(n,2) + pow(d2,2)*pow(m,2) - 4*pow(m,2)*pow(n,2))) 
	 
	 
//	s0=(time0*340); 
//	s1=(time1*340); 
//	a=(s0*s0/4); 
//	b=(300-a); 
//	c=(s1*s1); 
//	d=(300-c); 
//	y=((450*(a*d+c*d)+sqrt(8*(225^2)*a*b*c*d+4*b*d*(c-a)*(a*d-b*c)))/2*(a*d-b*c));  
//	x=((a*(y-225)*(y-225)-a*b)/b); 
//	OLED_ShowNum(28,16,time0,5,16);	 
//	OLED_ShowNum(28,32,time1,5,16);	 
	OLED_ShowNum(28,16,dec[0],10,16); 
	OLED_ShowNum(28,32,dec[1],10,16); 
	OLED_ShowNum(28,48,dec[2],10,16); 
	OLED_Refresh_Gram(); 
	LED=0; 
} 
 
 
 
//51单片机定位程序: wenku.baidu.com/view/4e49d7c05fbfc77da269b1ab?pn=26&ssid=&from=&bd_page_type=1&uid=AC6E09A01B2F1B92B47B846D6B2EED53&pu=rc@5,pic@on,sl@1,pw@1000,sz@1509_220,pd@1,fz@2,lp@8,tpl@color,&st=1&wk=rd&maxpage=29&pos=next 
//#include<reg51.h>  
//#include <intrins.h>  
//#include<math.h> 
//#define uint unsigned int #define uchar unsigned char 
//#define delayNOP(); {_nop_();_nop_();_nop_();_nop_();}; uint DATA_H0,DATA_L0,DATA_H1,DATA_L1; //???????? 
////sbit voice1=P3^2; //sbit voice2=P3^3; sbit voice3=P3^4; 
//uchar IRCOM[7]; 
//sbit LCD_RS = P2^6;//LCD ?? sbit LCD_RW = P2^5; sbit LCD_EN = P2^7; 
//uchar code cdis1[] = {"X:---"}; uchar code cdis2[] = {"Y:---"}; 
//bit lcd_busy(); 
//void lcd_wcmd(uchar cmd); void lcd_wdat(uchar dat); void lcd_init(); 
//void lcd_pos(uchar pos); void delay(uchar x); void delay1(uint ms); void Interrupt_Init(); void voicechuli(); 
///*******************************************************************/ /* */ /*??LCD??? */ /*lcd_busy?1?,?,???lcd-busy?0?,?,???????? */ /* */ /*******************************************************************/ 
//bit lcd_busy() 
//{ bit result; LCD_RS = 0; 
//LCD_RW = 1; LCD_EN = 1; delayNOP(); 
//result = (bit)(P0&0x80); LCD_EN = 0; return(result); } 
///*******************************************************************/ /* */ /*??????LCD */ /*RS=L,RW=L,E=???,D0-D7=???? */ /* */ /*******************************************************************/ void lcd_wcmd(uchar cmd) { while(lcd_busy()); LCD_RS = 0; LCD_RW = 0; LCD_EN = 0; _nop_(); _nop_(); P0 = cmd; delayNOP(); LCD_EN = 1; delayNOP(); LCD_EN = 0; } 
///*******************************************************************/ /* */ /*??????LCD */ /*RS=H,RW=L,E=???,D0-D7=??? */ /* */ /*******************************************************************/ 
//void lcd_wdat(uchar dat) 
//{ while(lcd_busy()); LCD_RS = 1; LCD_RW = 0; LCD_EN = 0; P0 = dat; 
//LCD_EN = 1; 
//delayNOP(); 
//LCD_EN = 0; 
//} 
///**********************************************************/ 
//void lcd_pos(uchar pos) 
//{ 
//lcd_wcmd(pos | 0x80); //????=80+???? 
//} 
///*******************************************************************/ 
///* */ 
///* LCD????? */ 
///* */ 
///*******************************************************************/ 
//void lcd_init() 
//{ 
//char m; 
//delay1(15); 
//lcd_wcmd(0x38); //16*2??,5*7??,8??? 
//delay1(5); 
//lcd_wcmd(0x38); 
//delay1(5); 
//lcd_wcmd(0x38); 
//delay1(5); 
//lcd_wcmd(0x0c); //???,??? 
//delay1(5); 
//lcd_wcmd(0x06); //???? 
//delay1(5); 
//lcd_wcmd(0x01); //??LCD????? 
//delay1(10); 
//lcd_pos(0x00); //????????????1??? 
//m = 0; 
//while(cdis1[m] != '\0') 
//{ //???? 
//lcd_wdat(cdis1[m]); 
//m++; 
//} 
//lcd_pos(0x40); //???????????1??? 
//m = 0; 
//while(cdis2[m] != '\0') 
//{ 
//lcd_wdat(cdis2[m]); //???? 
//m++; 
//} 
//} 
///**********************************************************/ 
//void delay(unsigned char x) //x*0.14MS 
//{ 
//unsigned char i; 
//while(x--) 
//{ 
//for (i = 0; i<13; i++) {} 
//} 
//} 
///**********************************************************/ 
////****************????**************************** 
//void delay1(uint ms) 
//{ 
//uchar y; 
//while(ms--) 
//{ 
//for(y = 0; y<250; y++) 
//{ 
//_nop_(); 
//_nop_(); 
//_nop_(); 
//_nop_(); 
//} 
//} 
//} 
////**************??????????************ 
//void Interrupt_Init() 
//{ 
//TMOD=0x11; //?????1棗3ms 16????? TH0=0x00; //?????2 16????? ?????? 
//TL0=0x00; 
//TH1=0x00; 
//TL1=0x00; //??? ??? ??????? ????? 
//EA=1; 
//EX0=1; 
//EX1=1; 
//IT0=1; 
//IT1=1; 
//PX0=1; 
//PX1=0; 
//} 
////***************????************************* 
//void display(uint a,uint b) 
//{ 
//IRCOM[0]=(char)(a/100); 
//IRCOM[1]=(char)((a%100)/10); 
//IRCOM[2]=(char)(a%10); 
//IRCOM[3]=(char)(b/100); 
//IRCOM[4]=(char)((b%100)/10); 
//IRCOM[5]=(char)(b%10); 
//lcd_pos(0x02); 
//lcd_wdat(IRCOM[0]); //X????? 
//lcd_wdat(IRCOM[1]); //X????? 
//lcd_wdat(IRCOM[2]); //X????? 
//lcd_pos(0x42); 
//lcd_wdat(IRCOM[0]); //Y????? 
//lcd_wdat(IRCOM[1]); //Y????? 
//lcd_wdat(IRCOM[2]); //Y????? 
//} 
////*********????????********************* 
//void voicechuli() 
//{ 
//float time0,time1; 
//uint s0,s1,x,y,a,b,c,d; 
//time0=(DATA_H0*256+DATA_L0)*0.001;//??time0?????????ab??  
//time1=(DATA_H1*256+DATA_L1)*0.001;//??time1?????????ac??  
//s0=(int)(time0*340);//ab?? 
//s1=(int)(time1*340);//ab?? 
//a=(int)(s0*s0/4); 
//b=(int)(300-a); 
//c=(int)(s1*s1); 
//d=(int)(300-c); 
//y=(int)((450*(a*d+c*d)+sqrt(8*225^2*a*b*c*d+4*b*d(c-a)*(a*d-b*c)))/2*(a*d-b*c));  
//x=(int)((a*(y-225)*(y-225)-a*b)/b); 
//display(x,y); 
//} 
///************************???**************************** 
//???:main() 
//??: ????? 
//????:? 
//????:? 
//************************************************************/ void main() 
//{ 
//Interrupt_Init(); //???????? 
//lcd_init(); //???LCD 
//while(1) 
//{ 
//if(voice3==0) 
//{ 
//DATA_H1=TH1; 
//DATA_L1=TL1; 
//TH1=0; 
//TL1=0; 
//TR1=0; 
//voicechuli(); 
//} 
//} 
//} 
////***********???0??******************* 
//Int0() interrupt 0//??????????????? 
//{ 
//TR0=1; 
//TR1=1; 
//EX0=0; 
//} 
////************??????***************** 
//Int1() interrupt 2//??????????????? 
//{ 
//DATA_H0=TH0; 
//DATA_L0=TL0; 
//TH0=0; 
//TL0=0; 
//TR0=0; 
//EX1=0; 
//}