Go Down

### Topic: Problem with float & trigonometric functions (Read 2904 times)previous topic - next topic

#### fritzbrause

##### Nov 29, 2009, 11:03 pm
I have the following code, trying to calculate a distance between two geo-coordinates in meters:

Code: [Select]
`#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:

Code: [Select]
`Lat_old: 49.668731Lon_old: 7.989830Lat: 49.668731Lon: 7.989810Distance in meters: 0.0`

Does anybody see my mistake?

Thanks for any help.

#### borref

#1
##### Nov 29, 2009, 11:12 pm
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).

#### PaulS

#2
##### Nov 29, 2009, 11:13 pm
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: [Select]
`temp0=temp0-(temp0*2);`

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

#### fritzbrause

#3
##### Nov 29, 2009, 11:24 pm
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: [Select]
`#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 :-)}`

#### borref

#4
##### Nov 29, 2009, 11:37 pm
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: [Select]
`// calculate distance in rad between two pointsfloat 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).

#### fritzbrause

#5
##### Nov 30, 2009, 10:13 am
That did the trick! Thanks a lot!

#### diamantmatch

#6
##### May 20, 2010, 10:23 am
:S i do not understand ..
what is the final code for calculating distance then ?

Go Up