Usage uNavINS with another GPS module

[brightproject]

Good day @flybrianfly
Thank you so much for your work!
I would like to use the uNavINS library code, but I see that for it to work you need a NEO GPS module with the uBlox protocol.
I can get it more accurately, but the TOW - Gnss.gps_tow_s() parameter confuses me.
https://github.com/FlyTheThings/uNavINS/blob/ed2d0c56a9099f66a5fe93d4d87da317f7f61b0f/uNavINS.h#L89C31-L89C34
I think that I can get other parameters using the NMEA protocol.

Gnss.north_vel_mps(),
Gnss.east_vel_mps(),
Gnss.down_vel_mps(),

As far as I know, this is some specific time format of uBlox modules.
Could you help with advice for the NMEA protocol?
I plan to use GPS module GN02D.

[flybrianfly]

Unfortunately, I have limited familiarity with NMEA. Most of my work is with uBlox receivers over the UBX protocol, currently I use the SAM-M8 and ZED-F9P most frequently.

[brightproject]

Unfortunately, I have limited familiarity with NMEA. Most of my work is with uBlox receivers over the UBX protocol, currently I use the SAM-M8 and ZED-F9P most frequently.

Ughh what a pity...
In general, don’t remember why they used the time of the week?
Not seconds, not hours, but TOW?

[flybrianfly]

For TOW, that's doing two things:

1. Seeing that the GPS data was updated for running the measurement update
2. Setting the time period for integrating the accels and gyros

TOW is millisecond accuracy. More current versions of the EKF use a constant for setting the time period for integration and leave it up to the programmer to appropriately call the measurement update.

[brightproject]

1. Seeing that the GPS data was updated for running the measurement update

What type of number does this function Gnss.gps_tow_s() return?
Unfortunately, I don’t have a module on the UBX protocol, so using the comparison method, I’ll try to substitute the time from the module on the NMEA protocol.
Thanx!

[flybrianfly]

It's a double in units of seconds.

[brightproject]

It's a double in units of seconds.

Thank you very much for your answers.

[brightproject]

It's a double in units of seconds.

Sorry again @flybrianfly.
I'm still trying to understand the meaning of substituting the time of the week, especially in double precision and understand that this time is simply given out by all uBlox receivers and this time has been used since the invention of satellite navigation(6 january 1980 г.), but how can I get it out of a receiver that only gives out the NMEA form - that’s the question.
After all, the UBX protocol does not add anything to the GPS signal, i.e. doesn't take data out of nothing.
Accordingly, everything that can be obtained from a device using the UBH protocol, theoretically and practically, can also be obtained using the NMEA protocol?
I decided to go by comparing the UBX and NMEA protocols and found this doc about UBX protocol

From my receiver GN02D - receive time in the format $GPRMC,154553.000,,,,,061223 and after parsing by TinyGPSPlus library 12/06/2023 15:45:53:0000
Unfortunately my module does not output centiseconds or milliseconds, but I don't think this would be a problem for getting time in week format or leap second.
But if over time it’s still possible to figure it out somehow, I think I’ll just try using the filter substitution method
https://github.com/FlyTheThings/uNavINS/blob/ed2d0c56a9099f66a5fe93d4d87da317f7f61b0f/examples/uNavINS-with-MPU9250/uNavINS_MPU9250/uNavINS_MPU9250.ino#L77
I will change the format in seconds or something else.
It is difficult to determine the values

Gnss.north_vel_mps(),
Gnss.east_vel_mps(),
Gnss.down_vel_mps(),

scares me more.
As far as I understand, receiving data from uBlox receivers using the UBX protocol allows you to obtain data on speeds and their vectors?

I understood from your answer that you did not work at NMEA and it is difficult to delve into the intricacies and features of another protocol.
The NMEA protocol has a standard VTG message

and I found the same one in the UBX protocol.

Do you think it is possible to replace the values of the velocity vectors obtained from the VTG message?
Or still the values obtained from:

