GPS Distance measurement

Hi Guys!

My first project is an GPS autopilot RC boat. I wrote a code, that is measure the distance between two GPS point. The two GPS point is(in the code):

  • local = the present/actual GPS coordinate where i am
  • cel = if i press the Joystick Switch then is will be the present/actual.

If i control the boat to destiantion, then i press the button, and the coordinate will save into cel variables. Then i control back the boat to me and i hope i can measure the distance from between this two point. I hope it will be very accurate…

My first problem is, when i stand still, press the button then the actual coordinate will be the destination coordinate too. So the distance need to be 0, or cloe to it, don’t? But i see on serial monitor, that the distance is going up from 0 to 60~70 or worst 100+… Is this normal??? I stand still, i hoped that this distance remain 0…

Ohh, i almost forgot: i use Arduino UNO with NEO-6M GPS as you see in the picture…

I paste the code, is there any wrong??

#include <TinyGPS++.h>
#include <SoftwareSerial.h>

float local[2] = {0,0};    // this is the actual place
float cel[2] = {0,0};      // this is the destination place
int SW = 9;                // this is the switch
int SW_state = 0;

float radius_of_earth = 63781000;   // radius_of_earth/m 
 
static const int RXPin = 3, TXPin = 2;
static const uint32_t GPSBaud = 9600;
 
TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);
 
void setup()
{
  Serial.begin(9600);
  ss.begin(GPSBaud);

pinMode(SW, INPUT_PULLUP);
pinMode(OUTPUT,8);
}


double distance_in_km(float start_lat, float start_long, float end_lat, float end_long) {

      start_lat/= 180 / PI; 
      start_long/= 180 / PI;
      end_lat/= 180 / PI;
      end_long/= 180 / PI;
      
      // haversine formula 
      float a = pow(sin((end_lat-start_lat)/2), 2) + cos(start_lat) * cos(end_lat) * pow(sin((end_long-start_long)/2), 2);
      float answer = radius_of_earth * 2 * atan2(sqrt(a), sqrt(1-a));
      return int(answer);
    }

 
void loop()
{

  while (ss.available() > 0)
    if (gps.encode(ss.read()))
      displayInfo();
}

 
void displayInfo()
{

     Serial.print("    -     Distance from the actual point to destenation point  ");
     Serial.print(distance_in_km(gps.location.lat(),gps.location.lng(), cel[0], cel[1]));

    if (distance_in_km(gps.location.lat(),gps.location.lng(), cel[0], cel[1]) < 80.00)
     {
    //  Serial.println("We arrived");
      tone(8, 500);
      delay(100);
      noTone(8);
     }
  
  if (distance_in_km(gps.location.lat(),gps.location.lng(), cel[0], cel[1]) > 81.10)
     {
  //    Serial.println("We are far from destination");
      noTone(8);
     }
  
    SW_state = digitalRead(SW);
   //   Serial.println(SW_state);
      if (SW_state == 0)               // switch pressed
      {
      cel[0]=gps.location.lat();      // is this ok, or need parameter????
      cel[1]=gps.location.lng();      // is this ok, or need parameter????
      
      
      }
 }
  else
  {
    Serial.print(F("INVALID"));
  }
 
  Serial.println();
}

What units is the 60-70 error you're seeing in? What is ACTUALLY being printed out?

You can always get some jitter in GPS readings so try printing both sets of GPS readings as well as the calculated distance.

Steve

The TinyGPS++ library contains a function to calculate the distance between two lat/lon points. Scroll down the page in the link to "Distance and Course".

You should use that function, as it is much more accurate than what you have written. Never use the pow() function to compute squares.

The following line is a serious mistake.

return int(answer);

Thanks for the tips, i use now the TinyGPS++ Distancebetween function. I used an example from internet.
It also not so accurate :frowning:

I post a video, where you can se, when i set the destenation coordinate to present coordinate the distance need to be 0~1m … But it sometime go up ~20-30 meter while i am standing still at the same place without move… (you can see in video too, in the row where the cursor is)

