经纬度计算两地距离

目前能提供经纬度距离的算法有很多，以下方法是将地球看为椭圆进行处理计算。具体算法过程不详解（感兴趣的可以网上搜索具体计算过程），这里只列出代码实现部分，代码可以直接植入单片机中，进行计算。

---

注:由于地球是不规则的椭球体，并且实际地理位置还需考虑海拔情况，因此计算会存在一定误差。
实际使用情况：本人采用了NEO-M8N的GPS&BD双模定位模块，测量有一定的距离误差，不适于高精度场合，合适长距离估算。

---

#define PI  3.1415926

//度转换成幅度值
double rad(double d)  
{  
     return d * PI / 180.0;  
}
/*******************************************************************************
* 函数名 : Get_GPSdistance
* 描述   : 计算经纬度距离
* 输入   : 参数：第一点经度，纬度 ，第二点经度，纬度
* 返回   : 返回距离，单位M
* 注意   : 
*******************************************************************************/
double Get_GPSdistance(double lon1, double lat1,double lon2, double lat2)
{
        double EARTH_RADIUS = 6378137;
        double x1 =0;
        double y1 =0; 
        double z1=0;
        double x2 =0;
        double y2 =0; 
        double z2=0;
        double d=0;

        double theta=0;
        double dist=0;



        double radLat1 = rad(lat1);  
        double radLat2 = rad(lat2);  

        double radLon1 = rad(lon1);  
        double radLon2 = rad(lon2);  

        if (radLat1 < 0)  
                radLat1 = PI / 2 + abs(radLat1);// south  
        if (radLat1 > 0)  
                radLat1 = PI / 2 - abs(radLat1);// north  
        if (radLon1 < 0)  
                radLon1 = PI * 2 - abs(radLon1);// west  
        if (radLat2 < 0)  
                radLat2 =  PI / 2 + abs(radLat2);// south  
        if (radLat2 > 0)  
                radLat2 =  PI / 2 -  abs(radLat2);// north  
        if (radLon2 < 0)  
                radLon2 =  PI * 2 -  abs(radLon2);// west  

         x1 = EARTH_RADIUS *  cos(radLon1) *  sin(radLat1);  
         y1 = EARTH_RADIUS *  sin(radLon1) *  sin(radLat1);  
         z1 = EARTH_RADIUS *  cos(radLat1);  

         x2 = EARTH_RADIUS *  cos(radLon2) *  sin(radLat2);  
         y2 = EARTH_RADIUS *  sin(radLon2) *  sin(radLat2);  
         z2 = EARTH_RADIUS *  cos(radLat2);  

         d =  sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)+ (z1 - z2) * (z1 - z2));  
        //余弦定理求夹角  
        theta =  acos((EARTH_RADIUS * EARTH_RADIUS + EARTH_RADIUS * EARTH_RADIUS - d * d) / (2 * EARTH_RADIUS * EARTH_RADIUS));  
        dist = theta * EARTH_RADIUS;  

        return dist/100.0;   //比实际算法小100倍数 (M)

}