Gnss.north_vel_mps(),
Gnss.east_vel_mps(),
Gnss.down_vel_mps(),

have specific data?
I will be immensely grateful to you for tips on how to substitute data from the receiver GN02D into the filter.
I'm interested in adapting the code for other GNSS modules.
Please tell me, perhaps the topic and discussion of the project
https://forum.pjrc.com/index.php?threads/unav-ins.48856/
moved to another location?
I am very interested in this topic and your work in developing a system for determining position in the air, but there have been no discussions on the topic for about one years.

[flybrianfly]

Regarding time, you can just supply your own time source (i.e. from the microcontoller). Really, it just needs to know that the GPS data was updated and what the time interval is for integrating the gyro and accel measurements during the TimeUpdate portion of the EKF.

The velocity will be tough if you don't have vertical velocity available. You can try using just the north and east velocity to see how that does, but without sitting down to re-work the EKF implementation, I'm not sure how good the results would be. I suspect probably not good.

The current library is located here:
https://github.com/bolderflight/navigation

There's some helper functions along with the 15 state EKF. The EKF is not well documented, but the basic use is to call:
void Initialize(const Eigen::Vector3f &accel, const Eigen::Vector3f &gyro,
const Eigen::Vector3f &mag, const Eigen::Vector3f &ned_vel,
const Eigen::Vector3d &lla)

With vectors of accel, gyro, mag, NED velocity, and lat / lon / alt data when you have a good GNSS solution and new data for everything. This initializes the EKF and only should be called once.

The time update is called with:

void TimeUpdate(const Eigen::Vector3f &accel, const Eigen::Vector3f &gyro,
const float dt_s)

This should be run whenever you have new accel and gyro data. Note that it takes a constant time delta (in seconds) for the integration.

The measurement update is called with:

void MeasurementUpdate(const Eigen::Vector3f &ned_vel,
const Eigen::Vector3d &lla)

When you have new GNSS based NED velocity and lat / lon / alt data.

[brightproject]

Regarding time, you can just supply your own time source (i.e. from the microcontoller).

Thank you @flybrianfly, your answers clarify a lot for me!
I decided to put the GN02D module aside for a while and take a rather old module NEO-6M, through the program u-center I configured the output of the UBX protocol, and set the output frequency to 10 Hz.
I also receive message packages in the binary console.

But when I connect the output from the module NEO-6M to the serial port, I get garbage, which is probably logical, because UBX protocol is probably a binary format.

Or, even in the serial port, should I see readable UBX protocol lines from the module NEO-6M, like NMEA protocol?
I also see in the program u-center the time of week iTOW and speed VELNED.

But either the UBX protocol issued by the module is NEO-6M too outdated and the library does not understand it, or I’m doing something wrong.
It also seems to me that due to a dead battery on the module, after de-energizing the module NEO-6M and re-energizing it, the baud rate, which I set for the module at 38400, is lost.
Although the battery on the module NEO-6M is generally intended to store the satellite almanac, and is in no way connected with recording to FLASH memory.

But even with a speed of 9600, the data does not want to be parsed by bfs::Ubx gnss(&Serial2);.
I also noticed that the HardwareSerial is used by the library itself, so I tried to initialize the port differently, but the result was the same - the library does not produce anything and does not parse the UBX package.
Code from example, modified for stm32f411 with core and uses HardwareSerial

#include "ubx.h"

#define SERIAL_BAUD 115200
#define GNSS_BAUD 9600

HardwareSerial Serial2(USART2); // PA2(TX2), PA3(RX2)
// HardwareSerial Serial2(PA2, PA3);

/* Ublox object, GNSS on Serial3 */
bfs::Ubx gnss(&Serial2);

void setup() {
  /* Serial to display data */
  Serial.begin(SERIAL_BAUD);
  while(!Serial) {}
  gnss.Begin(GNSS_BAUD);
  Serial.print("Serial port speed: ");
  Serial.println(SERIAL_BAUD);
}

