Add RTK lat/lon/alt High Precision components to SensorGps.msg for centimeter level GPS accuracy

[slgrobotics]

This PR seeks to match PX4/PX4-GPSDrivers#136 - adding high precision RTK lat/lon/alt components from MSG_NAV_HPPOSLLH

Solved Problem

This creates a possibility to operate coordinates with precision necessary to drive land based Rovers, for example, agricultural machines.

Solution

please follow the link above for details

Note:

once 1e-9 precision is available in _gps_position ORB message, it can be used in the following modules, as far as I know:

• EKF2
• GPS Blending

For my Rover project, I modified RoverPositionControl.cpp to subscribe to ORB_ID(sensor_gps) and use precise lat/lon for course planning. This won't be necessary if EKF2 could deliver centimeter level precision coordinates.

[dagar]

I understand the motivation, but if this is needed I'd rather just take the hit and switch lat/lon to double precision floating point (degrees or radians?) and update the consumers.

PX4-Autopilot/msg/VehicleGlobalPosition.msg

Lines 11 to 12 in c468266

	float64 lat # Latitude, (degrees)
	float64 lon # Longitude, (degrees)

[slgrobotics]

I do agree that converting the lat, lon, alt and alt_ellipsoid fields in SensorGps.msg makes the most sense. From what I see in the code, degrees in WGS84 for lat/lon and meters for alt seems to be a standard.

To feel the extent of the changes I did a quick experiment - renamed the four fields (adding a _dbl postfix and making them float64) and went through all files which broke the compile (I used "make emlid_navio2" and "make px4_sitl gazebo-classic_r1_rover"). I just hacked the code to compile, not even trying to make calculations correct.

Here is the list of files I had to modify in the process:

xxx:~/src11/PX4-Autopilot$ git status
On branch RTK-HPPOSLLH
Changes not staged for commit:
	modified:   msg/SensorGps.msg
	modified:   src/drivers/gps/devices (modified content)
	modified:   src/drivers/osd/msp_osd/uorb_to_msp.cpp
	modified:   src/drivers/rc_input/crsf_telemetry.cpp
	modified:   src/drivers/rc_input/ghst_telemetry.cpp
	modified:   src/examples/fake_gps/FakeGps.cpp
	modified:   src/examples/fake_magnetometer/FakeMagnetometer.cpp
	modified:   src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
	modified:   src/modules/commander/HomePosition.cpp
	modified:   src/modules/commander/baro_calibration.cpp
	modified:   src/modules/commander/mag_calibration.cpp
	modified:   src/modules/ekf2/EKF2.cpp
	modified:   src/modules/local_position_estimator/sensors/gps.cpp
	modified:   src/modules/mavlink/mavlink_receiver.cpp
	modified:   src/modules/mavlink/streams/GPS2_RAW.hpp
	modified:   src/modules/mavlink/streams/GPS_RAW_INT.hpp
	modified:   src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp
	modified:   src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp
	modified:   src/modules/navigator/geofence.cpp
	modified:   src/modules/sensors/vehicle_gps_position/gps_blending.cpp
	modified:   src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
	modified:   src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

From what I saw so far, the EKF2 has its own struct gpsMessage (...PX4-Autopilot/src/modules/ekf2/EKF/common.h:158) with int32_t members - this might lead to deeper dive in the rabbit hole. Mavlink is a bit tricky too. Other areas seem to be rather trivial.

In any case, this is way above my head - while I can prepare a draft PR (changing the files above to have correct conversions), I cannot test it to full extent. It will also require coordination with at least GPS and EKF2 submodules maintainers.

Let me know how I can help.

[dagar]

Thanks @slgrobotics, if you want to get it started in a pull request I can jump in and help with the remaining pieces.

Naming these new fields differently (eg full latitude/longitude?) might also make it slightly easier to update Flight Review and any other random usage.

[slgrobotics]

Thanks, @dagar - I'll start on the PR ASAP. I just need a decision from you - on the naming and on what we do with the altitudes.

1. Naming. While latitude and longitude are nice and natural, there's 380 matches in the codebase for the first and 371 for the second. Making a more unique name has a benefit of immediately finding all occurrences of a variable by a simple global search.
2. Altitudes (alt and alt_ellipsoid) as they are - are already in millimeters, RTK hp components just add one decimal digit - 0.1mm granularity - totally unnecessary. Consumers do convert the int32 to float, yielding meters. We can leave these altitudes untouched and work with lat/lon only - to avoid injecting unnecessary bugs into the code, or can go ahead with submillimeter float64's for diligence. In either case, while we are at it, I would suggest renaming alt to alt_msl for easier search. We can also use full altitude_msl and altitude_ellipsoid names.
3. Usually in my code I append measurement units to variables that represent physical phenomena. This way the variables discussed would be latitude_deg, longitude_deg, altitude_msl_m and altitude_ellipsoid_m (altitude_msl_mm and altitude_ellipsoid_mm if left as int32 and simply renamed). Just a thought.

Let me know what you've decided, I am fine with any choice.

[slgrobotics]

@dagar - please review the above, I need your input on naming and alt handling to proceed. Thanks!

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-community-q-a-june-21-2023/32764/1

[slgrobotics]

@dagar - I checked in all changes related to renaming lon/lat/alt

It compiles fine under "make emlid_navio2" and "make px4_sitl gazebo-classic_r1_rover"

Related GPSDriver changes are now in new PR: PX4/PX4-GPSDrivers#136

I wasn't able to compile with UAVCAN enabled, so there are places where verification and more changes might be needed:

