mborgerson
Well-known member
A few weeks before Christmas, a friend contacted me about putting together a program to log position and IMU data from a ZED-F9R to help a friend of his analyze his performance as an amateur driver of a Porsche 911 on tracks in their area. The goal is to collect latitude, longitude, speed, XYZ gyro rates and XYZ acceleration values. He would like to collect the data at 25 samples per second and log it to an SD card for offline analysis using Matlab. A quick look at the ZED-F9R specs indicated that the module should be capable of delivering data at that rate.
I decided to use a T4.1 to collect the data using the Sparkfun-ublox_GNSS_Arduino_Library, since I had a Sparkfun board on loan for the project. After all, why reinvent the wheel if not necessary? What I didn't realize was exactly how big that wheel really was!
Library structure definitions: 2789 lines
Library .h header file: 1805 lines
Library.cpp source: 18,459 lines
A lot of the source lines are in excess of requirements for my project. I don't use all the debugging output, the RTK stuff and the helper functions that reduce the learning curve for Arduino programmers with little or no experience in decoding binary data streams. I also have a longstanding aversion to allocating data structures on the heap, which the library does in an attempt to reduce RAM overhead by only allocating structures that are actually used in your particular program. (The library does check for a NULL pointer each time before accessing a heap-based structure.) The allocation as required canb help a lot on board with less memory than a T4.1.
I then understood why my friend considered the logging program a bit beyond the capabilities of any of his nearby friends (I'm in Corvallis, Oregon and he is in Port Townsend, Washington.)
The ZED-F9R is a complex module: It handles just about every navigation satellite system, Real-Time-Kinematics, SBAS, etc. etc. It also does dead reckoning during loss of satellite signals. For the initial version of the logger, I don't need all those things. I need GNSS output at 25Hz with 100cm resolution and 2-meter accuracy. I also need IMU data at that same rate. I know the T4.1 can collect and log the data at the needed rate: 25 * 64 ,or 1600 bytes per second in a stream of binary packets collected into a buffer of about 8KB for efficient SD card writes.
After a few days studying the Sparkfun library and the ZED setup commands and UBX binary packet specifications, I put together a simple logger in a few more days. After a few days of tests, I started seeing some puzzling results.
1. The 1mAh rechargeable backup battery on the ZED module won't backup the GNN for hot starts for more than a couple of hours from a full charge. The battery charges at about 30uA and discharges at about 50uA. A couple of hours is OK for hot or warm starts, but can't depend on the configurations in the battery-backed-RAM to last overnight if the Sparkfun module is not connected to a 5V supply.
2. The UBlox manuals and U-Center software say that you can set the fix update interval to as low as 25 milliseconds, or 40Hz. I wanted 40 milliseconds for 25Hz. However, if you try to configure the ZED-F9R at that rate it doesn't stick. The actual minimum fix interval seems to be 50 milliseconds.
3. You can get position update packets at 25Hz by setting the priority navigation rate to 25Hz. That gives priority to certain NAV message, among them the NAV_PVT messages I am using. These delivered packets are apparently generated from the 20Hz actual computed fixes. I've found no good explanation of exactly how that is done, but I suspect it is the output of an internal Kalman filter working with the GNSS data and the IMU data.
4. If you try to do 20Hz fixes and 25Hz output using all the available satellite constellations, the overworked MPU inside the ZED seems to give up in surprising ways. (And not nice surprises!). I've seen many instances of the ZED either resetting to factory defaults, or losing some or all of the setup information I've sent to the module. This often means the Teensy stops collecting as the baud rate on the ZED UART output has changed and packets can no longer be decoded. This particular problem seems to be eliminated if I restrict the ZED to using just the GPS constellation and SBAS. It's not surprising that it can better cope with just eight satellites in view that with 24 satellites.
5. It may be possible to use more satellite constellations by reducing the fix rate below 20Hz. Perhaps that will reduce the ZED MPU load enough so that it can keep up with the extrapolated 25Hz output.
I'm in the process of writing a 'training wheel' subset of the Sparkfun library to handle just the NAV_PVT and ESF_MEAS packets I need for the logger. As a first step, I wrote a simple UBX packet parser that runs in about 40 lines of code. I then added functions to examine the collected packets and extract the required data from the UBX_NAV_PVT and UBX_ESF_MEAS packets while ignoring any other packets. That took about another hundred lines. I cribbed shamelessly from the Sparkfun library's structure definitions--which added up to another 70 or 80 lines of source. There will be more work to be done if I discover the need to handle other packets. For now, I'm sticking with a minimalist approach.
As a teaser here is a minimalist test program for the UBX packet parser: (note: functions to break out the data are not included.)
I decided to use a T4.1 to collect the data using the Sparkfun-ublox_GNSS_Arduino_Library, since I had a Sparkfun board on loan for the project. After all, why reinvent the wheel if not necessary? What I didn't realize was exactly how big that wheel really was!
Library structure definitions: 2789 lines
Library .h header file: 1805 lines
Library.cpp source: 18,459 lines
A lot of the source lines are in excess of requirements for my project. I don't use all the debugging output, the RTK stuff and the helper functions that reduce the learning curve for Arduino programmers with little or no experience in decoding binary data streams. I also have a longstanding aversion to allocating data structures on the heap, which the library does in an attempt to reduce RAM overhead by only allocating structures that are actually used in your particular program. (The library does check for a NULL pointer each time before accessing a heap-based structure.) The allocation as required canb help a lot on board with less memory than a T4.1.
I then understood why my friend considered the logging program a bit beyond the capabilities of any of his nearby friends (I'm in Corvallis, Oregon and he is in Port Townsend, Washington.)
The ZED-F9R is a complex module: It handles just about every navigation satellite system, Real-Time-Kinematics, SBAS, etc. etc. It also does dead reckoning during loss of satellite signals. For the initial version of the logger, I don't need all those things. I need GNSS output at 25Hz with 100cm resolution and 2-meter accuracy. I also need IMU data at that same rate. I know the T4.1 can collect and log the data at the needed rate: 25 * 64 ,or 1600 bytes per second in a stream of binary packets collected into a buffer of about 8KB for efficient SD card writes.
After a few days studying the Sparkfun library and the ZED setup commands and UBX binary packet specifications, I put together a simple logger in a few more days. After a few days of tests, I started seeing some puzzling results.
1. The 1mAh rechargeable backup battery on the ZED module won't backup the GNN for hot starts for more than a couple of hours from a full charge. The battery charges at about 30uA and discharges at about 50uA. A couple of hours is OK for hot or warm starts, but can't depend on the configurations in the battery-backed-RAM to last overnight if the Sparkfun module is not connected to a 5V supply.
2. The UBlox manuals and U-Center software say that you can set the fix update interval to as low as 25 milliseconds, or 40Hz. I wanted 40 milliseconds for 25Hz. However, if you try to configure the ZED-F9R at that rate it doesn't stick. The actual minimum fix interval seems to be 50 milliseconds.
3. You can get position update packets at 25Hz by setting the priority navigation rate to 25Hz. That gives priority to certain NAV message, among them the NAV_PVT messages I am using. These delivered packets are apparently generated from the 20Hz actual computed fixes. I've found no good explanation of exactly how that is done, but I suspect it is the output of an internal Kalman filter working with the GNSS data and the IMU data.
4. If you try to do 20Hz fixes and 25Hz output using all the available satellite constellations, the overworked MPU inside the ZED seems to give up in surprising ways. (And not nice surprises!). I've seen many instances of the ZED either resetting to factory defaults, or losing some or all of the setup information I've sent to the module. This often means the Teensy stops collecting as the baud rate on the ZED UART output has changed and packets can no longer be decoded. This particular problem seems to be eliminated if I restrict the ZED to using just the GPS constellation and SBAS. It's not surprising that it can better cope with just eight satellites in view that with 24 satellites.
5. It may be possible to use more satellite constellations by reducing the fix rate below 20Hz. Perhaps that will reduce the ZED MPU load enough so that it can keep up with the extrapolated 25Hz output.
I'm in the process of writing a 'training wheel' subset of the Sparkfun library to handle just the NAV_PVT and ESF_MEAS packets I need for the logger. As a first step, I wrote a simple UBX packet parser that runs in about 40 lines of code. I then added functions to examine the collected packets and extract the required data from the UBX_NAV_PVT and UBX_ESF_MEAS packets while ignoring any other packets. That took about another hundred lines. I cribbed shamelessly from the Sparkfun library's structure definitions--which added up to another 70 or 80 lines of source. There will be more work to be done if I discover the need to handle other packets. For now, I'm sticking with a minimalist approach.
As a teaser here is a minimalist test program for the UBX packet parser: (note: functions to break out the data are not included.)
Code:
// Simple demo to collect UBX packets. This demo assumes that
// the UBlox ZED-F9R module has been set up to automatically send NAV_PVT
// and ESF_MEAS ppackets and that the ZED UART1 baud rate has been set
// to 230400 Baud. May require that UART1 NMEA packets have been disabled
// for higher data rates.
// MJB 1/8/2023
const char compileTime[] = " Compiled on " __DATE__ " " __TIME__;
#define gpsPort Serial1
// Define the three status results from the parser
#define STATEMPTY 0
#define STATFILLED 1
#define STATERROR 2
#define PAYLOADMAX 248
typedef struct __attribute__((aligned(4))) {
uint32_t status;
uint8_t pktclass;
uint8_t pktid;
uint8_t lenlow; // payload length low byte
uint8_t lenhi; // payload length low byte
uint8_t payload[PAYLOADMAX]; // total is 256 bytes
} ubxPacket_t;
ubxPacket_t myPacket;
elapsedMillis showDataTimer;
elapsedMicros parseTimer;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
// while (!Serial); //Wait for user to open terminal
delay(1000);
Serial.printf("\n\nUBlox Parser Test %s \n", compileTime);
delay(100);
showDataTimer = 0;
parseTimer = 0;
gpsPort.begin(230400); // Open GPS hardware serial port
myPacket.status = STATEMPTY; // initialize packet status
}
// Loop calls the parser every 300uSec the shows statistics on
// packets collected
void loop() {
if(parseTimer > 300){
parseTimer = 0;
ParsePacketInput();
}
if (showDataTimer > 1000) {
ShowStats();
showDataTimer = 0;
}
// handle the packet if it is complete or there was an error
if(myPacket.status != STATEMPTY) HandlePacket();
}
// constants that identify packets received
const uint8_t UBX_CLASS_NAV = 0x01;
const uint8_t UBX_CLASS_ESF = 0x10;
const uint8_t UBX_NAV_PVT = 0x07;
const uint8_t UBX_ESF_MEAS = 0x02;
uint32_t meascount, pvtcount, errcount; // for statistics output
uint16_t pldlength;
// Check parser packet status and react to filled packets
void HandlePacket(void) {
if (myPacket.status == STATERROR) {
errcount++;
pldlength = myPacket.lenlow + (myPacket.lenhi << 8);
myPacket.status = STATEMPTY; // continue after error
} else if (myPacket.status == STATFILLED) {
if (myPacket.pktclass == UBX_CLASS_NAV) {
if (myPacket.pktid == UBX_NAV_PVT){
pvtcount++;
//copyPVTdata(&myPacket); This is where you would save or display the data
}
}
if (myPacket.pktclass == UBX_CLASS_ESF) {
if (myPacket.pktid == UBX_ESF_MEAS){
meascount++;
//copyESFdata(&myPacket); This is where you would save or display the data
}
}
myPacket.status = STATEMPTY; // mark as empty to resume parsing
} // end of if(myPacket.status == STATFILLED)
}
/*************************************************************
Packet Input and checksum
*****************************************************************/
volatile uint8_t ck_a, ck_b; // accumulated during reception
volatile uint8_t cksumA, cksumB; // read after end of payload
inline void add2Cksum() __attribute__((always_inline));
void add2Cksum(uint8_t pbt) {
ck_a += pbt;
ck_b += ck_a;
}
bool goodCksum(uint8_t pbA, uint8_t pbB) {
return ((pbA == ck_a) && (pbB == ck_b));
}
void initCksum(void) {
ck_a = 0;
ck_b = 0;
}
// UBX Packet Parser Called from loop. Reads from serial gpsPort
// reads while serial data available or until packet is complete.
void ParsePacketInput(void) {
ubxPacket_t *pptr;
pptr = &myPacket; // looking forward to using a queue of packets
uint8_t pbyte;
// variables retained between calls for state machine
static uint16_t pldcount;
static uint16_t pldsize;
static uint16_t pstate;
while (gpsPort.available() && (myPacket.status == STATEMPTY)) {
pbyte = gpsPort.read();
switch (pstate) {
case 0: // check for 0xB5 first preamble byte
if (pbyte == 0xB5) pstate = 1;
break;
case 1: // check for 0x62 second preamble byte
if (pbyte == 0x62) pstate = 2;
else pstate = 0; // back to start if not proper preamble
break;
case 2: // get pktclass
initCksum(); // reset checksum
add2Cksum(pbyte); // add the first byte to checksum
pptr->pktclass = pbyte; // save packet class
pstate = 3;
break;
case 3: // get pktid
add2Cksum(pbyte);
pptr->pktid = pbyte;
pstate = 4;
break;
case 4: // get len low byte
pptr->lenlow = pbyte;
pstate = 5;
add2Cksum(pbyte);
pldsize = pbyte; // save incoming payload length for state 7
break;
case 5: // get lenhi
pptr->lenhi = pbyte;
pstate = 6;
pldcount = 0;
add2Cksum(pbyte);
pldsize += ((uint16_t)pbyte << 8);
break;
case 6: // get payload until pldcount >= pldsize;
pptr->payload[pldcount] = pbyte;
add2Cksum(pbyte);
pldcount++;
if (pldcount >= pldsize) pstate = 7; // next state when payload full
if (pldcount > PAYLOADMAX ){ // exit with error before buffer overrun
pptr->status = STATERROR;
}
break;
case 7: // read first cksum byte
cksumA = pbyte;
pstate = 8;
break;
case 8: // read 2nd cksum byte
cksumB = pbyte;
if (goodCksum(cksumA, cksumB)) {
pptr->status = STATFILLED;
} else {
pptr->status = STATERROR;
}
pstate = 0; // reset state for next packet
break;
} // end of switch(pstate)
} // end of while (gpsPort->available())
}
// Show the number of packets received of each type, including those with checksum errors
void ShowStats(void) {
Serial.printf("PVT:%6lu MEAS:%6lu Error:%6lu \n", pvtcount, meascount, errcount);
}
