GPS, finally

[Minuteman]

OK, guys, I think it's finally time to try my hand at GPS-assisted navigation. My objective is to start at a "known" location (my driveway) and have my rover navigate down the street, to the park, around a few waypoints, and back to my house. I will use my current dead reckoning algorithm for control and navigation, but I will infuse GPS updates using a weighted average so that hopefully the current estimated location will be reasonably accurate. By using dead reckoning, the instantaneous direction and control should be very fluid (like it is currently).

I have mostly ignored GPS discussions, but going back through some some of the older posts, it looks like there is no standard library that everyone uses for parsing. I see that the ublox devices are popular. The NEO-6M seems ridiculously cheap. What would I miss compared to the NEO-M8? I'm planning to use the UBX binary format. Any suggestions on a good arduino based library?

I naively envision this method as working really well, and being simple to implement. Any guesses on what the big hurdles will be?

Thanks,

Nathan

[Wayne Holder]

I look forward to reading about your progress.  My current favorite "all around" uBlox module is the NEO-M8N, as it supports most of the other systems in addition to GPS and has a fast update rate (18 Hz when using only GPS and 10 Hz when using GLONASS, BeiDou, etc.)  There are a lot of cheap versions of this module on eBay, but I prefer this one for experimentation:

  http://www.csgshop.com/product.php?id_product=173

as it gives access to USB, I2C and Serial ports.  Most uBlox modules directly support a USB interface but few breakout boards seem to bother adding the connector.  Having a USB port makes it easier to test and configure the module with the uBlox U-Center application, which I find convenient to use as a way to test and characterize a module.  This same CSG-made breakout PCB also comes with a decent antenna (taoglas) as opposed to some no-name ones you get out of China.  In my experience, the quality of the antenna can make a significant different in how well a receiver performs.  In fact, 

If you go with a module that takes an external antenna, make sure it can provide power to the LNA (low voice amplifier) in the antenna and use an antenna that has an LNA, as this also helps reception and can be required of the breakout PCB does not include an LNA with the GPS module.  My favorite antennas are the Tallysman series (carried by Digi-Key) as they're well spec'd and come in a variety of configurations.  I used a Tallysman antenna with good results with my NEO-6P module as part of my experiments with "Decimeter Precision GPS" (although I've now upgraded to a NEO-7M module from CSG because of its faster, 10 Hz update rate.)

However, having used many of the commercial GPS modules, the differences you're going to see in one module to the next as not that great, as all are limited to around 2.0-2.5 meters CEP accuracy under the best of conditions.  So, you're idea of starting with something cheap, such as the NEO-6M, is not a bad option, either.  I recommend using the uBlox binary protocol, but caution that it can be complicated and confusing in places, and uBlox has a habit of introducing new commands and deprecating old ones on a fairly frequent basis.  So, you should also try to consult the correct programing manual for the module you're using.  For this reason, I've avoided writing a general-purpose library for the uBLox modules, so far.  However, I have a lot of codeI've written for use with the uBlox series and I'm happy to share it, or answer questions so you can write your own.

To get you started, I've attached a copy of one my test apps.  The code is designed to run on an Arduino Leonardo (because it supports a 2nd serial port and allows the use of the USB port as a monitor.)  This code can be configured to run using either a serial port to talk to the uBLox GPS, or vis the I2C port (Arduino Wire l;library.)  Of the two, I recommend using I2C, as the serial port is harder to work with due to the fact that it's hard to sort out ACK/NAK responses to commands from the steam of position packets it want to constantly spit out.

If this code has a weakness it's that there are a few configuration commands, such as CFG_PRT (enableProtocols()) that are hard to implement because they require you to first read the current state of certain bits so you can include these in the config command packet.  So, to keep my code simple, I've kludged around this by hard coding some of this data rather than first reading it from the module. 

Now, to share some of my frustrations with GPS modules, in general, I've found that one big annoyance with all the consumer modules it that they seem to use some sort of internal model, perhaps a Kalman filter, or something similar, to process the GPS fix information before spitting out a fix.  With the uBlox modules, you can sort of configure this model with the CFG_NAV (setDynamicModel()) command and set it for "pedestrian", "automotive", etc., but that about it.  I'd prefer getting rawer fix information, but only the more advanced modules, such as the NEO-6P and NEO-7P give access to this kind of information.  And, what you actually get is just the raw "pseudo range" measurements, which means you'd need to implement most of the GPS code stack, or use something like RTKLIB to convert it into a proper location fix.

What this means, in practice, is that the module is really telling you where it thinks you are based on this model rather than where the raw data might indicate you are.  This makes it difficult to integrate with the heading and position you might compute via odometry and other means.   

Wayne

  // Designed to run on an Arduino Leonardo (ATMega32u4-based) board

#define  USE_SERIAL  0    // If 1, talk to UBlox using Serial1, else use I2C

#define  DEBUG       0    // If 1, enable debug messages

#if !USE_SERIAL

#include <Wire.h>

#endif

#include "Arduino.h"

#if USE_SERIAL

#define send(args) Serial1.write(args)

#define read(args) Serial1.read(args)

#else

#define send(args) Wire.write(args)

#define read(args) Wire.read(args)

#endif

#define  output  Serial

#define  BUF_SIZE  400

int gpsAddress = 0x42;    //GPS I2C Address

unsigned char  state = 0;

unsigned char  cls, id, chk1, chk2;

unsigned int   pktLen, pktIdx;

bool           acked;

typedef struct {

  unsigned long  iTOW;    // GPS Millisecond Time of Week

  long           lon;     // Longitude (1e-7)

  long           lat;     // Latitude (1e-7)

  long           height;  // Height above Ellipsoid (mm)

  long           hMSL;    // Height above mean sea level (mm)

  unsigned long  hAcc;    // Horizontal Accuracy Estimate (mm)

  unsigned long  vAcc;    // Vertical Accuracy Estimate (mm)

} NAV_POSLLH;

typedef struct {

  unsigned long  iTOW;    // GPS Millisecond Time of Week

  unsigned int   gDOP;    // Geometric DOP

  unsigned int   pDOP;    // Position DOP

  unsigned int   tDOP;    // Time DOP

  unsigned int   vDOP;    // Vertical DOP

  unsigned int   hDOP;    // Horizontal DOP

  unsigned int   nDOP;    // Northing DOP

  unsigned int   eDOP;    // Easting DOP

} NAV_DOP;

typedef struct {

  unsigned long  iTOW;    // GPS Millisecond Time of Week

  unsigned char  gpsFix;  // Fix type: 0=none, 1=dead rek, 2=2D, 3=3D, etc.

  unsigned char  flags;   // Bitfield

  unsigned char  fixStat;

  unsigned char  flags2;  // Bitfield

  unsigned long  ttff;

  unsigned long  msss;

} NAV_STATUS;

typedef struct {

  unsigned char  rsvd;    // Reserved

  unsigned long  cpMesL;  // Carrier phase measurement [L1 cycles] (low 32 bits of IEEE 754 Double Precision)

  unsigned long  cpMesH;  // Carrier phase measurement [L1 cycles] (high 32 bits of IEEE 754 Double Precision)

  unsigned long  prMesL;  // Pseudorange measurement [m] (low 32 bits of IEEE 754 Double Precision)

  unsigned long  prMesH;  // Pseudorange measurement [m] (high 32 bits of IEEE 754 Double Precision)

  unsigned long  doMes;   // Doppler measurement [Hz] (IEEE 754 Single Precision)

  unsigned char  sv;      // Space Vehicle Number

  signed char    mesQI;   // Nav Measurements Quality Indicator

  signed char    cno;     // Signal strength C/No. (dbHz)

  unsigned char  lli;     // Loss of lock indicator (RINEX definition)

} RAW_ITEM;

typedef struct {

  unsigned long  iTOW;    // GPS Millisecond Time of Week

  unsigned int   week;    // Measurement GPS week number (Receiver Time).

  unsigned char  numSV;   // # of satellites following

  unsigned char  rsvd;    // Reserved

  RAW_ITEM       sat[];   // Array of RAW_ITEMs

} RXM_RAW;

typedef struct {

  unsigned char  chn;     // Channel

  unsigned char  svid;    // Satellite Id

  unsigned char  flags;   // B0=Used for Nav, B7=smoothed (see manual for others)

  unsigned char  quality; // Bits 3-0 - 0-Idle, 1=Search, 2=Aquired, 3=Unusable, 4=Code Lock, 5,6,7=Code and Carrier locked

  unsigned char  cno;     // Carrier to Noise Ratio (Signal Strength in dbHz)

  signed char    elev;    // Elevation in integer degrees

  int            azim;    // Azimuth in integer degrees

  long           prRes;   // Pseudo range residual in centimetres

} SVINFO_ITEM;

typedef struct {

  unsigned long  iTOW;    // GPS Millisecond Time of Week

  unsigned char  numCh;   // Number of channels

  unsigned char  gFlags;  // Bits 2-0 - 0 = Anataris, 1 = uBlox5, 2 = uBlox6

  unsigned int   rsvd;    // Reserved

  SVINFO_ITEM    sItem[]; // Array of SVINFO_ITEM

} NAV_SVINFO;

typedef struct {

  unsigned char  msgClass;

  unsigned char  msgId;

  unsigned char  rate;

} CFG_MSG;

typedef struct {

  unsigned int   measRate;

  unsigned int   navRate;    // Must equal 1

  unsigned int   timeRef;

} CFG_RATE;

typedef struct {

  unsigned int   mask;

  unsigned char  dynModel;

  unsigned char  fixMode;

  long           fixedAlt;

  unsigned long  fixedAltVar;

  signed char    minElev;

  unsigned char  drLimit;

  unsigned int   pDop;

  unsigned int   tDop;

  unsigned int   pAcc;

  unsigned int   tAcc;

  unsigned char  staticHoldThresh;

  unsigned char  dgpsTimeOut;

  unsigned long  reserved2;

  unsigned long  reserved3;

  unsigned long  reserved4;

} CFG_NAV5;

typedef struct {

  unsigned char  portId;

  unsigned char  reserved0;

  unsigned int   txReady;

  unsigned long  mode;

  unsigned long  baudRate;

  unsigned int   inProtoMask;

  unsigned int   outProtoMask;

  unsigned int   reserved4;

  unsigned int   reserved5;

} CFG_PRT;

typedef struct {

  char          swVersion[30];

  char          hwVersion[10];

  char          romVersion[30];

  char          extension[][30];

} MON_VER;

union {

  unsigned char  buf[BUF_SIZE];

  NAV_POSLLH     pos;

  NAV_DOP        dop;

  NAV_STATUS     stat;

  RXM_RAW        raw;

  CFG_NAV5       nav5;

  MON_VER        ver;

  NAV_SVINFO     svInfo;

} UBX_PKTS;

void sendCmd (unsigned char id1, unsigned char id2, unsigned char *data, unsigned int len) {

  unsigned char chk1 = 0, chk2 = 0, cc;

#if !USE_SERIAL

  Wire.beginTransmission(gpsAddress);

#endif

  send(0xB5);

  send(0x62);

  unsigned char hdr[] = {id1, id2, len & 0xFF, len >> 8};

  for (int ii = 0; ii < sizeof(hdr); ii++) {

    send(cc = hdr[ii]);

    chk1 += cc;

    chk2 += chk1;

  }

  for (int ii = 0; ii < len; ii++) {

    send(cc = data[ii]);

    chk1 += cc;

    chk2 += chk1;

  }

  send(chk1);

  send(chk2);

#if !USE_SERIAL

  Wire.endTransmission();

#endif

  // Give time to process shutting down messages

  for (int ii = 0; ii < 20; ii++) {

    process();

    delay(10);

  }

}

void sendRetry (unsigned char id1, unsigned char id2, unsigned char *data, unsigned int len) {

  acked = false;

  while (true) {

    sendCmd(id1, id2, data, len);

    if (acked)

      break;

    delay(100);

#if DEBUG

    output.println(F("Retry"));

#endif

  }

}

void enableProtocols (bool ubx, bool nmea) {

  CFG_PRT prt;

  unsigned int val = (ubx ? 1 : 0) | (nmea ? 2 : 0);

#if USE_SERIAL

  prt.portId = 1;

  prt.reserved0 = 0x0F;

  prt.txReady = 0x42;    // read from unit

  prt.mode = 0x8C0;      // read from unit

  prt.baudRate = 9600;   // read from unit

#else

  prt.portId = 0;

  prt.reserved0 = 0;

  prt.txReady = 0x42;    // read from unit

  prt.mode = 0x84;       // read from unit

  prt.baudRate = 0;      // read from unit

#endif

  prt.inProtoMask = val;

  prt.outProtoMask = val;

  prt.reserved4 = 0;

  prt.reserved5 = 0;

  sendCmd(0x06, 0x00, (unsigned char *) &prt, sizeof(prt));

}

void enableMsg (unsigned char cls, unsigned char msg, unsigned char rate) {

#if DEBUG

  output.print(F("enableMsg: cls: "));

  output.print(cls, HEX);

  output.print(F(", msg: "));

  output.println(msg, HEX);

#endif

  CFG_MSG enable;

  enable.msgClass = cls;

  enable.msgId = msg;

  enable.rate = rate;

  sendRetry(0x06, 0x01, (unsigned char *) &enable, sizeof(enable));

}

/*

 *  Set rate that measurements are taken

 *  msUpdate - milliseconds between updates (200 = 5Hz)

 */

void setUpdateRate (unsigned int msUpdate) {

  CFG_RATE rate;

  rate.measRate = msUpdate;

  rate.navRate = 1;    // Must be 1

  rate.timeRef = 1;    // Referenced to GPS time (?)

  sendCmd(0x06, 0x08, (unsigned char *) &rate, sizeof(rate));

}

  /*

   *  Set dynamic model, as follows:

   *    0 - Portable

   *    2 - Stationary

   *    3 - Pedestrian

   *    4 - Automotive

   *    5 - Sea

   *    6 - Airborne with <1g Acceleration

   *    7 - Airborne with <2g Acceleration

   *    8 - Airborne with <4g Acceleration

   */

void setDynamicModel (unsigned char model, signed char elev) {

  CFG_NAV5 nav5;

  nav5.mask = 3;          // Set dynamic model and elev

  nav5.dynModel = model;

  nav5.minElev = elev;

#if 0

  nav5.fixMode = 0;

  nav5.fixedAlt = 0;

  nav5.fixedAltVar = 0;

  nav5.drLimit = 0;

  nav5.pDop = 0;

  nav5.tDop = 0;

  nav5.pAcc = 0;

  nav5.tAcc = 0;

  nav5.staticHoldThresh = 0;

  nav5.dgpsTimeOut = 0;

  nav5.reserved2 = 0;

  nav5.reserved3 = 0;

  nav5.reserved4 = 0;

  traceCmd(0x06, 0x24, (unsigned char *) &nav5, sizeof(nav5));

#endif

  sendCmd(0x06, 0x24, (unsigned char *) &nav5, sizeof(nav5));

}

void setup () {

 #if USE_SERIAL

  Serial1.begin(9600);

#else

  Wire.begin(); 

  Wire.beginTransmission(gpsAddress);

  send(0xFF);                      // Set regisaddress

  Wire.endTransmission();

#endif

  output.begin(115200);

  delay(4000);

#if USE_SERIAL

  output.println(F("U-Blox NEO-6P - Serial 1 Interface"));

#else

  output.println(F("U-Blox NEO-6P - DDC (I2C) Interface"));

#endif

  while (process())

    ;

  // Disable unused messages

  enableMsg(0x01, 0x02, 0);            // NAV-POSLLH - Geodetic Position Solution

  enableMsg(0x01, 0x06, 0);            // NAV-SOL - Navigation Solution Information (ECEF)

  enableMsg(0x01, 0x04, 0);            // NAV_DOP - Dilution of precision

  enableMsg(0x01, 0x01, 0);            // NAV-POSECEF - Position Solution in ECEF

  enableMsg(0x01, 0x32, 0);            // NAV-SBAS - SBAS Status Data

  enableMsg(0x01, 0x03, 0);            // NAV-STATUS - Receiver Navigation Status

  enableMsg(0x01, 0x30, 0);            // NAV-SVINFO - Space Vehicle Information

  enableMsg(0x02, 0x10, 0);            // RXM-RAW - Raw Measurement Data

  // Configure ports and other items

  sendCmd(0x0A, 0x04, 0, 0);           // Query MON-VER

  enableProtocols(true, false);        // CFG-PRT - Enable UBX, Disable NMEA

  setDynamicModel(2, 15);              // CFG-NAV5 - Navigation Engine Settings

  setUpdateRate(1000);                 // CFG-RATE (NEO-6P limited to 1000 ms)

#if 1

  // Verify that changes made by CFG-NAV5 have been applied

  sendCmd(0x06, 0x23, 0, 0);           // Query CFG-NAVX5 (responds with ACK?)

  sendCmd(0x06, 0x24, 0, 0);           // Query CFG-NAV5 (responds with ACK?)

#endif

  // Enable used messages

  enableMsg(0x01, 0x02, 1);            // NAV-POSLLH - Geodetic Position Solution

  enableMsg(0x01, 0x30, 1);            // NAV-SVINFO - Space Vehicle Information

}

void loop () {

  process();

}

#if DEBUG

int idx = 0;

#endif

bool process() {

  bool processed = false;

#if USE_SERIAL

  while (Serial1.available()) {

#else

  Wire.beginTransmission(gpsAddress);

  send(0xFD);                      // Select MSB of data available count

  Wire.endTransmission();

  Wire.requestFrom(gpsAddress, 2);

  unsigned int count = (unsigned int) read() << 8;

  count |= (unsigned int) read();

  if (count == 0) {

    return false;

  }

  Wire.requestFrom(gpsAddress, count);

  while (Wire.available()) {

#endif

    unsigned char cc = read();

#if DEBUG

    if ((idx++ & 0x1F) == 0) {

      output.println();

    }

    if (cc < 16) {

      output.print('0');

    }

    output.print(cc, HEX);

    output.print(F(", "));

#endif

    switch (state) {

    case 0:  // Wait for sync 1 (0xB5)

      chk1 = chk2 = 0;

      if (cc == 0xB5) {

        state++;

      }

      break;

    case 1:  // wait for sync 2 (0x62)

      if (cc == 0x62) {

        state++;

      } else {

        state = 0;

      }

      break;

    case 2:  // wait for class code

      cls = cc;

      chk1 += cc;

      chk2 += chk1;

      state++;

      break;

    case 3:  // wait for Id

      id = cc;

      chk1 += cc;

      chk2 += chk1;

      state++;

      break;

    case 4:  // wait for length byte 1

      pktLen = cc;

      chk1 += cc;

      chk2 += chk1;

      state++;

      break;

    case 5:  // wait for length byte 2

      pktLen |= (cc << 8);

      pktIdx = 0;

      chk1 += cc;

      chk2 += chk1;

      state++;

      break;

    case 6:  // wait for <pktLen> payload bytes

      UBX_PKTS.buf[pktIdx++] = cc;

      chk1 += cc;

      chk2 += chk1;

      if (pktIdx == pktLen || pktIdx == sizeof(UBX_PKTS.buf)) {

        state++;

      }

      break;

    case 7:  // wait for checksum 1

      if (chk1 == cc) {

        state++;

      } else {

        output.print(F("Chk1 fail: cls: "));

        output.print(cls, HEX);

        output.print(F(", id: "));

        output.print(id, HEX);

        output.print(F(", len: "));

        output.print(pktLen, DEC);

        output.print(F(" - computed "));

        output.print(chk1, HEX);

        output.print(F(" received "));

        output.print(cc, HEX);

        output.println();

        state = 0;

      }

      break;

    case 8:  // wait for checksum 2

      if (chk2 == cc) {

        decodePacket();

      } else {

        output.print(F("Chk2 fail: cls: "));

        output.print(cls, HEX);

        output.print(F(", id: "));

        output.print(id, HEX);

        output.print(F(", len: "));

        output.print(pktLen, DEC);

        output.print(F(" - computed "));

        output.print(chk1, HEX);

        output.print(F(" received "));

        output.print(cc, HEX);

        output.println();

      }

      state = 0;

      break;

    }

    processed = true;

  }

  return processed;

}

void decPrint3 (int val) {

  if (val < 100)

    output.print(' ');

  if (val < 10)

    output.print(' ');

  output.print(val, DEC);

}

void decPrint5 (int val) {

  int tmp = abs(val);

  if (tmp < 1000)

    output.print(' ');

  if (tmp < 100)

    output.print(' ');

  if (tmp < 10)

    output.print(' ');

  if (val >= 0)

    output.print(' ');

  output.print(val, DEC);

}

void decodePacket () {

  // Note: process <pktLen> bytes

#if DEBUG

  output.println();

#endif

  if (cls == 0x01 && id == 0x02 && pktLen == 28) {

    // NAV_POSLLH packet

    output.print(UBX_PKTS.pos.iTOW, DEC);

    output.print(F(" - NAV_POSLLH - Lat: "));

    output.print((float) UBX_PKTS.pos.lat / 10000000.0, DEC);

    output.print(F(", Lon: "));

    output.print((float) UBX_PKTS.pos.lon / 10000000.0, DEC);

    output.print(F(", hAcc: "));

    output.print(UBX_PKTS.pos.hAcc, DEC);

    output.print(F(" mm"));

    output.println();

  } else if (cls == 0x01 && id == 0x30) {

    // NAV_SVINFO packet

    output.print(UBX_PKTS.svInfo.iTOW, DEC);

    output.print(F(" - SVINFO - numCh: "));

    output.print(UBX_PKTS.svInfo.numCh, DEC);

    output.println();

    for (int ii = 0; ii < UBX_PKTS.svInfo.numCh; ii++) {

      output.print(F("  Id: "));

      decPrint3(UBX_PKTS.svInfo.sItem[ii].chn);

      output.print(F(", elev: "));

      decPrint3(UBX_PKTS.svInfo.sItem[ii].elev);

      output.print(F(", Azim: "));

      decPrint3(UBX_PKTS.svInfo.sItem[ii].azim);

      output.print(F(", cno: "));

      decPrint3(UBX_PKTS.svInfo.sItem[ii].cno);

      output.print(F(", prRes: "));

      decPrint5(UBX_PKTS.svInfo.sItem[ii].prRes);

      output.print(F(", used: "));

      output.print(((UBX_PKTS.svInfo.sItem[ii].flags & 0x01) != 0 ? "Y" : "N"));

      output.print(F(", smoothed: "));

      output.print(((UBX_PKTS.svInfo.sItem[ii].flags & 0x80) != 0 ? "Y" : "N"));

      output.println();

     }

  } else if (cls == 0x02 && id == 0x10) {

    // RXM_RAW packet

    output.print(UBX_PKTS.raw.iTOW, DEC);

    output.print(F(" - RXM_RAW ("));

    output.print(UBX_PKTS.raw.numSV, DEC);

    output.println(F(" sats)"));

    for (int ii = 0; ii < UBX_PKTS.raw.numSV; ii++) {

      output.print(F("    "));

      output.print(F("Sat: "));

      int sat = UBX_PKTS.raw.sat[ii].sv;

      if (sat < 100)

        output.print(F(" "));

      if (sat < 10)

        output.print(F(" "));

      output.print(sat, DEC);

      output.print(F(", "));

      output.print(UBX_PKTS.raw.sat[ii].cpMesH, HEX);

      output.print(F(", "));

      output.print(UBX_PKTS.raw.sat[ii].cpMesL, HEX);

      output.println();

    }

  } else if (cls == 0x06 && id == 0x23) {

    // CFG_NAVX5 Packet

    output.print(F("CFG_NAVX5 - usePPP: "));

    output.print((unsigned char) UBX_PKTS.buf[26], DEC);

    output.println();

  } else if (cls == 0x06 && id == 0x24) {

    // CFG_NAV5 Packet

    output.print(F("CFG_NAV5 - dynModel: "));

    output.print(UBX_PKTS.nav5.dynModel, DEC);

    output.print(F(", minElev: "));

    output.print(UBX_PKTS.nav5.minElev, DEC);

    output.println();

  } else if (cls == 0x0A && id == 0x04) {

    // MON_VER Packet (swVersion: 6.02 (36023), hwVersion: 00040007, romVersion: )

    output.print(F("MON_VER - swVersion: "));

    output.print((char *) &UBX_PKTS.ver.swVersion);

    output.print(F(", hwVersion: "));

    output.print((char *) &UBX_PKTS.ver.hwVersion);

    output.print(F(", romVersion: "));

    output.print((char *) &UBX_PKTS.ver.romVersion);

    if (pktLen > 70) {

      int exts = (pktLen - 70) / 30;

      for (int ii = 0; ii < exts; ii++) {

        output.print(F(", ext"));

        output.print(ii + 1, DEC);

        output.print(F(": "));

        output.print((char *) &UBX_PKTS.ver.extension[ii]);

      }

    }

    output.println();

  } else if (cls == 0x05 && (id == 0x00 || id == 0x01) && pktLen == 2) {

    // ACK, or NAK packets

    acked = true;

    output.print(id == 1 ? "ACK" : "NAK");

    unsigned char aCls = UBX_PKTS.buf[0];

    unsigned char aId = UBX_PKTS.buf[1];

    if (aCls == 0x06 && aId == 0x01) {

      output.println(F(" CFG_MSG"));

    } else if (aCls == 0x06 && aId == 0x08) {

      output.println(F(" CFG_RATE"));

    } else if (aCls == 0x06 && aId == 0x00) {

      output.println(F(" CFG_PRT"));

    } else if (aCls == 0x06 && aId == 0x23) {

      output.println(F(" CFG_NAVX5"));

    } else if (aCls == 0x06 && aId == 0x24) {

      output.println(F(" CFG_NAV5"));

    } else {

      output.print(F(" cls: "));

      output.print(aCls, HEX);

      output.print(F(", id: "));

      output.print(aId, HEX);

      output.println();

    }

  } else {

    output.print(F("Pkt cls: "));

    output.print(cls, HEX);

    output.print(F(", id: "));

    output.print(id, HEX);

    output.print(F(", len: "));

    output.print(pktLen, DEC);

    output.println();

    output.print(F("    "));

    for (int ii = 0; ii < pktLen; ii++) {

      unsigned jj = UBX_PKTS.buf[ii];

      if (jj < 16)

        output.print('0');

      output.print(jj, HEX);

      if ((ii & 0x0F) == 0x0F) {

        output.println();

        output.print(F("    "));

      } else {

        output.print(F(", "));

      }

    }

    output.println();

  }

}

--
You received this message because you are subscribed to the Google Groups "diyrovers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.
To post to this group, send email to diyr...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/f99b2fa8-c6e5-4fd5-8a36-3503aae69803%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Minuteman]

Wayne,

Thanks for the detailed reply. I think I will go with the NEO-6M for now, but while it arrives I will break out one of my old Garmins and an RS232 chip...haven't used one of those for ages! Despite your recommendation, I will probably try serial with my Ublox. I don't know if the 6M supports I2c, but the teensy has buffered serial ports, so I'm eager to see how that works. I will give your code a try when I get the Ublox.

I've been considering how to implement GPS in my code. The first question that comes up is coordinate system. Iv'e seen some discussion on whether it's better to do all navigation in LAT/LON, but I think I will use my current local x,y coordinate system. I will just make the starting point 0,0, and will have to convert all gps coordinates to my local system. I think (hope) this will have minimal impact on my navigation routines.

As I've mentioned, the plan is to use the dead reckoning (DR)  coordinates to navigate (same as now). The incoming GPS data will be analyzed to determine if it is valid, and to what degree it should be weighted in the position calculation. If I get a position report that indicates an error of more the 10 ft compared to the current DR-based position, I will discard it. Otherwise I will probably average it with the DR-based position at about 10% (ie, new coordinate = DR * 0.9 + GPS * 0.1). This should cause the measured position to converge on the true position over time.

Does the GPS give a good indicator on the reliability of the current readings? Does it correlate well with the number of satellites? If I know how reliable the readings are, then I can have the weighting factor adjusted to add-in more GPS when the signal is good.

Also I have heard that the heading reported by the GPS is generally very good, especially when traveling in a straight line. Assuming this is true, I would have a running average on my turn rate (from the gyro) to determine when the vehicle is moving straight. When it has moved straight for more than a couple of seconds, the DR-based heading would be very heavily weighted with the GPS-based heading. I'm curious to see if this will work.

Any comments on these strategies are welcome.

Thanks,

Nathan

[Thomas Roell]

What a shame, sorry to be late to the discussion ;-)