void loop() {

// forwarding serial2 to serial
// if (Serial2.available() > 0)
//   Serial.write(Serial2.read());

  if(gnss.Read()) {
    Serial.print(gnss.fix());
    Serial.print("\t");
    Serial.print(gnss.num_sv());
    Serial.print("\t");
    Serial.print(gnss.lat_deg(), 6);
    Serial.print("\t");
    Serial.print(gnss.lon_deg(), 6);
    Serial.print("\t");
    Serial.print(gnss.alt_wgs84_m(), 2);
    Serial.print("\n");
  }
}

Based on the time, I realized that I will most likely use the microcontroller time.
As for the VELNED speeds, I think the vertical speed should be calculated using the data from the barometric sensor.
In general, the specificity of data from GPS is that it is out of date by at least 1 second.
That is, all these speeds towards the cardinal points and downwards are all “past”, and you cannot completely trust them, just rely on them.

[flybrianfly]

UBX looks like garbage over the serial port, it's binary, not human readable. Try an old version of this library for parsing the packet. At some point uBlox made a breaking change to the UBX-NAV-PVT packet, which this library uses. You could also try SparkFun's ublox library:
https://github.com/sparkfun/SparkFun_Ublox_Arduino_Library

Their library works pretty well - it just wasn't available when I made this one.

[brightproject]

Their library works pretty well - it just wasn't available when I made this one.

Unfortunately, the Sparkfan could not be made to work.
Of the abundance of examples, only one is suitable for my connection method and this is UART.
The library somehow sends data to the module and blocks any transmission.
Then you need to connect the module directly to the UART and reset it by press DEBUG message button only.
Apparently modern GPS receivers are connected only via the i2c bus, which is certainly convenient and faster in terms of speed.
The code connects and produces empty values.

SparkFun Ublox Example
GPS: trying 9600 baud
GPS serial connected
Lat: 0 Long: 0 (degrees * 10^-7) Alt: 0 (mm) SIV: 0 velD: 0
Lat: 0 Long: 0 (degrees * 10^-7) Alt: 0 (mm) SIV: 0 velD: 0
Lat: 0 Long: 0 (degrees * 10^-7) Alt: 0 (mm) SIV: 0 velD: 0

I played with speed, but all in vain ... probably need to configure the NEO-6M module in some clever way before starting to work with the module with the code.
I managed to find a simple code to parse only specific messages.
Before working with the code, I configured the NEO-6M module to output data at 10 Hz.
The main difficulty is saving the settings, because they are saved very unstable.
It’s as if this is done every other time, and even then not always.
Therefore, I save several times and check after turning off the power.
The parser code is written according to the datasheet.

The code works surprisingly quickly, parsing every 100 milliseconds, according to iTOW time.

iTOW: 45677000 velN: -0.16 velE: 0.10 velD: 0.16 Spd (3D): 0.25 GndSpd (2D): 0.19 Hdg (2D): 136.58 Spd Acc: 0.53 Crs/Hdg Acc: 43.10
iTOW: 45677100 velN: -0.15 velE: 0.11 velD: 0.16 Spd (3D): 0.25 GndSpd (2D): 0.19 Hdg (2D): 136.58 Spd Acc: 0.54 Crs/Hdg Acc: 43.11
iTOW: 45677400 velN: -0.12 velE: 0.11 velD: 0.15 Spd (3D): 0.22 GndSpd (2D): 0.17 Hdg (2D): 136.58 Spd Acc: 0.54 Crs/Hdg Acc: 43.12
iTOW: 45677500 velN: -0.08 velE: 0.14 velD: 0.15 Spd (3D): 0.21 GndSpd (2D): 0.16 Hdg (2D): 136.58 Spd Acc: 0.52 Crs/Hdg Acc: 43.13

I'll leave the code here, maybe it will be useful for someone.

#define SERIAL_BAUD 115200
#define GNSS_BAUD 9600

HardwareSerial Serial2(USART2); // PA2(TX2), PA3(RX2)

const unsigned char UBX_HEADER[] = {0xB5, 0x62};

struct NAV_VELNED {
  unsigned char cls;
  unsigned char id;
  unsigned short len;
  unsigned long iTOW; // U4
  long velN; // I4
  long velE;
  long velD;
  unsigned long speed;
  unsigned long gSpeed;
  long heading;
  unsigned long sAcc;
  unsigned long cAcc;
};

NAV_VELNED velned;

void calcChecksum(unsigned char* CK) {
  memset(CK, 0, 2);
  for (int i = 0; i < (int)sizeof(NAV_VELNED); i++) {
    CK[0] += ((unsigned char*)(&velned))[i];
    CK[1] += CK[0];
  }
}

bool processGPS() {
  static int fpos = 0;
  static unsigned char checksum[2];
  const int payloadSize = sizeof(NAV_VELNED);

  while ( Serial2.available() ) {
    byte c = Serial2.read();
    if ( fpos < 2 ) {
      if ( c == UBX_HEADER[fpos] )
        fpos++;
      else
        fpos = 0;
    }
    else {
      if ( (fpos-2) < payloadSize )
        ((unsigned char*)(&velned))[fpos-2] = c;

      fpos++;

      if ( fpos == (payloadSize+2) ) {
        calcChecksum(checksum);
      }
      else if ( fpos == (payloadSize+3) ) {
        if ( c != checksum[0] )
          fpos = 0;
      }
      else if ( fpos == (payloadSize+4) ) {
        fpos = 0;
        if ( c == checksum[1] ) {
          return true;
        }
      }
      else if ( fpos > (payloadSize+4) ) {
        fpos = 0;
      }
    }
  }
  return false;
}

void setup() 
{
  Serial.begin(SERIAL_BAUD);
  Serial2.begin(GNSS_BAUD);
}

void loop() {
  if ( processGPS() ) {
    Serial.print("iTOW: "); Serial.print(velned.iTOW); // GPS Millisecond Time of Week
    Serial.print(" velN: "); Serial.print(velned.velN * 1e-2f); // m/s
    Serial.print(" velE: "); Serial.print(velned.velE * 1e-2f); // m/s
    Serial.print(" velD: "); Serial.print(velned.velD * 1e-2f); // m/s
    Serial.print(" Spd (3D): "); Serial.print(velned.speed * 1e-2f); // m/s
    Serial.print(" GndSpd (2D): "); Serial.print(velned.gSpeed * 1e-2f); // m/s
    Serial.print(" Hdg (2D): "); Serial.print(velned.heading * 1e-5f); // same that divide on 100000.0f
    Serial.print(" Spd Acc: "); Serial.print(velned.sAcc * 1e-2f);
    Serial.print(" Crs/Hdg Acc: "); Serial.print(velned.cAcc/100000.0f);
    Serial.println();
  }
}

Next, I need to start obtaining similar data using the NMEA protocol, because UBX is still very redundant, complex and applicable only to GPS receivers from u-blox.
@flybrianfly Please tell me what is the update frequency of these values for transmission to the filter?

Gnss.gps_tow_s(),
Gnss.north_vel_mps(),
Gnss.east_vel_mps(),
Gnss.down_vel_mps(),
Gnss.lat_rad(),Gnss.lon_rad(),
Gnss.alt_msl_m(),

Once per second or more often?

[flybrianfly]

Any rate between 1 Hz and 10 Hz should be fine.

[brightproject]

Any rate between 1 Hz and 10 Hz should be fine.

