caculate_distance.c

飞行控制中的转换坐标下的距离计算

#include<p30f4013.h>
#include<math.h>
float x0,y0,distance,k,x0,y0,x1,y1,x2,y2,x,y,t;
void main(void)
{
	k=-4.528753;
	y2=283541.4;
	x2=3545851;
	x1=3380331;
	y1=320090.2;
	x0=3379128;
	y0=316920.1;
	t=x0-x1+(y0-y1)/k;
	y=(t*k)/(k*k+1);
	x=(k*k*t)/(k*k+1);
	distance=sqrt((y0-y1-y)*(y0-y1-y)+(x0-x1-x)*(x0-x1-x));
}