Hey everybody,
I’m working on a GPS navigated robot and my distance measurements seem… off. My code is below. It keeps telling me that the next waypoint is 9448313m away. It’s not though, it’s across the street. Any advice?
void distance() {
float flat1 = flat; // flat1 = our current latitude. flat is from the gps data.
float flon1 = flon; // flon1 = our current longitude. flon is from the fps data.
float dist_calc = 0;
float dist_calc2 = 0;
float diflat = 0;
float diflon = 0;
x2lat = 33 ; //enter a latitude point here this is going to be your waypoint
x2lon = -84;
//---------------------------------- distance formula below. Calculates distance from current location to waypoint
diflat = radians(x2lat - flat1); //notice it must be done in radians
flat1 = radians(flat1); //convert current latitude to radians
x2lat = radians(x2lat); //convert waypoint latitude to radians
diflon = radians((x2lon) - (flon1)); //subtract and convert longitudes to radians
dist_calc = (sin(diflat / 2.0) * sin(diflat / 2.0));
dist_calc2 = cos(flat1);
dist_calc2 *= cos(x2lat);
dist_calc2 *= sin(diflon / 2.0);
dist_calc2 *= sin(diflon / 2.0);
dist_calc += dist_calc2;
dist_calc = (2 * atan2(sqrt(dist_calc), sqrt(1.0 - dist_calc)));
dist_calc *= 6371000.0; //Converting to meters
Serial.println("distance");
Serial.println(dist_calc); //print the distance in meters
}
