Copter 3.5 rc8 - Injection of UBlox RTK corrections don't appear at Reach GPS, but RTCM3 do appear

I am using the Reach RTK GPS connected to my Pixhawk as a secondary GPS. (Its not used for navigation just for reference.) With Copter 3.4, I could send the UBlox formatted corrections to the Reach via the MAVLINK telemetry from the base station and they were routed correctly to the Reach. With Copter 3.5 RC8, the corrections don’t seem to be routed correctly since the Copter’s Reach never registers that the corrections arrived. It did work as expected with RTCM3 formatted messages in both 3.4 and 3.5. I’d like to continue to use the UBlox formatted messages since they provide quicker RTK fixes and contain other useful information for the RTK algorithms.

I know there was a lot of work done on the GPS code for Copter 3.5. Were there any changes that would’ve affected the routing of the UBlox formatted messages? (They get encapsulated in the MAVLINK msg_gps_rtcm_data_dynamic messsage.) One difference in the UBlox vs. RTCM3 messages is that the UBlox messages are larger and have to be broken up into multiple MAVLINK messages.

It appears that smaller UBlox messages get forwarded to the Copter’s RTK GPS, but once many more satellites start getting tracked, the messages get larger and do not get forwarded to the GPS. It is likely due to logic within the firmware related to combining multiple message fragments into one to forward to the GPS. Like I mentioned, this appears to be a regression in the 3.5 RC8 firmware as compared to 3.4.6 since the only thing I changed was the firmware version on the copter.

RTCM3 formatted messages work fine with even 13 satellites or more with both 3.5 and 3.4.6.

Any ideas @WickedShell?

How are you sending the corrections? You mention msg_gps_rtcm_data_dynamic however that isn’t a standard message in common so I’m unsure how you are sending it.

3.5 does use http://mavlink.org/messages/common#GPS_RTCM_DATA however you do have to implement the fragmenting rules described there which Mission Planner and MAVProxy both do, but I’m really lost on what message you are trying to use for this.

@WickedShell, I’m using the gps_rtcm_data message. (The dynamic version was an extended class to make the array size variable based on the message size. The original generated class forced the payload to 180 even if all 180 bytes weren’t used. I’m doing this in Java BTW.)

I’ll review the fragmenting rules to ensure that I’m doing it correctly. My implementation works for 3.4.6, but I didn’t see much different in the https://github.com/ArduPilot/ardupilot/blame/master/libraries/AP_GPS/AP_GPS.cpp#L957 that joins the fragments together so I’m rechecking my implementation. It didn’t work in Mission Planner either, but I’m not sure if Mission Planner forwards the UBlox formatted messages as opposed to the RTCM3 messages. (I didn’t test MP with 3.4.6 and UBlox messages.)

@WickedShell, I noticed that messages that were 248 bytes transfer fine, but 280 bytes or greater don’t. The RTCM3 messages never exceed 180 bytes so the fragmentation logic isn’t stressed with those.

@WickedShell, I added debug statements to the AP_GPS code and it looks like the size of the buffer being sent here is correct for the larger UBlox buffers:

@WickedShell Why is the length of the buffer in this function an unsigned byte? Shouldn’t it be an unsigned short?

That may explain why buffers bigger than 255 are not being sent correctly to the GPS driver.

@WickedShell and @rmackay9 The following changes fixed the bug:

libraries/AP_GPS/AP_GPS.cpp | 8 ++++----
 libraries/AP_GPS/AP_GPS.h   | 4 ++--
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 1efb274..a053ba6 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -812,7 +812,7 @@ void AP_GPS::lock_port(uint8_t instance, bool lock)
 }
 
 // Inject a packet of raw binary to a GPS
-void AP_GPS::inject_data(uint8_t *data, uint8_t len)
+void AP_GPS::inject_data(uint8_t *data, uint16_t len)
 {
     //Support broadcasting to all GPSes.
     if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
@@ -824,7 +824,7 @@ void AP_GPS::inject_data(uint8_t *data, uint8_t len)
     }
 }
 
