I’m using an old pixhawk 2.4.8 to test an ardusimple simplertk2b heading kit and I’ve been having problem that I’ve had no success trying to work out, and i was hoping someone could offer some advice?
I have the yaw data present in GPS_RAW_2 however the pixhawk is making no use of it, just the position data.
Presumably I have a setting wrong somewhere, but I’ve had no success trying to work out what or where I’ve gone wrong.
I’ve attached some images and the param file, hopefully it helps illustrate whats wrong.
I’m reasonably confident I’ve ruled out the basics (not that that’s a guarantee of anything):
• the pixhawk was updated and had all its parameters reset prior to starting
• both GPS modules spit back their respective config file after a power cycle (perfect match to that uploaded), so I’m reasonably confident they’re configured correctly.
• The GPS is the only peripheral connected, The wiring is very basic, and I’m getting data so I’d assuming its not an electrical issue.
• Mission planner and u-center both report ‘RTK fixed’ and the position is stable so I should have good signal.
• The antenna spacing is 848mm which u-center is reporting accurately, well with the 20% tolerance of that set in ardupilot.
• the gps and ardupilot are on a flat level surface (no specified tolerance for that but within a few degrees)
thanks for taking the time to read this. hopefully its a quick fix.
I’ve encountered this issue on firmware after 4.3. After 4.3, the ublox driver added checks to assume that you are connecting one ardusimple to each of the gps inputs and passing data between them, and throws away data if it is not configured correctly. They’ve matured the gps “auto config” code to assume that this is the physical configuration.
In my case, I am using a stacked set of ardusimples and don’t need any of the driver’s config smarts, but I was not able to work around the checks and couldn’t get the driver to accept the gps heading message even though I know it was being sent. For fun, try making sure that gps autoconfig is off, and flashing 4.3 for your system and see if it “just works”
It’s on my list to circle back to fix this issue, but for now I’m running 4.3
Thanks for the reply, I gave it a try on 4.3.0 but I unfortunately get the same results. (I also gave it a try with 4.3.8 just in case it was a bug that was fixed)
but it was certainly worth a go.
Do you know if there’s any advantage to having them stacked vs routing via the ardupilot? I only went for the stacked option to simply wiring and to keep the footprint smaller.
So far I think you just save a telem port, which is why we do it, but I know others on this forum swear by the autoconfig code of later firmware versions. I haven’t had a chance to try it and on our systems that extra telem port is spoken for. I will report back someday if I find something definitive
So I’ve walked through the code to figure out how much work the driver is doing. Basically it boils down to the following.
AB_GPS_UBLOX.cpp does a sanity check to make sure that the magnitude of the baseline reported by the rtk solution is within 20% of the actual configured distance between the gps antennas as configured in GPS_POS. It also does a number of other checks, and if they all pass, the driver will report that it has gps yaw available and the value will be set to the gps yaw. See line 1379. There is also a staleness check in line 1581.
AP_NavEKF3_Measurements.cpp line 644 checks to see if yaw is available for incorporation into the EKF, which uses the have_gps_yaw check before proceeding
sending the gps yaw down in a mavlink message is contingent on a slightly different set of bools,
AP_GPS::gps_yaw_cdeg is used to report the yaw on the downlink, but it does an extra check to see if gps yaw is “configured”. In the case of a standalone ublox stack, this will always be false, as for the driver to consider that gps is “configured” it needs to pass the checks in AP_GPS_UBLOX.cpp line 92, or that the gps is configured as a ublox moving baseline rover. When I set this, the driver doesn’t seem to correctly read the ublox stack, but you might have more luck.
I ultimately want to make it shut up and send me the baseline and gps yaw in spite of any “configured” status. I made a debug build to figure out why gps yaw wasn’t being used and these are the salient code paths I could find.
I’ve been messaging ardusimple, but they’re insisting I haven’t followed their setup instructions correctly, despite already having video evidence proving otherwise. So no luck there unfortunately.
I’ve dabbled a bit with compiling ardupilot for a custom board and interfacing with surface level bits and pieces, but I’ll freely admit I’m not a coder, or even familiar with most of ardupilots code.
Would you be able to humour me and explain what I should do with the patch?
Presumably add it to a specific file and compile A version of ardupilot?
Again, thankyou for your time and effort looking into this.
This thread is why I never recommend the SImpleRTK2B+Heading kit for ArduPilot. It’s a giant pain in the ass. Just use two SimpleRTK2B boards and be done with it (usually cheaper, too).
That said, it does work. Most common reason for failure, assuming you did actually follow all instructions, is either a wrong antenna offset measurement or testing in a poor reception area (indoors or next to buildings).
But that’s assuming you actually followed the instructions to the letter…the failure of which is the real most common reason so many can’t get this kit to work.
The easiest path to using what I’ve pasted is to remove the lines that start with - and add the lines that start with +. The lines that start with @@ tell you what line number in the file the changes are close to.
The better solution is to use the patch command line utility. Copy my text into a file called gps.patch and put it at the root directory of your repo, and issue git patch apply or something like that. If it “just works” you should see a git status change that amounts to the changes above, it should look very similar. I made these changes against the latest 4.3 branch, your mileage may vary. Essentially, I enable some debug output for the driver, and then in the check where the gps yaw “might” be used, I add debug output that says why it wasn’t used.
that would have been nice to know before I started
I can still use the two boards as two separate receivers, so I may do that if that typically has more success.
i’m testing in the middle of field with very good view of the sky. as optimal conditions as I can get (far better than these receivers typically need when not setup for heading).
and i have the receivers rigidly mounted and their position down to the millimetre, so that its also unlikely.
which does leave me as the failure point, which would seem the most likely option. I’ve been though the setup enough times and I’ve sent ardusimple videos and photos of the entire setup procedure and they’ve yet to point out anything that was done incorrectly, so I have no idea how or where ive gone wrong.
unless ardusimple come back with some magic bullet, I’ll give it a go as two separate units and try from there.
thanks for the advice