arduino - 半正矢公式错误 - 不正确的距离 - Arduino

Question

目前正在参与一个项目，该项目需要从 GPS 模块获取读数，然后使用这些读数计算读数与固定航路点之间的距离。 GPS 工作并给出 LAT - 54.9289 和 LON - -1.368 的值，这应该给出约 3,200 米的距离。不过它给出了6105左右。我也有一种感觉6105是公里哈哈。我想知道它是否没有正确获取负数或者代码中是否存在一些变量冲突。任何对此的启发都会很棒，谢谢。

#include <TinyGPS.h>
#include <SoftwareSerial.h>
#include <rgb_lcd.h>
#include <Wire.h>

//Sets TX And RX Pins
SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void CheckGPS();
void GetCoords();
 long lat, lon;
 float LAT, LON; // Latitude is gained from GPS and stored in another variable to avoid errors - Should change with changing GPS value - Which would alter distance to waypoint.
 float LAT1,LON1;
rgb_lcd lcd;


void setup()
{
// Sets Baud Rate
  GPS.begin(9600);
  Serial.begin(115200);
}
// Determines The Distance Between Current Location And Waypoint

void GetDistance() 
{
  // Calculating Distance Between Waypoints

  double Distance_Lat; // Distance between Lattitude values
  double Distance_Lon; // Distance between Lonitude values
  double Distance_Total = 0;// Total Distance 
  double val,val2; // Subsidary variable for holding numbers. - No actual value represented.
  double fLAT1,fLAT2;
  double LAT2 = 54.900000; // Waypoint Latitude
  double LON2 = -1.368072; // Waypoint Longitude

 // Initialising Calculation
  Distance_Lat = radians(LAT2-LAT1); // Must be done in radians
  fLAT1 = radians(LAT1);
  fLAT2 = radians(LAT2);
  Distance_Lon = radians((LON2)-(LON1));

  // Calculating Distance - Using Haversines Formulae
  Distance_Total = (sin(Distance_Lat/2.0)*sin(Distance_Lat/2.0));
  val = cos(fLAT1);
  val = val*(cos(fLAT2));
  val = val*(sin(Distance_Lon/2.0));
  val = val*(sin(Distance_Lon/2.0));
  Distance_Total = Distance_Total + val;

  Distance_Total = 2*atan2(sqrt(Distance_Total),sqrt(1.0-Distance_Total)); 
  Distance_Total = Distance_Total*6371.0000; // Converting to meters.
Serial.println("Distance: ");
Serial.println(Distance_Total);

  //---------------------------------------------------------------------------------
}

// Returns Latitude And Longitude As Decimal Degrees (DD).
void GetCoords()
{
 long lat, lon;
  CheckGPS();
  Serial.print("Latitude : ");
  Serial.print(LAT/1000000,7);
  Serial.print(" :: Longitude : ");
  Serial.println(LON/1000000,7);

}

void CheckGPS()

  {
  bool newdata = false;
  unsigned long start = millis();
 // Every 1 seconds, Print an update
  while (millis() - start < 1000)
    {
    if (feedgps ())
    newdata = true;
    if (newdata)
    gpsdump(gps);
    }
  }

// Checks If The GPS Has Any Data To Transmit
  bool feedgps()
  {
    while (GPS.available())
    if (gps.encode(GPS.read()))
      return true;
    else
      return false;  
  }

// Transmits GPS Data And Gets Latitude And Longitude Positions.
  void gpsdump(TinyGPS &gps)

{    gps.get_position(&lat, &lon);
     LAT = lat;
     LON = lon;
     //Keeps The GPS Fed To Avoid Checksum Errors.
     feedgps();

}

void loop()
{
// Function That Returns The GPS Coordinates In DD.
  GetCoords();
  GetDistance();
}

Answer 1

我现在在维基百科上查看的半正矢公式，https://en.wikipedia.org/wiki/Haversine_formula ，有 arcsin(sqrt(Distance_Total)) ，其中有 atan2。