initial_route.c

16位单片机dsPIC30F4013的PWM初始化及起的飞行控制系统中的应用

//***********计算程序，计算起点及终点的平面坐标，以及直线的斜率*************//
//**************************************************************************//
#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°之间
		}													
}