Arduino 計算兩個衛星定位點之間的距離和相對角度

/*
 *   2018-11-09 Test OK
 *
 * 計算兩個衛星定位點之間的距離和相對角度
 * 這個函式是從TinyGPSPlus-0.92 Libraty 擷取出來的
 *
 *
 */

void setup() {
  Serial.begin(115200, SERIAL_8N1);

//distanceBetween(24.1365, 121.6588,24.1403, 121.64723);

Serial.println(distanceBetween(24.1365, 121.6588,24.1403, 121.64723));
Serial.println(courseTo(24.1365, 121.6588,24.1403, 121.64723));


}


void loop() {


}

/* static */
double distanceBetween(double lat1, double long1, double lat2, double long2){
  /*
  // returns distance in meters between two positions, both specified
  // as signed decimal-degrees latitude and longitude. Uses great-circle
  // distance computation for hypothetical sphere of radius 6372795 meters.
  // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
  // Courtesy of Maarten Lamers
  //返回指定的兩個位置之間的距離（以米為單位）
     //作為帶符號的十進制度緯度和經度。 使用大圓
     //半徑為6372795米的假想球體的距離計算。
     //因為地球不是精確的球體，所以舍入誤差可能高達0.5％。
     //由Maarten Lamers提供
*/
  double delta = radians(long1 - long2);
  double sdlong = sin(delta);
  double cdlong = cos(delta);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  double slat1 = sin(lat1);
  double clat1 = cos(lat1);
  double slat2 = sin(lat2);
  double clat2 = cos(lat2);
  delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
  delta = sq(delta);
  delta += sq(clat2 * sdlong);
  delta = sqrt(delta);
  double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
  delta = atan2(delta, denom);
  return delta * 6372795;
}

double courseTo(double lat1, double long1, double lat2, double long2){
  /*
  // returns course in degrees (North=0, West=270) from position 1 to position 2,
  // both specified as signed decimal-degrees latitude and longitude.
  // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
  // Courtesy of Maarten Lamers
  //從位置1到位置2以度數（北= 0，西= 270）返迴路線，
     //均指定為帶符號的十進制度緯度和經度。
     //因為地球不是精確的球體，計算過程可能會受到很小的影響。
     //由Maarten Lamers提供
*/
  double dlon = radians(long2 - long1);
  lat1 = radians(lat1);
  lat2 = radians(lat2);
  double a1 = sin(dlon) * cos(lat2);
  double a2 = sin(lat1) * cos(lat2) * cos(dlon);
  a2 = cos(lat1) * sin(lat2) - a2;
  a2 = atan2(a1, a2);
  if (a2 < 0.0)
  {
    a2 += TWO_PI;
  }
  return degrees(a2);
}