I have the following code, trying to calculate a distance between two geo-coordinates in meters:
#include <math.h>
void setup(){
Serial.begin(4800);
float lat_old = 49.66873;
float lon_old = 7.98983;
float lat= 49.66873;
float lon = 7.98981;
float dist = acos(sin(lat)*sin(lat_old)+cos(lat)*cos(lat_old)*cos(lon-lon_old))*6378000.0;
Serial.print("Lat_old: ");
serial_printFloat(lat_old);
Serial.print("Lon_old: ");
serial_printFloat(lon_old);
Serial.print("Lat: ");
serial_printFloat(lat);
Serial.print("Lon: ");
serial_printFloat(lon);
Serial.print("Distance in meters: ");
serial_printFloat(dist);
}
void loop(){
}
void serial_printFloat(float value) {
//Check if negative value and print - sign
if(value<0)
{Serial.print("-");}
//Print the number before the decimal point
//We check if the number before the decimal point
//is negative. If it is then we make it into a positive
//number. So we don't get something like --10.286
//As we already printed the negative sign (-)
int temp0=(int)value;
if(temp0<0){
temp0=temp0-(temp0*2);
}
Serial.print(temp0);
//Print decimal point
Serial.print(".");
//We insert the numbers after the deciaml (.) into temp
//Using long as we can store more values
//Add additional zeros (000) to 1000000 to increase degree of accuracy
//beyond the decimal point
long temp=long((value - (long)value) * 1000000);
//If temp is negative we convert it to a positive number so we
//don't get something like -0.-286
if(temp<0){
temp=temp-(temp*2);
}
Serial.println(temp);
//And that's it. We're done :-)
}
The distance should yield to 1.48152 meters, however, the output via serial monitor always gives me the following output:
The trigonometric functions expects radians not degrees. You need to convert your lat/lon to radians before you use them in your calculations (deg*PI/180).
I don't understand the formula for converting lat/long pairs to distance, so I don't know if that's right or wrong. You could add a Serial.print statement to see if the result is even close to right.
I have a question about the serial_printFloat funcction, though. I two places you have code like this:
temp0=temp0-(temp0*2);
Wouldn't "temp0 = -temp0;" be easier to understand, quicker to type, and faster to execute?
When I convert the lat & lon values to radians I get the same result: distance = 0.0.
So this cannot be the problem, sorry.
#include <math.h>
void setup(){
Serial.begin(4800);
float Pi = 3.1415926535897932384626433;
float lat_old = 49.66873;
float lat_old_rad = lat_old* Pi/180.0;
float lon_old = 7.98983;
float lon_old_rad = lon_old* Pi/180.0;
float lat = 49.66873;
float lat_rad = lat* Pi/180.0;
float lon = 7.98981;
float lon_rad = lon* Pi/180.0;
float dist = acos(sin(lat_rad)*sin(lat_old_rad)+cos(lat_rad)*cos(lat_old_rad)*cos(lon_rad-lon_old_rad))*6378000.0;
Serial.print("Lat_old: ");
serial_printFloat(lat_old);
Serial.print("Lon_old: ");
serial_printFloat(lon_old);
Serial.print("Lat: ");
serial_printFloat(lat);
Serial.print("Lon: ");
serial_printFloat(lon);
Serial.print("Distance in meters: ");
serial_printFloat(dist);
}
void loop(){
}
void serial_printFloat(float value) {
//Check if negative value and print - sign
if(value<0)
{Serial.print("-");}
//Print the number before the decimal point
//We check if the number before the decimal point
//is negative. If it is then we make it into a positive
//number. So we don't get something like --10.286
//As we already printed the negative sign (-)
int temp0=(int)value;
if(temp0<0){
temp0=temp0-(temp0*2);
}
Serial.print(temp0);
//Print decimal point
Serial.print(".");
//We insert the numbers after the deciaml (.) into temp
//Using long as we can store more values
//Add additional zeros (000) to 1000000 to increase degree of accuracy
//beyond the decimal point
long temp=long((value - (long)value) * 1000000);
//If temp is negative we convert it to a positive number so we
//don't get something like -0.-286
if(temp<0){
temp=temp-(temp*2);
}
Serial.println(temp);
//And that's it. We're done :-)
}
Here's working Arduino code for calculating distance between two points.
// calculate distance in rad between two points
float c_dist(float lat1,float lon1,float lat2,float lon2)
{
// the standard version of formulae yields unacceptable precision
// return acos(sin(lat1)*sin(lat2)+cos(lat1)*cos(lat2)*cos(lon2-lon1));
// alternative formulae (haversine)
const float two=2.0;
return two*asin(sqrt(square(sin((lat1-lat2)/two)) +
cos(lat1)*cos(lat2)*square(sin((lon2-lon1)/two))));
}/*c_dist*/
To get distance in meters you must multiply the returned value with earth's radius (#define EARTH_RADIUS 6371009.0).