NEO-7M is currently the best. Passive patch is good, active patch is better. The best I found was the NEO-7M-C unit from Waveshare, you can get via various sources from ebay or aliexpress. The only issue I had with those units in general is that the little battery they have can come lose too easily. A high speed impact is not surviveable for them, unless you use epoxy to strengthen them, and/or use heat shrink around the unit before mounting it. Lesson learned the hard way. N.b. the same is true for most units out there, unless you get something like a VK2635U7G5LF, which has better internal mounting.

NEO-M8N sucks. If you have GLONASS enabled at the same time as GPS (yes, even if you get WAAS to work), the position is off, and HDOP/VDOP are worse than with NEO-7M. I have not seen a passive patch NEO-M8N beat an active patch NEO-7M (and that is really all you can get).

NEO-6 sucks. It will work only with 4Hz relieable, and position jumps around badly. Tested way too many units there. Given the price delta to NEO-7/NEO-8 it does not seem to be a good investment. 

From the github I posted somewhere else there is gps.c/gps.h which should have a nice parser plus some easy config options. I used that in buffered uart mode (16 byte FIFO) off an ISR. No problem. The code is more complex than Wayne's, but it was meant to be async in a task or an ISR. 

The heading is still one of my headaches. Below a certain speed (1m/sec) you can forget about the heading. For ublox I added code to my parser that would return what the GPS thought it's heading accuracy was. The other minor detail (and one of my items of the list of things to look into) is that the heading has a delay relative to the position that depends upon the dynamic mode. For example if you select pedestrian mode, it will lag by almost half a second. It's better in automotive mode. I did some check with simply deriving the heading from looking at the last position and the current position. That worked for some cases (low speed), but was worse at high speed. A good indicator for the lag is if you correlate compass heading with what the GPS heading tells you. The compass starts to tell you a turn way earlier than the GPS (yes, I know that the precision of compass heading is not that great, but in this case it's a nice indicator).

I played around with I2C and UBlox (including their DRDY logic). It did not seem to make sense perf wise. I seem to recall that the PVT packet for NEO-7/NEO-8 is only up to 92 bytes. With a 10Hz report rate that is about 1kb/sec, nothing you cannot handle even with 38400 baud over a serial line. N.b. there is always latency between the reports and when the measurement had been taken. The latency I saw with a 115200 baud serial connection was about 80ms at average, slighly less on a NEO-M8N. You have to compensate for that anyway, so using I2C does not seem to offer any real advantage.

Coordinate system. I am using X/Y/Z based upon a simple transformation from LLA with a home-location. LLA can only be precisely represented with doubles, not floats. A local X/Y/Z coordinate system will work with floats.

Ublox gives you back the usual HDOP/VDOP/PDOP/TDOP, as well as it's estimates of 1-sigma for 2D position, vertical altitude, heading accurancy and speed accurancy. If you'd go to a NMEA based parser they'd also break out the 2D position accurancy into longitudinal and lateral ... ah well ;-)

I am using a similar scheme as to what you describe or are targeting at. It uses an EKF to weigh the Gyro/WheelTick against the GPS data. It turned out to be fairly good at that, except that GPS is sometimes off, which means your system will report a relative large error relative to the GPS position, except that in reality GPS is off. Main issue though is/was that the GPS heading is delayed, but if you update your gyro-heading with that, it creates more uncertainty. Not a biggy with a 10Hz GPS rate, but not what I wanted.

- Thomas

[Thomas Roell]

Here some quick links off e-bay of the units I'd recommend (should I say that I have a small collection at home by now ;-))

If you want a NEO-7M with active patch (currently my go to module):

http://www.ebay.com/itm/UART-GPS-NEO-7M-C-Serial-GPS-Module-with-High-Gain-Active-Antenna-IPX-interface-/251588237772?pt=LH_DefaultDomain_0&hash=item3a93d3d9cc

If you want a NEO-M8N:

http://www.ebay.com/itm/RY835AI-18Hz-USB-GPS-Glonass-BeiDou-antenna-module-pressure-Compass-Gyro-sensors-/181566850426?pt=LH_DefaultDomain_0&hash=item2a463a457a

If you want a NEO-7M with I2C accessable:

http://www.ebay.com/itm/RY725AI-10Hz-UART-USB-interface-GPS-Glonass-QZSS-antenna-module-flash-memory-/181562403752?pt=LH_DefaultDomain_0&hash=item2a45f66ba8

If you want to go cheap (ok, it is nice as it has mounting holes):

http://www.ebay.com/itm/V-KEL-VK2635U7G5LF-56CH-GPS-Module-Ublox7-Chip-Built-in-FLASH-1-10Hz-g-/281681998920?pt=LH_DefaultDomain_0&hash=item41958e3448

- Thomas

...

[Minuteman]

Not too late. Not decisions are final :)

I will seriously consider the NEO-7M now. I was about to purchase the 6M.

Do you prefer NMEA over the Ublox binary? I have looked at the TinyGPS++ library, which parses NMEA. I like that it is asynchronous. 

If LLA can't be handled (accurately) by a float, how to do you convert it to your x/y/z without losing accuracy? 

As for the heading latency, I would only use the GPS heading when moving quickly in a straight line over several seconds. With very low gyro drift, I only need to rely on the GPS heading to help correct very long-term drift in the gyro (ie over a minute or two). There are plenty of occasions when the car is travelling straight so that the GPS heading should good enough for making corrections in drift.

On Friday, July 10, 2015 at 12:40:50 PM UTC-6, Thomas Roell wrote:

...

[Thomas Roell]

My pet peeve is PPS output, and having connectors that I can mess around with ;-)

NMEA vs. UBLOX. I have code for both ;-) It does not make a *real* difference, other than the numbers of bytes used, at least for position reports. But there are a few details. NMEA as in UBLOX has a few extensions (PUBX,xx) which have extended information. That is good, except that the sentence that deals with the SVINFO is capped at 22 SVs, and is huge. Standard NMEA has no clock bias/drift, and is missing vertical velocity and is missing GPS to UTC correction (leap seconds). I had some code that could parse those PUBX sentences, but I threw that out again, as UBX binary had the same information, but with less pain to extract (especially with GLONASS). For debugging initially I preferred NMEA, but after I got the binary parser stable, I switched over. There is much less data from the GPS, so less latency, and less chance in the ISR to miss data. N.b. for UBLOX you can only configure the unit using UBX binary, and of course the ACK coming back is in binary mode. So even if you'd parse only NMEA for the location reports, you still need the UBX binary parser.

