GPS Distance Calculation

I'm trying to calculate the distance between two sets of coordinates, one set coming from a GPS module. I'm using the Haversine formula, which produces the correct answer when I enter it directly into Wolfram Alpha. I'm also using the TinyGPS library. Unfortunately, my code on the Arduino Duemilanove is not producing the correct answer, and I can't figure out why.

  float flat, flon;
    gps.f_get_position(&flat, &flon);            
    long dest_latitude = xx.xxxxxxxx;
    long dest_longitude = -yy.yyyyyyyy;                  
    int radius = 3956.6;                               
    const float two = 2.0;
    float delta_lat = radians(dest_latitude - flat);
    float delta_lon = radians(dest_longitude - flon);
    float a = square(sin(delta_lat/two)) + cos(radians (flat)) * cos(radians (dest_latitude)) * square(sin(delta_lon/two));
    float c = two * asin(sqrt(a));
    float d = radius * c;   

(it goes on from here)

When I'm approximately 15.5 miles away from the destination coordinates, the Arduino board and GPS tell me I'm only 12.75 miles away. When I am at the destination coordinates, it tells me I'm 17.79 miles away. All of my calculations are floating point. What am I doing wrong?

For starters, you're declaring the earth's radius as an int, not a float. Are radians() and square() your functions?

long dest_latitude = 45.001867;
    long dest_longitude = -93.364265;                  
    int radius = 3956.6;

Those are all going to give you problems.

Okay, thanks. I’ve made them all “float.”

I’ve included the math.h library in my code, but for some reason, it isn’t colored orange like any of the other libraries I’ve included.

#include <EEPROM.h>
#include <NewSoftSerial.h>
#include <TinyGPS.h>
#include <math.h>

When I simulate what my code would have done in Wolfram Alpha, I get 12.64 miles - very close to the 12.75 I’ve been getting. I’ll try out the new code tonight. Thanks.

it doesn't turn orange because it's not one of the Arduino provided libraries listed in the keywords files. As long as it doesn't complain when you compile, don't worry about it. You could try changing all of your floats to doubles to see if that makes any difference in the results

Here’s mine if you are looking to compare different alogrithms. It’s pretty much cut’n’paste from the Ardupilot project


  • //Function to calculate the distance between two waypoints
    float calc_dist(float flat1, float flon1, float flat2, float flon2)
    float dist_calc=0;
    float dist_calc2=0;
    float diflat=0;
    float diflon=0;

//I’ve to spplit all the calculation in several steps. If i try to do it in a single line the arduino will explode.

dist_calc = (sin(diflat/2.0)sin(diflat/2.0));
dist_calc2= cos(flat1);
dist_calc +=dist_calc2;


dist_calc*=6371000.0; //Converting to meters
return dist_calc;

I just wanted to report back that the problem has been solved. Changing the variables from Int to Float worked just fine. Thanks!

Care to share the rest of the code that worked?