-void AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint8_t len)
+void AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint16_t len)
 {
     if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) {
         drivers[instance]->inject_data(data, len);
@@ -954,7 +954,7 @@ bool AP_GPS::blend_health_check() const
 void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t *msg)
 {
     mavlink_gps_rtcm_data_t packet;
-    mavlink_msg_gps_rtcm_data_decode(msg, &packet);
+    mavlink_msg_gps_rtcm_data_decode(msg, &packet);
 
     if (packet.len > MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN) {
         // invalid packet
@@ -990,7 +990,7 @@ void AP_GPS::handle_gps_rtcm_data(const mavlink_message_t *msg)
 
     // add this fragment
     rtcm_buffer->sequence = sequence;
-    rtcm_buffer->fragments_received |= (1U << fragment);
+    rtcm_buffer->fragments_received |= (1U << fragment);
 
     // copy the data
     memcpy(&rtcm_buffer->buffer[MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN*(uint16_t)fragment], packet.data, packet.len);
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index da63a9b..6903a22 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -323,8 +323,8 @@ public:
     void lock_port(uint8_t instance, bool locked);
 
     //Inject a packet of raw binary to a GPS
-    void inject_data(uint8_t *data, uint8_t len);
-    void inject_data(uint8_t instance, uint8_t *data, uint8_t len);
+    void inject_data(uint8_t *data, uint16_t len);
+    void inject_data(uint8_t instance, uint8_t *data, uint16_t len);
 
     //MAVLink Status Sending
     void send_mavlink_gps_raw(mavlink_channel_t chan);

Very good catch!!!
I will try this tomorrow. Did you create a pull request for this ?

@rrr6399 Nice catch, thanks! Do you want to do a PR for it?

@amilcarlucas Great you here, this is your fault :stuck_out_tongue: Small things can really be missed even when looked at by 3 people; here is the PR that introduced this bug: https://github.com/ArduPilot/ardupilot/pull/6154 I’m beating myself already :joy:

@OXINARF, Do I need to create a fork in my repository to create a branch for the pull request? (I haven’t done this on github before.)

Also, it looks like the 4 record limit on fragments can be too low when you include GPS, GLONASS, SBAS and Galileo satellites. I have to filter or remove constellations to get under the 4 record limit.

What messages are you trying to use? The buffer was setup to allow transporting RTCM data correctly, and there just aren’t enough bits in the flags field to allow more then 4 fragments. (Blame the fact that the message was created without any thought on how to handle fragmenting, and the fragmentation protocol was developed by ArduPilot to attempt to cope with that).

UBlox Class=2 ID=21 (0x15) messages for the RTK base corrections.
30.19.4.2 Multi-GNSS Raw Measurement Data

@rrr6399 you can fit up to 22 measurements in with that message, the RTCM stuff was designed to handle the probable range of RTCM messages, so if you can move to those instead you should be fine, but anything else it could be to large.

You could fall back to just the old injection method which doesn’t have fragmenting or reassembling, but you end up racing against the GPS driver or any data loss so that won’t be a reliable solution in that case…

I was getting faster and more reliable fixes with the UBlox messages. There have been some fixes (like higher resolution timestamps and such) with the RTCM3 messages so its possible I may get decent results now. It would be nice in the future to accommodate the size of the UBlox messages in the future for all constellations since it provides more information for the RTK algorithms and removes a translation step.

The down side is RTCM is what the industry uses and has standardized upon, and you can’t keep increasing the message size for corrections. IE RTCM already supports L1/L2 multiple constellations quite nicely, and does inject through the current path.

I’m not set against a larger message per say, but it means larger buffers on the autopilot, and the message is more and more likely to be lost in transport over the radios most commonly used. I suspect looking at the proven solutions that can already handle all these input types is better long term then just trying to handle everything in a single message.

I’ll test it out and see. I’d rather use RTCM3 since there is less custom configuration work and the bandwidth is less, but in the past I had a very difficult time getting fixes with RTCM3 messages on the Reach. That was back in January though so the landscape may be different now. (UBlox had the doppler shift info and didn’t truncate measurements as much. RTKExplorer recommended the UBlox messages and they did work better.)

I’m reforking the ardupilot repository and will submit a pull request.

Nice, thank you. I will try that tomorrow.

Here’s the pull request: