Pages: [1]   Go Down
 Author Topic: Problem with float & trigonometric functions  (Read 1355 times) 0 Members and 1 Guest are viewing this topic.
0
Offline
Newbie
Karma: 0
Posts: 24
Arduino rocks
 « on: November 29, 2009, 05:03:37 pm » Bigger Smaller Reset

I have the following code, trying to calculate a distance between two geo-coordinates in meters:

Code:
#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
//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:

Code:
Lat_old: 49.668731
Lon_old: 7.989830
Lat: 49.668731
Lon: 7.989810
Distance in meters: 0.0

Does anybody see my mistake?

Thanks for any help.
 Logged

Offline
Edison Member
Karma: 3
Posts: 1001
Arduino rocks
 « Reply #1 on: November 29, 2009, 05:12:10 pm » Bigger Smaller Reset

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).
 Logged

Seattle, WA USA
Offline
Brattain Member
Karma: 601
Posts: 48556
Seattle, WA USA
 « Reply #2 on: November 29, 2009, 05:13:26 pm » Bigger Smaller Reset

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:

Code:
temp0=temp0-(temp0*2);

Wouldn't "temp0 = -temp0;" be easier to understand, quicker to type, and faster to execute?
 Logged

0
Offline
Newbie
Karma: 0
Posts: 24
Arduino rocks
 « Reply #3 on: November 29, 2009, 05:24:36 pm » Bigger Smaller Reset

When I convert the lat & lon values to radians I get the same result: distance = 0.0.
So this cannot be the problem, sorry.

Code:
#include <math.h>

void setup(){
Serial.begin(4800);
float Pi = 3.1415926535897932384626433;
float lat_old = 49.66873;
float lon_old = 7.98983;
float lat = 49.66873;
float lon = 7.98981;

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
//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 :-)

}
 Logged

Offline
Edison Member
Karma: 3
Posts: 1001
Arduino rocks
 « Reply #4 on: November 29, 2009, 05:37:48 pm » Bigger Smaller Reset

Quote
So this cannot be the problem, sorry.
Well -one error doesn't exclude another.

Here's working Arduino code for calculating distance between two points.

Code:
// 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).

 Logged

0
Offline
Newbie
Karma: 0
Posts: 24
Arduino rocks
 « Reply #5 on: November 30, 2009, 04:13:34 am » Bigger Smaller Reset

That did the trick! Thanks a lot!
 Logged

Bedum. grunn, The netherlands
Offline
Newbie
Karma: 0
Posts: 22
Arduino rocks
 « Reply #6 on: May 20, 2010, 03:23:47 am » Bigger Smaller Reset

:S i do not understand ..
what is the final code for calculating distance then ?
 Logged

 Pages: [1]   Go Up