I do not like TinyGPS++. It does not properly match sentences with the proper time tag. And it does not emit reports if there was no fix. So you don't know whether you missed something (that bit me in AVC2014), and in a lot of cases you don't know how old a report was. If you only want to look at GPGGA/GPRMC, that may be ok, but as soon as you want to have more information it will haunt you.

LLA2FLAT (or LLA2NED) is simple. You can set up a scale factor for latitude and longitude that converts the difference between you current longitude/latitude and that of a home latitude/longitude into X/Y offsets relative to the home locations. UBX binary (and most other binary protocols) use a 1e7 scaling on latitude/longitude and transmit those as int32_t. Since you only use the difference between 2 int32_t and those are numerically close together, you will not overflow the 24 bit mantissa. I'll paste in the code later ...

With the latency, I am just mentioning it, as most folks are not aware of that. And if you try to correct your current gyro heading with something that is 200ms old (or even older), you correct the wrong thing.

- Thomas

[Minuteman]

Not true, if you are still moving in the same heading as 200ms ago! :)

[Thomas Roell]

Well, that's the point. You don't know ;-) You can guess.

- Thomas

On Fri, Jul 10, 2015 at 3:40 PM, Minuteman <nathan....@gmail.com> wrote:

Not true, if you are still moving in the same heading as 200ms ago! :)

 