I make a run from the starting position for about 20-25m, it count almost good. I stay for there about 3-4min, after that i run back to start position… The distance begin reduced, but somethimes on halfway (10-12m) it begins to count upward to 30-40m (its interesting, becouse i getting close to destination, why it sense that i getting far from destination? :() and a little bit later begin to count downward but when i stop in the destination place it stop about 10-11m… Is this normal???

I am in the garden, clear sky, got fixed on 7-8-9 Satelite (as you can see in the video first row)

What i do wrong???

  • Better antenna fix this? I used the factory one, witch is so little.
  • The delay i use is good?

VIDEO : VID_20190915_111752

#include <TinyGPS++.h>
#include <SoftwareSerial.h>

static const int RXPin = 4, TXPin = 3;
static const uint32_t GPSBaud = 9600;
int SW = 9;
int SW_state = 0;

double LONDON_LAT = 0.0, LONDON_LON = 0.0;
TinyGPSPlus gps;
SoftwareSerial ss(RXPin, TXPin);

void setup()
{
  Serial.begin(115200);
  ss.begin(GPSBaud);
  pinMode(SW, INPUT_PULLUP);
  pinMode(OUTPUT,8);

  Serial.println(F("Sats HDOP  Latitude   Longitude   Fix  Date       Time     Date Alt    Course Speed Card  Distance Course Card  Chars Sentences Checksum"));
  Serial.println(F("           (deg)      (deg)       Age                      Age  (m)    --- from GPS ----  ---- to London  ----  RX    RX        Fail"));
  Serial.println(F("----------------------------------------------------------------------------------------------------------------------------------------"));
}

void loop()
{

  printInt(gps.satellites.value(), gps.satellites.isValid(), 5);
  printFloat(gps.hdop.hdop(), gps.hdop.isValid(), 6, 1);
  printFloat(LONDON_LAT, gps.location.isValid(), 11, 6);
  printFloat(LONDON_LON, gps.location.isValid(), 12, 6);
  printInt(gps.location.age(), gps.location.isValid(), 5);
  printDateTime(gps.date, gps.time);
  printFloat(gps.altitude.meters(), gps.altitude.isValid(), 7, 2);
  printFloat(gps.course.deg(), gps.course.isValid(), 7, 2);
  printFloat(gps.speed.kmph(), gps.speed.isValid(), 6, 2);
  printStr(gps.course.isValid() ? TinyGPSPlus::cardinal(gps.course.deg()) : "*** ", 6);

  unsigned long distanceKmToLondon =
    (unsigned long)TinyGPSPlus::distanceBetween(
      gps.location.lat(),
      gps.location.lng(),
      LONDON_LAT, 
      LONDON_LON);
  printInt(distanceKmToLondon, gps.location.isValid(), 9);

  double courseToLondon =
    TinyGPSPlus::courseTo(
      gps.location.lat(),
      gps.location.lng(),
      LONDON_LAT, 
      LONDON_LON);

  printFloat(courseToLondon, gps.location.isValid(), 7, 2);

  const char *cardinalToLondon = TinyGPSPlus::cardinal(courseToLondon);

  printStr(gps.location.isValid() ? cardinalToLondon : "*** ", 6);

  printInt(gps.charsProcessed(), true, 6);
  printInt(gps.sentencesWithFix(), true, 10);
  printInt(gps.failedChecksum(), true, 9);
  Serial.println();
  
  smartDelay(1);
  
  SW_state = digitalRead(SW);
      if (SW_state == 0) 
      {
      LONDON_LAT=gps.location.lat();
      LONDON_LON=gps.location.lng();
      }
  if (millis() > 5000 && gps.charsProcessed() < 10)
    Serial.println(F("No GPS data received: check wiring"));
}

static void smartDelay(unsigned long ms)
{
  unsigned long start = millis();
  do 
  {
    while (ss.available())
      gps.encode(ss.read());
  } while (millis() - start < ms);
}

static void printFloat(float val, bool valid, int len, int prec)
{
  if (!valid)
  {
    while (len-- > 1)
      Serial.print('*');
    Serial.print(' ');
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1); // . and -
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i=flen; i<len; ++i)
      Serial.print(' ');
  }
  smartDelay(0);
}

static void printInt(unsigned long val, bool valid, int len)
{
  char sz[32] = "*****************";
  if (valid)
    sprintf(sz, "%ld", val);
  sz[len] = 0;
  for (int i=strlen(sz); i<len; ++i)
    sz[i] = ' ';
  if (len > 0) 
    sz[len-1] = ' ';
  Serial.print(sz);
  smartDelay(0);
}

static void printDateTime(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print(F("********** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }
  
  if (!t.isValid())
  {
    Serial.print(F("******** "));
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour()+2, t.minute(), t.second());
    Serial.print(sz);
  }

  printInt(d.age(), d.isValid(), 5);
  smartDelay(0);
}

static void printStr(const char *str, int len)
{
  int slen = strlen(str);
  for (int i=0; i<len; ++i)
    Serial.print(i<slen ? str[i] : ' ');
  smartDelay(0);
}

Most GPS units are accurate to 2-3 meters only under the best possible conditions, that is, clear view of the blue sky, lots of satellites visible, no nearby buildings, etc. You should expect and plan for position errors of 50 meters or more from time to time, or no satellite fix at all.

It also not so accurate

Is that the distance calculation, or the error in your position?

To check for errors in distance calculation, enter your endpoint coordinates into the calculator on this web site: Calculate distance and bearing between two Latitude/Longitude points using haversine formula in JavaScript

Here is a video of a small autonomous model boat navigating a course 60 meters on a side. Even on a lake, the GPS signal drops out from time to time, so it uses a magnetic compass to keep on track.

Edit: Just a question: i used the Arduino Uno 3-4 PIN to connect to GPS TX/RX port… But somewhere i read, that i need to use the Ardunio Uno 0-1 PIN (witch is dedicated to serial TX/RX)… Is this can cause the error?

This is not a distance calculator’s error… The TinyGPS++ Distancebetween function (and other’s what i try) works great!

This error due the jumping gps coorinates while i’m standing still…

I found a lot picture on the internet, where the coordinates are the same:

But with my gps its looks like this:

Is this normal? With a better antenna can i solve this? Now i got a mini ceramic antenne on the NEO-6M, like this:

Is this normal?

Yes. GPS coordinates "jump" and "wander" due to the motion of the GPS satellites, interference from clouds, trees, buildings, etc.

See these examples from a GPS unit on top of a mountain: Smoky Mountains GPS wander

But how can Bait Boat guaranteed +/-1m accuracy?
The most GPS reciever like (NEO-6M) have 2,5-3m treshold.

Is there any way to increase the accuracy? Maybe adding 3-axis gyroscope, 3-axis accelerometer and 3-axis magnetometer help???

But how can Bait Boat guaranteed +/-1m accuracy?

You can spend more money and buy or build an RTK GPS unit, but to get 20 cm accuracy or better absolutely requires a nearby base station with accurately known position.

But there are no guarantees in life.