Haversine Formula Error - Incorrect Distance - Arduino - arduino

Question

Currently taking part in a project that requires to take readings from a gps module and then using these calculate the distance between the readings and a fixed waypoint. The Gps works and gives the values of LAT - 54.9289 and LON - -1.368 this should give a distance of about 3,200 meters. however it gives around 6105. I also have a feeling that 6105 is km to haha. Im wondering if its not taking the negative numbers correctly or if i have some variable conflicts in the code. Any light shed on this would be great, thanks.
#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

The haversine formula I'm looking at on Wikipedia at right now, https://en.wikipedia.org/wiki/Haversine_formula, has arcsin(sqrt(Distance_Total)) where you have your atan2.