if you try to correct your current gyro heading with something that is 200ms old (or even older), you correct the wrong thing.

--
You received this message because you are subscribed to a topic in the Google Groups "diyrovers" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/diyrovers/N4T0LrBVfcE/unsubscribe.
To unsubscribe from this group and all its topics, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/848bdaa4-783e-43f6-aee3-a68848283823%40googlegroups.com.

[Minuteman]

Or, you can let your gyro tell you...

[Jon Watte]

I do not like TinyGPS++.

Ditto! I prefer the older TinyGPS C code to TinyGPS++.


Sincerely,

Jon Watte


--
"I find that the harder I work, the more luck I seem to have." -- Thomas Jefferson

--

You received this message because you are subscribed to the Google Groups "diyrovers" group.

To unsubscribe from this group and stop receiving emails from it, send an email to diyrovers+...@googlegroups.com.

To post to this group, send email to diyr...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/diyrovers/8d6513cc-8b94-4f66-836a-97bab591eb60%40googlegroups.com.

[Thomas Roell]

Ok, sorry for the delay. Here the snipped. I attached the gps parser at the end of the e-mail.

typedef struct _lla_home_t {
    int32_t latitude;
    int32_t longitude;
    int32_t altitude;
    float   S[3];
} lla_home_t;

void LLA2HOME(int32_t latitude, int32_t longitude, int32_t altitude, lla_home_t *home )
{
    double lat, slat, clat, Rn, Rm;

    const double a = 6378137;
    const double e = 8.1819190842622e-2;
       
    lat = (double)latitude * (DEG2RAD / 1e7);
   
    slat = sin(lat);
    clat = cos(lat);
   
    Rn = a / sqrt(1 - e*e * slat*slat);
    Rm = (Rn * (1-e*e)) / (1 - e*e * slat*slat);

    home->latitude = latitude;
    home->longitude = longitude;
    home->altitude = altitude;
    home->S[0] = (double)(DEG2RAD / 1e7) * Rm;
    home->S[1] = (double)(DEG2RAD / 1e7) * Rn * clat;
    home->S[2] = (double)(1.0 / 1e3) * (-1.0);
}

void LLA2NED(int32_t latitude, int32_t longitude, int32_t altitude, const lla_home_t *home, float ned[3] )
{
    float dlat, dlon, dalt;

    dlat = (float)(latitude - home->latitude);
    dlon = (float)(longitude - home->longitude);
    dalt = (float)(altitude - home->altitude);

    ned[0] = home->S[0] * dlat;
    ned[1] = home->S[1] * dlon;
    ned[2] = home->S[2] * dalt;
}

- Thomas