caculate_distance.c

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

#include<p30f4013.h>
#include<math.h>
float x0,y0,distance,t;
void caculate_distance(void)
{	
	t=x-x1+(y-y1)/k;							//计算附加值t
	y0=(t*k)/(k*k+1);							//计算e点横纵坐标
	x0=(k*k*t)/(k*k+1);							//计算e点纵坐标
	distance=sqrt((y-y1-y0)*(y-y1-y0)+(x-x1-x0)*(x-x1-x0));	//计算所处位置与航向间的偏距
}