//***********計算程序，計算起點及終點的平面坐標，以及直線的斜率*************//
//**************************************************************************//
#include<p30f4013.h>
float k1,x1,y1,x2,y2,k2,angle,fly_flag,angle1;					
unsigned char spw[9]={0x33,0x30,0x33,0x30,0x2E,0x30,0x30,0x30,0x30};
unsigned char spj[10]={0x31,0x31,0x34,0x32,0x30,0x2E,0x30,0x30,0x30,0x30};				//起點的經緯度坐標值
unsigned char dpw={0x33,0x32,0x30,0x30,0x2E,0x30,0x30,0x30,0x30};
unsigned char dpj={0x31,0x32,0x30,0x30,0x30,0x2E,0x30,0x30,0x30,0x30};					//終點的經緯度坐標值
//*******************計算起點的經緯度*************//
//************************************************//
void caculate_xy1(void)															
{
	double a0,a3,a4,a5,a6,N,cb,sb,c2b,L0,L1,L,m,t,L2,L3;
	float rjd,rwd;
	int N1;
	const double pi=3.141592653589,p=206264.806247;								//定義π值及一弧度的秒值
	rjd=(spj[0]-0x30)*100+(spj[1]-0x30)*10+(spj[2]-0x30)+((float)(spj[3]-0x30)*10+(float)(spj[4]-0x30)+(float)(spj[6]-0x30)/10+(float)(spj[7]-0x30)/100+(float)(spj[8]-0x30)/1000+(float)(spj[9]-0x30)/10000)/60;
	rwd=(spw[0]-0x30)*10+(spw[1]-0x30)+((float)(spw[2]-0x30)*10+(float)(spw[3]-0x30)+(float)(spw[5]-0x30)/10+(float)(spw[6]-0x30)/100+(float)(spw[7]-0x30)/1000+(float)(spw[8]-0x30)/10000)/60;				//將經緯度數據轉換成度的形式			
	N1=(rjd/6);																	//計算某點已知經度后的帶號,以便下一步計算中央子午線的經度	
	L0=6*N1-3;																	//按6°帶計算中央子午線的經度
	L1=rjd-L0;																	//計算與中央子午線的經度差;
	m=(int)L1;																	//經度于中央子午線的差值的度提取出來
	t=L1-m;																		//再將分提取出來
	L2=m*3600+t*3600;															//轉換成秒的格式
	L=L2/p;																		//標準化為弧度
	L3=L*L;																		//計算差值的平方
	rwd=(rwd*pi)/180;															//將經緯度都化成弧度
	rjd=(rjd*pi)/180;
	cb=cos(rwd);																//計算經緯度的正余弦
	sb=sin(rwd);
	c2b=cb*cb;																	//計算余弦的平方
	a0=32140.404-(135.3302-(0.7092-0.0040*c2b)*c2b)*c2b;
	a3=(0.3333333+0.001123*c2b)*c2b-0.1666667;
	a4=(0.25+0.00252*c2b)*c2b-0.04166;
	a5=0.0083-(0.1667-(0.1968+0.0040*c2b)*c2b)*c2b;
	a6=(0.166*c2b-0.084)*c2b;
	N=6399698.902-(21562.267-(108.973-0.612*c2b)*c2b)*c2b;						//計算公式所需系數
	x1=6367558.4969*rwd-(a0-(0.5+(a4+a6*L3)*L3)*L3*N)*sb*cb;
	y1=(1+(a3+a5*L3)*L3)*L*N*cb;													//按優化公式計算高斯-克呂格投影下的x,y坐標;
}
//***************計算終點的經緯度******************//
//*************************************************//
void caculate_xy2(void)
{
	double a0,a3,a4,a5,a6,N,cb,sb,c2b,L0,L1,L,m,t,L2,L3;
	float rjd,rwd;
	int N1;
	const double pi=3.141592653589,p=206264.806247;									//定義π值及一弧度的秒值
	rjd=(dpj[0]-0x30)*100+(dpj[1]-0x30)*10+(dpj[2]-0x30)+((float)(dpj[3]-0x30)*10+(float)(dpj[4]-0x30)+(float)(dpj[6]-0x30)/10+(float)(dpj[7]-0x30)/100+(float)(dpj[8]-0x30)/1000+(float)(dpj[9]-0x30)/10000)/60;
	rwd=(dpw[0]-0x30)*10+(dpw[1]-0x30)+((float)(dpw[2]-0x30)*10+(float)(dpw[3]-0x30)+(float)(dpw[5]-0x30)/10+(float)(dpw[6]-0x30)/100+(float)(dpw[7]-0x30)/1000+(float)(dpw[8]-0x30)/10000)/60;				//將經緯度數據轉換成度的形式			
	N1=(rjd/6);																	//計算某點已知經度后的帶號,以便下一步計算中央子午線的經度	
	L0=6*N1-3;																	//按6°帶計算中央子午線的經度
	L1=rjd-L0;																	//計算與中央子午線的經度差;
	m=(int)L1;																	//經度于中央子午線的差值的度提取出來
	t=L1-m;																		//再將分提取出來
	L2=m*3600+t*3600;															//轉換成秒的格式
	L=L2/p;																		//標準化為弧度
	L3=L*L;																		//計算差值的平方
	rwd=(rwd*pi)/180;															//將經緯度都化成弧度
	rjd=(rjd*pi)/180;
	cb=cos(rwd);																//計算經緯度的正余弦
	sb=sin(rwd);
	c2b=cb*cb;																	//計算余弦的平方
	a0=32140.404-(135.3302-(0.7092-0.0040*c2b)*c2b)*c2b;
	a3=(0.3333333+0.001123*c2b)*c2b-0.1666667;
	a4=(0.25+0.00252*c2b)*c2b-0.04166;
	a5=0.0083-(0.1667-(0.1968+0.0040*c2b)*c2b)*c2b;
	a6=(0.166*c2b-0.084)*c2b;
	N=6399698.902-(21562.267-(108.973-0.612*c2b)*c2b)*c2b;						//計算公式所需系數
	x2=6367558.4969*rwd-(a0-(0.5+(a4+a6*L3)*L3)*L3*N)*sb*cb;
	y2=(1+(a3+a5*L3)*L3)*L*N*cb;													//按優化公式計算高斯-克呂格投影下的x,y坐標;
}
//*********初始化路徑*********//
//****************************//
void initial_route()
{
	fly_flag=1;
	caculate_xy1();																//計算起點的x1,y1坐標
	caculate_xy2();																//計算終點的x2,y2坐標												
	k1=(x2-x1)/(y2-y1);															//計算路徑斜率
	k2=-1/k1;
	angle=atan(k1);																//計算夾角-弧度
	angle=angle*180/3.14159;													//轉換為角度
	angle1=angle;
	if(k1>0)
		{
			angle=90-angle;
		}
	else
		{
			angle=180-fabs(angle);												//計算設定路徑航行路線與正北方向的夾角，以上計算還將夾角限制在-180°-180°之間
		}													
}