經緯度計算兩地距離

目前能提供經緯度距離的算法有很多，以下方法是將地球看爲橢圓進行處理計算。具體算法過程不詳解（感興趣的可以網上搜索具體計算過程），這裏只列出代碼實現部分，代碼可以直接植入單片機中，進行計算。

---

注:由於地球是不規則的橢球體，並且實際地理位置還需考慮海拔情況，因此計算會存在一定誤差。
實際使用情況：本人採用了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)

}