SPI clock is set per transaction, so it doesn’t matter. We’ll see what else can be there…
Hello Lars!
After looking at your code, I think I can see the potential problem. The lag is probably due to the nature of the decodeSingleMessage() function from our example. This function was designed to wait specifically for the message you pass as a parameter and decode it.
If you look at the source code, it reads u-blox output until it gets a valid message of the specified type, or until it fills half of internal buffer(which is 512 chars). This is probably the source of all the delays, waiting 3 times for the required message to show up is highly inefficient.
Instead, you should probably use decodeMessages() method, that starts an infinite loop waiting for any message to appear and tries to decode them. You only need to make a small modification for this:
https://github.com/egorf/Navio/blob/master/C%2B%2B/Navio/Ublox.cpp#L372
You should change the highlighted line to something like:
msg_id = parser->decodeMessage(position_data);
if (msg_id > 0) {
// message successfully decoded
// use msg_id to understand what type of message this is
// use position_data vector with the msg data
}
As you probably know already, you can specify additional message types in decodeMessage() method.
Please, keep in mind that our examples focus on simplicity over efficiency, they are mostly designed to help understand the way things work in Navio. If you seek better quality code, take a look at the python u-blox parser by Andrew Tridgell, or maybe even APM.
Thank you for your patience. Hope my advice helps
Thanks for the info egorfedorov. I implemented the suggested changes by adding
int msg_id = parser->decodeMessage(position_data);
if (msg_id > 0) {
if (msg_id==258) {
gps_lat=position_data[2]/10000000;
gps_lon=position_data[1]/10000000;
gps_h=position_data[3]/1000;
}
if (msg_id==259) {
gps_stat=(int)position_data[0];
}
if (msg_id==274) {
gps_N=position_data[1];
gps_E=position_data[2];
gps_D=position_data[3];
gps_3D=position_data[4];
gps_2D=position_data[5];
gps_crs=position_data[6]/100000;
}
Unfortunately the lag has increased from 1.9 to 2.8 seconds. I looked at both the python parser and the APM implementation and both look incredibly complex … at least to me.
Does the GPS receiver have a buffer on board and is it possible to flush it?
Since the lag is constant with time, it appears that the Ublox functions are reading the data fast enough, its just not reading the newest data. I would think flushing the buffer would force the functions to read the newest data first. I looked through the Ublox data sheet and found a message for the receiver buffer status monitor (0x0A 0x07) and tried to implement it but it keeps giving back zeros … probably something I’m doing wrong.
This actually surprises me. Would you mind pasting all of your source code? Maybe on GitHub?
I was surprised as well. I put the modified ublox.cpp file and the main file up on github.
Well, there’s a lot of things to consider. First and foremost, you need to protect spidev and i2cdev interfaces from simultaneous writing and reading. For example the IMU unit and GPS unit use the same spidev interface and your threads talk to these units at the same time without separation. This produces errors and unexpected behavior. I can suggest a couple of modifications:
- Modify gps code to read a big buffer of data(say, 1024 bytes) and then cycle through the array from memory
- Put mutex blocks before talking to spi, like it’s described here
I still strongly suggest referring to APM code, as even though it may not look pretty, it was field-proven and delivers great efficiency.
Having some issues adding the
pthread_mutex_lock(&_mutex);
to the SPI/I2C read/writes since they all occur within classes. How do you pipe the mutex’s into classes? I found some examples in google but can not get any of them to work. I successfully add the locks and unlocks in the main program before and after the functions that do have SPI/I2C calls in them instead of within the function/class itself. Works for everything except the GPS since it has a infinite loop inside the class. Also from printing output to the screen I will occasionally get this…
Failed to write reg: Input/output error
Failed to write device(-1): Input/output error
which I’m assuming is related to one of the SPI (or I2C) devices not being able to access the bus since its locked by another process. Is this bad?
One final note, in the Ublox data sheet it talks about a TX ready indicator that can be enabled. Would it be possible to read the buffer only when the tx ready indicator is set? I’m thinking one way to get rid of the lag would be to initially read all of the data until the indictor goes low and then only read when its high and decode that data.
I set the decodeMessages() function to print every byte from the GPS to the screen and not worry about decoding and found that the GPS output is as follows:
Status message
Position message
Velocity message
227 bytes of non UBX conforming data
Status message
Position message
Velocity message
227 bytes of non UBX conforming data
… and repeat. The data sheet states that if the receiver has no more data to send all bytes transmitted decode to 0xFF. Searching through the data stream, I don’t see any 0xFF bytes outside of the messages.
Any idea on what the 227 bytes of seemingly random data are? They are always equal in length and contain identical data except for a few bytes here and there.
You may connect ublox to u-center software to see what kind of messages come through
Managed to hunt down a windows machine and get Navio connected to U-center. Output from the packet console is as follows:
16:24:04 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
16:24:04 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
16:24:04 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
16:24:04 R -> UBX 0C-31, Size 644, 'Unknown'
16:24:04 R -> UBX 0C-10, Size 652, 'Unknown'
16:24:04 R -> UBX 03-09, Size 128, 'Unknown'
16:24:04 R -> UBX 03-0A, Size 468, 'Unknown'
16:24:04 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
16:24:04 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
16:24:04 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
16:24:04 R -> UBX 0C-31, Size 644, 'Unknown'
16:24:04 R -> UBX 0C-10, Size 652, 'Unknown'
16:24:04 R -> UBX 03-09, Size 128, 'Unknown'
16:24:04 R -> UBX 03-0A, Size 468, 'Unknown'
16:24:04 R -> UBX NAV-STATUS, Size 24, 'Navigation Status'
16:24:04 R -> UBX NAV-POSLLH, Size 36, 'Geodetic Position'
16:24:04 R -> UBX NAV-VELNED, Size 44, 'Velocity in WGS 84'
16:24:04 R -> UBX 0C-31, Size 644, 'Unknown'
16:24:04 R -> UBX 03-09, Size 128, 'Unknown'
16:24:04 R -> UBX 03-0A, Size 404, 'Unknown'
Looked up the unknown message class/ID and couldn’t find anything in the data sheet and a google search turned up a few other posts from people with the same question. The size of the unknown messages also doesn’t match up to what I was seeing from the Ublox library output. Don’t know if U-center see some things that SPI doesn’t.
It appears the GPS lag has finally been solved and I think it was caused by the default NMEA messages. According to the data sheet, 6 NMEA messages are enabled (pg. 191) at startup (confirmed by U-center):
GGA
GLL
GSA
GSV
RMC
VTG
Based on the U-center output, these messages total 1.8 to 1.9KB (depending on the number of satellites visible) which if you’re reading one byte at a time, as does the Ublox library, this can take a while. After disabling the NMEA messages and conducting several tests, there appears to be no lag.
Hi Lars,
I would be interested in how you disabled NMEA messages. I guess one can do it from the u-center, but is it possible to do it programmatically ?
Thanks
Disabling the NMEA messages is done the same way as enabling the additional messages as shown in the example file.
int Ublox::disableNMEA_GLL()
{
unsigned char gps_nmea_gll[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x2B};
int gps_nmea_gll_length = (sizeof(gps_nmea_gll)/sizeof(*gps_nmea_gll));
unsigned char from_gps_data_nmea[gps_nmea_gll_length];
return SPIdev::transfer(spi_device_name.c_str(), gps_nmea_gll, from_gps_data_nmea, gps_nmea_gll_length, 5000000);
}
int Ublox::disableNMEA_GGA()
{
unsigned char gps_nmea_gga[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x24};
int gps_nmea_gga_length = (sizeof(gps_nmea_gga)/sizeof(*gps_nmea_gga));
unsigned char from_gps_data_nmea[gps_nmea_gga_length];
return SPIdev::transfer(spi_device_name.c_str(), gps_nmea_gga, from_gps_data_nmea, gps_nmea_gga_length, 5000000);
}
int Ublox::disableNMEA_GSA()
{
unsigned char gps_nmea_gsa[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x32};
int gps_nmea_gsa_length = (sizeof(gps_nmea_gsa)/sizeof(*gps_nmea_gsa));
unsigned char from_gps_data_nmea[gps_nmea_gsa_length];
return SPIdev::transfer(spi_device_name.c_str(), gps_nmea_gsa, from_gps_data_nmea, gps_nmea_gsa_length, 5000000);
}
int Ublox::disableNMEA_GSV()
{
unsigned char gps_nmea_gsv[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x39};
int gps_nmea_gsv_length = (sizeof(gps_nmea_gsv)/sizeof(*gps_nmea_gsv));
unsigned char from_gps_data_nmea[gps_nmea_gsv_length];
return SPIdev::transfer(spi_device_name.c_str(), gps_nmea_gsv, from_gps_data_nmea, gps_nmea_gsv_length, 5000000);
}
int Ublox::disableNMEA_RMC()
{
unsigned char gps_nmea_rmc[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x04, 0x40};
int gps_nmea_rmc_length = (sizeof(gps_nmea_rmc)/sizeof(*gps_nmea_rmc));
unsigned char from_gps_data_nmea[gps_nmea_rmc_length];
return SPIdev::transfer(spi_device_name.c_str(), gps_nmea_rmc, from_gps_data_nmea, gps_nmea_rmc_length, 5000000);
}
int Ublox::disableNMEA_VTG()
{
unsigned char gps_nmea_vtg[] = {0xb5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x05, 0x47};
int gps_nmea_vtg_length = (sizeof(gps_nmea_vtg)/sizeof(*gps_nmea_vtg));
unsigned char from_gps_data_nmea[gps_nmea_vtg_length];
return SPIdev::transfer(spi_device_name.c_str(), gps_nmea_vtg, from_gps_data_nmea, gps_nmea_vtg_length, 5000000);
}
Then add this inside the ‘testConnection()’ function
// Disable default NMEA messages
if (disableNMEA_GLL()<0)
{
std::cerr << "Error disabling message\n";
}
if (disableNMEA_GGA()<0)
{
std::cerr << "Error disabling message\n";
}
if (disableNMEA_GSA()<0)
{
std::cerr << "Error disabling message\n";
}
if (disableNMEA_GSV()<0)
{
std::cerr << "Error disabling message\n";
}
if (disableNMEA_RMC()<0)
{
std::cerr << "Error disabling message\n";
}
if (disableNMEA_VTG()<0)
{
std::cerr << "Error disabling message\n";;
}Thanks for the quick answer.
In the meantime I also found a way of doing it, which is the same as you mentioned except I use a 3-byte message. For instance for the GLL messages:
dis_gll[] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11}
I am wondering what is the difference with your way of doing it. In the Ublox documentation it says for the ID 0x06 0x01 and an 8-byte message “Set Message Rate(s)” (“Set/Get message rate configuration (s) to/from the receiver” in the explanation box of CFG-MSG) and for a 3-byte message “Set Message Rate” (“Set message rate configuration for the current target”), but I don’t really get what it means in this case.
The 3-byte version of the message you gave is also valid. I copied my messages directly from U-center, where you set on which ports the message is active/not active and the rate. The only real difference I can see between the messages is that the 8-byte version allows the user to turn on or off the messages (and set rate) on any of the I/O ports versus just the current one in the 3-byte version. The ‘rate’ setting is how often the message is sent based on the rate of the navigation solution. Setting it to one means every time the GPS calculations a solution, a message is also sent. If you set it to two, a message is sent on every other solution calculation, and so on. Changing the rate of the GPS solution calculation is done using CFG-RATE.
I chose to use the 8-byte version of the message to ensure that the NMEA messages were completely turned off. Since the total size of the NMEA messages is quite large, I wanted to make sure they weren’t taking up any processing power anywhere.
1 Like