src/drivers/uavcan/sensors/gnss.cpp : 340
src/drivers/uavcannode/Publishers/GnssFix2.hpp : 84
src/drivers/cyphal/Publishers/udral/Gnss.hpp : 69

I am pretty sure there are some more places that need a little touch, and, of course, testing.
I will be doing testing on my Rovers, but my vehicle park is very limited.

[slgrobotics]

I will go through "checks" and fix the errors where possible.

[slgrobotics]

@dagar - here is the status:

1. I temporarily changed .gitmodules to point to RTK-HPPOSLLH branch of https://github.com/slgrobotics/PX4-GPSDrivers, to allow builds
2. I corrected build errors, including astyle format ones
3. Jenkins seems to build everything fine, but chokes on "make px4_fmu-v5_test". The code doesn't fit in FLASH_AXIM. Same happend when I build on my machine, but "make px4_fmu-v5" finishes fine.
4. Not sure why "tests" phase of "make px4_fmu-v4_test" has failed - it builds fine on my machine at least.

Let me know what else can I do.

[slgrobotics]

I fixed errors related to double/float promotion in CLANG (make clang-tidy-quiet) and all other checks.

"make tests-coverage" succeeded on my Ubuntu 20.04 PC, all 138 tests passed, created coverage/lcov.info (11MB file)

I'd guess we need another full build done.

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-maintainers-call-july-04-2023/32943/1

[bkueng]

Thanks for going through this.

Can you increase the data version to 2 in

PX4-Autopilot/src/modules/logger/logger.cpp

Line 2106 in dccfcb8

write_info(type, "ver_data_format", static_cast<uint32_t>(1));

and add a comment regarding the gps message update?

And can you provide a log after all the updates (using the rover is fine)?
I can then update flight review accordingly.

[slgrobotics]

@bkueng - I uploaded log file from my full size gas-powered zero-turn lawnmower:

https://review.px4.io/plot_app?log=6c95bdf7-57a8-4e1b-b292-1d2e1d4362a7

I fixed all I could, need your decision on src/drivers/osd/msp_osd/uorb_to_msp.cpp

Thanks!

[bkueng]

Thanks @slgrobotics. Does this include my comments from the GPS submodule as well?

[slgrobotics]

sorry, missed that - working on it.

[slgrobotics]

@bkueng - Updated as you suggested, including GPSDrivers. Let me know if I understood your suggestions correctly. Thanks!

[slgrobotics]

Just in case - a fresh log here:

https://review.px4.io/plot_app?log=b835e623-eb13-4720-b964-a3562e1d7ce5

[bkueng]

PR for flight review: PX4/flight_review#263

[slgrobotics]

I updated the gps submodule and reverted .gpsmodules, but when it came to rebasing and squashing - things got messy.

I am relatively new to github, and this is my first attempt on squashing (rebase -i RTK-HPPOSLLH~35) - so, when I got through it I found many strangely squashed commits.

Here I had to do "git reset --hard e3ba98c145" and push -f, to get back to a nice state (i.e. .gitmodules updated and then
e4349a3 merged latest from main. "This branch is 36 commits ahead of PX4:main.").

Will try to squash again, but just in case - invited @bkueng as collaborator with, I hope, full rights to this branch.

[slgrobotics]

@bkueng -- Ok, squashing seems to be "above my pay grade" ;-)

I tried to squash only my changes, and removed lines with others - here's what I've got on a guinea pig branch:

https://github.com/slgrobotics/PX4-Autopilot/tree/RTK-HPPOSLLH-2

This was result of the following:

reword 0be6d770f0 Update SensorGps.msg, lat/lon/alt as double for RTK precision
squash 67b0c6600b Did the renaming, altitudes need work
squash a4ab470219 Everything replaced/corrected and compiles fine
squash 95970a64f9 Update SensorGps.msg
squash 8284936e24 More renaming and scaling
squash 9d8ab5301a Applied "make format" to pass "make check_format" test
squash 700fba1bdd Update .gitmodules
squash 10566d8a29 Submodule updated
squash cb34c56c00 Update sPort_data.cpp
squash 7841d2f567 Updated submodule GPSDrivers
squash 698b3968f0 Fixed more Jenkins build errors
squash c5937e6ed3 Apply suggestions from code review
squash cdbcbe7a0e Update logger.cpp
squash 3ed5a8d654 Update SagetechMXS.cpp
squash f7058b7a1b Updated per @bkueng suggestions
squash 8ed713385a Update devices
squash 365a305183 Update devices
squash e3ba98c145 Updated .gitmodules to use "main" branch of GPSDevices

I can force push of the same to this branch (RTK-HPPOSLLH) - just feel uncomfortable with submodules changes and possible difficulty in merging. The branch in its current state can be easily merged though.

Basically I need either a direction (what exactly the purpose of the squashing - to squash only my changes and ignore others, or to bring everything up in sync with main and then apply my changes in one squash?) - or I'd give a more qualified person full access to the RTK-HPPOSLLH branch to do remaining conversions.

[bkueng]

Apart from the submodule changes that slipped in your rebase looks correct. I did it now for your branch.
We generally want to make sure every commit compiles, and having a clean history while splitting independent changes into separate commits. As in this case everything is related, it makes sense to keep it all in one.

Let's wait for CI, otherwise it's good to merge.

[slgrobotics]

I just cloned the "main" and my RTK-HPPOSLLH locally and compared the trees. I see my changes and some changes related to latest main check-ins (like UWB stuff) - but nothing in the submodules (rather than GPSDrivers of course). So, I'd guess, the squashed RTK-HPPOSLLH branch is as good as it gets for the merge.