Unfortunately, I still can’t get stable data output from the module.
I use a self-written parser, but it does not work if there is another program in an endless loop, such as Filter.update()
Therefore, I try to run the uBlox library, but I encounter a compilation problem.
First I tried to compile
Bolder Flight Systems UBLOX 6.0.6
I get the error elapsedMicros _t; type not specified - in file uNavINS.h
I had to comment out this line.
elapsedMicros _t in uNavINS and elapsedMillis t_ms_ in ubx-5.0.0 are these libraries or what?
As far as this is feature for teensy?
https://www.pjrc.com/teensy/td_timing_elaspedMillis.html
The iNavINS example code was assembled, but due to a problem with my parser, I was unable to receive the data.
So I decided to delve into the library archives
I found version 5.0.0 and tried to run the protocol parser on it

error: 'elapsedMillis' does not name a type       
  159 |   elapsedMillis t_ms_;

#include <ubx-5.0.0.h>
// #include <ubx.h>

#define SERIAL_BAUD 115200
#define GNSS_BAUD 115200

HardwareSerial Serial2(USART2); // PA2(TX2), PA3(RX2)
bfs::Ubx gnss(&Serial2);

void setup() {
   /* Serial to display data */
   Serial.begin(SERIAL_BAUD);
   while(!Serial) {}
   gnss.Begin(GNSS_BAUD);
   // Serial.print("Serial port speed: ");
   // Serial.println(SERIAL_BAUD);
}

void loop() {

   // forwarding serial2 to serial
   // if (Serial2.available() > 0)
   // Serial.write(Serial2.read());
   // Serial2.write(Serial.read());

   delay(20);

   if(gnss.Read()) {
     Serial.print(gnss.fix());
     Serial.print("\t");
     Serial.print(gnss.num_sv());
     Serial.print("\t");
     Serial.print(gnss.lat_deg(), 6);
     Serial.print("\t");
     Serial.print(gnss.lon_deg(), 6);
     Serial.print("\t");
     Serial.print(gnss.alt_wgs84_m(), 2);
     Serial.print("\n");
   }
}

As a result, I received a similar error that I received when building the iNavINS library.
You often said that iNaviNS is outdated and not supported by you, and that you are now using the code
https://github.com/bolderflight/spaaro/blob/main/flight_code/flight/nav.cc
Everything is very confusing and it was difficult for me to understand some things.
Now I think I have the following problems:

1. Library ubx.h version 6.0.6 works with uBlox PVT
   my neo-6m module cannot issue PVT, this function is only available on newer models.
2. Perhaps version of the ubx.h library 5.0.0 will work with my neo-6m module, but the iNavINS code does not work with this library.

A stalemate, the way out of which would probably be to purchase a new uBlox module.
I would like to understand the code partially and compare the versions in order to put it into operation - then I could help you modify the code into a library.
I know you've been wanting to do this for several years now🙂

[flybrianfly]

elapsedMillis is a Teensy library. It's header only and available from here:
https://github.com/PaulStoffregen/cores/blob/7435e4bbe96c0819dc28ea64bfe4f152f92a486e/teensy/elapsedMillis.h

I've seen some other ports of that code as well. Should be easy to incorporate. I'm only using it for timing in the Begin method to say whether communication has been established with the GPS receiver. It's not critical and could be removed.

SPAARO is full-blown UAS flight controller software. The EKF / INS is in this library:
https://github.com/bolderflight/navigation

I only cite SPAARO as showing an example of how to call the EKF.

Right, I require UBX-NAV-PVT as one of my packets. You might need to use some other library or your own parser to get around that requirement by using other packets available from the receiver.

I typically run the GNSS parser in the loop and the INS in an interrupt context tied to the IMU data ready interrupt. You need a large enough buffer for the serial port to handle delays of not being able to continuously parse the GPS data. It works really well, but requires understanding of interrupt context and being able to tune an EKF.

I use the EKF in commercial products and happily implement and tune it for paying customers. But it's difficult to make accessible to the masses without a good autotuner that would take into account different sensors, vehicle dynamics, and operating environments. Ultimately, the naive implementation that I used for uNavINS was not adequate at autotuning, was beyond the complexity of most Arduino libraries, and would be hard to support through open-source (i.e. free) methods. It really would require a robust, automated process to tune the EKF to work effectively.