Move GPS aggregation and blending to sensors #14447

dakejahl · 2020-03-20T19:24:25Z

Brought the work from this PR #13584 up to date with current master

Need to test still.

TSC21 · 2020-03-20T19:30:51Z

@dakejahl the correct way of rebasing work is to rebase commits. Not to pick up commits from another developer and bring it together as your own. That removes the credit and effort from the other developer.

dakejahl · 2020-03-20T19:33:47Z

Rebasing was a mess, this was much quicker, cleaner and easier. I am working with @dagar on this.

I could care less about credit so if that matters we can close this and dagar can unstage and reopen...

TSC21 · 2020-03-20T19:36:56Z

Rebasing was a mess, this was much quicker, cleaner and easier. I am working with @dagar on this.

I could care less about credit so if that matters we can close this and dagar can unstage and reopen...

Cleaner it isn't. Easier probably is for sure. Well done rebasing takes effort and it's the right thing to do. Anyway if @dagar is fine with it, I don't oppose as it's not my work.

mrpollo · 2020-03-20T20:13:18Z

@TSC21 Thanks for raising this issue 👍. After a quick Slack discussion with @dakejahl, we were able to find a proper solution.

@dakejahl, thanks for your remarkable contribution, and for addressing @TSC21 comments by adding @dagar as a co-author on your commits.

Great teamwork. 🎉

dakejahl · 2020-03-20T21:59:52Z

Tested successfully using Neo V2 GPS connected to a Pixhawk4 and a UBX GPS connected to a pixracer configured as a CAN node connected to the Pixhawk4 over CAN.

Blending of both GPS receivers (only works after boot, if you hotplug one or the other it won't blend because hdop vdop and speed accuracy are not being sent over uavcan)
Unplug Neo V2, fall back to UBX
Unplug UBX, fall back to Neo V2
Unplug both, time out
Replug both back in, first connects to the better GPS (Neo V2), and then after some time begins to blend both again

dakejahl · 2020-03-20T22:16:01Z

Hmm looks like we don't publish hdop, vdop or s_variance_m_s over uavcan from the cannode gps, the uavcan Fix2 message doesn't even contain these fields... this is important to the logic in blend_gps_data()

What should we do about this? @dagar @JacobCrabill

		if (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) {
			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
				if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s > 0.0f) {
					speed_accuracy_sum_sq += _gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s;
				}
			}
		}

		// calculate the sum squared horizontal position accuracy across all GPS sensors
		float horizontal_accuracy_sum_sq = 0.0f;

		if (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) {
			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
				if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) {
					horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph;
				}
			}
		}

		// calculate the sum squared vertical position accuracy across all GPS sensors
		float vertical_accuracy_sum_sq = 0.0f;

		if (_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) {
			for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
				if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) {
					vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv;
				}
			}
		}

		// Check if we can do blending using reported accuracy
		bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f
					|| speed_accuracy_sum_sq > 0.0f);

		// if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
		if (!can_do_blending) {
			return false;
		}

JacobCrabill · 2020-03-20T22:31:25Z

My simple solution was just to use pdop for both hdop and vdop; I believe they're similar enough quantites that it works ok (if you're only using CAN GPS units, then it's totally fine). Definitely not perfect if you have another GPS with real hdop/vdop data, but I don't know of a better solution.

mhkabir · 2020-03-20T23:45:38Z

@JakeDahl can you fix these too: #6275

This is the best time to do this.

dakejahl · 2020-03-21T02:49:15Z

Tested successfully! After a days worth of debugging (mostly cannode side) this finally works.

Blending of both GPS receivers
Unplug Neo V2, fall back to UBX
Unplug UBX, fall back to Neo V2
Unplug both, time out
Replug both back in, first connects to the better GPS (Neo V2), and then after some time begins to blend both again

@JacobCrabill would you mind testing this on your end as well?

dakejahl · 2020-03-21T02:51:15Z

@mhkabir I'm on a critical path right now. Would you mind making the changes you are requesting and just pushing up the commit? Otherwise I'd like to get this merged and keep chugging along with CAN stuff.

LorenzMeier · 2020-03-21T10:54:08Z

@dakejahl If you care about your commits, please interactively rebase. Otherwise I will squash and merge your PRs as colloquial commits like "thanks clang" do not belong into an upstream open source project.

I would recommend to run make check_format and installing the pre-commit hook. That will make sure you're code style is always correct.

I will squash this PR since it seems appropriate to have single commit.

LorenzMeier

This looks architecturally correct to me (moving this into the work queue and "cleaning up" the resulting message flow from GPS -> fused estimate -> estimator), but I'd like to get the review of @CarlOlsson and @bresch on it.

dakejahl · 2020-03-21T17:17:23Z

This log is less helpful than I thought it would be, but here it is.

Unplug NeoV2
Unplug UBX on CAN
Plug in NeoV2 (driver errors so it does not actually connect)
nsh> WARN [gps] ubx msg 0x0104 invalid len 61458
Plug in UBX on CAN
Replug NeoV2
Observe blending of both GPS again

https://review.px4.io/plot_app?log=4b8998a9-428a-448c-b9bc-b4bdd722b8b9

JacobCrabill · 2020-03-22T20:16:15Z

src/drivers/uavcan/sensors/gnss.cpp

@@ -52,7 +52,7 @@ using namespace time_literals;
 const char *const UavcanGnssBridge::NAME = "gnss";

 UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
-	UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(vehicle_gps_position)),
+	UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(sensor_gps)),


In the future we should consider removing the use of CDev as a base class for UavcanSensorBridge; I don't think the CDev interfaces are used anymore. If they are, then ideally those sensor types would have a generic interface class like PX4Barometer / PX4Magnetometer.

JacobCrabill · 2020-03-22T20:19:30Z

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

-			if ((gps1_updated && _gps_select_index == 0) || (gps2_updated && _gps_select_index == 1)) {
-				_gps_new_output_data = true;
+			// Second, compare GPS's with best fix and take the one with most satellites
+			uint8_t max_sats = 0;


This was just the quickest/simplest heuristic I came up with for picking which GPS was best; we could also use an accuracy measurement if all GPSs are publishing compatible quantities.

Is it possible for a GPS unit to report "No fix" (fix_type == 0) while still reporting some number of satellites?

And vice versa, reporting "3D fix" while having zero satellites?

If one GPS doesn't have a fix while another does, this will automatically discard the GPS without the fix (only the GPSs with the "best fix" will be sorted through).

And as to 0 satellites... I doubt that would happen, but I could see a case where two different units both have a 3D fix with multiple satellites, but the one with fewer satellites could have a better speed/position accuracy estimate than the one with more satellites. Unlikely perhaps, but plausible.

@dakejahl could you add me as a co-author on this commit, seeing as I did the original blending modifications here?

I've likely already made a mess of things with squashed commits. Happy to restore credit.

lukegluke · 2020-08-09T11:07:01Z

Hi all,

While working with rovers with GPS RTK precision it turned out that 1E-7 degrees precision for lat and long was not enough in several specific cases. I suppose it's worst (at least in this PR) to discuss proposal to make lat and lon as double (and alt as float) in sensor_gps.msg:

double lat		# Latitude in degrees
double lon		# Longitude in degrees
float alt		# Altitude in meters above MSL
float alt_ellipsoid 	# Altitude in meters above Ellipsoid

Yes, I understand that 1e-7 precision is quite good (1.11 cm at equator) and enough for many general drones use cases, but I believe that losing precision in gps drivers straight away on parsing is not good thing for px4 framework and mitigates its universality and customization potential!

Another argument - that in different calculations (especially in ekf) px4 internally already use lat, lon as doubles converting them on input and output every time back and forward from deg/1e-7deg (and m/mm for alt). I don't know how much, but I suppose CPU will be reduced a little, just by increasing RAM usage for 8 bytes * (GPS receivers number).
Obviously, 1e-7 precision can still be used as output in legacy things, like current gps mavlink messages.

Most disadvantage - it will break all log analyzes that use vehicle_gps_position (but anyway finally they have to correct parser to sensor_gps anyway, right?)

Also I would like to suggest here to add dgps_age field to sensor_gps.msg (and fill it in MavlinkStreamGPS2Raw) and parsed it in drivers where available (for example in @bys1123 PR GpsDrivers #58 NMEA-0183)

dagar · 2020-08-23T16:53:16Z

Time to finally bring this in!

dagar · 2020-08-23T16:57:27Z

@lukegluke I haven't reviewed the various GPS receivers, but from a PX4 message standpoint I don't currently see a reason to be opposed to moving latitude and longitude to double precision floating point. Computationally we're already working with lat/lon in double and the difference in message size is pretty minor for a relatively low rate message like this.

I would propose we get this PR safely into master and do an immediate followup pass.

dagar · 2020-08-23T18:07:46Z

Rebased on current master and GPSDrivers submodule updated to latest.

dagar · 2020-08-23T18:40:52Z

I've added parameter migration for the changes.

EKF2_GPS_MASK -> SENS_GPS_MASK
EKF2_GPS_TAU -> SENS_GPS_TAU

dagar · 2020-08-23T19:15:33Z

Later we should move the GPS position offset configuration to out of EKF2 and into the sensors module handled per instance.

EKF2_GPS_POS_{X, Y, Z}

One question is if we actually shift the sensor_gps data before publishing (vehicle_gps_position) or publish it as metadata for the estimator to handle.

@CarlOlsson I believe this is how we should properly handle your GPS offset issue.

dagar · 2020-08-23T21:35:40Z

We don't have to do it in this PR, but ideally we'd finally get some unit testing for the actual GPS blending logic here after another incremental refactor.

- N x sensor_gps => vehicle_gps_position - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

bresch · 2020-08-24T07:30:53Z

One question is if we actually shift the sensor_gps data before publishing (vehicle_gps_position) or publish it as metadata for the estimator to handle.

I'm in favor of sending the gps measurements without any modification and send the metadata such that the estimator can perform all the required corrections: removing tangential velocity requires gyro data.

CarlOlsson · 2020-08-24T08:03:00Z

I'm in favor of sending the gps measurements without any modification and send the metadata such that the estimator can perform all the required corrections: removing tangential velocity requires gyro data.

Me too, otherwise we get a strong coupling between the health of the estimator and the "raw" gps data

src/modules/sensors/vehicle_gps_position/CMakeLists.txt

bresch · 2020-08-24T09:02:57Z

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

+		double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7;
+		double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7;
+		double lat_deg_res, lon_deg_res;
+		add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res,


I don't think we should modify the GPS individual readings; IMO, the blending logic should create a blended output from all the receivers but not adjust each GPS solution, why do we do that?

@dakejahl @JacobCrabill any comment here? I don't believe I modified any of the logic when I did the original extraction in #13584, although obviously it's been a while...

Ok, looks like this isn't modified from what ekf2 is currently doing.

I don't think we should modify the GPS individual readings; IMO, the blending logic should create a blended output from all the receivers but not adjust each GPS solution

I agree, storing some of this incremental state was one of my criticisms of the original gps blending work in ekf2, but it was a bit more confusing going between uORB vehicle_gps_position -> ecl/EKF gps_message and then back to ekf_gps_position (nearly identical to vehicle_gps_position). Let me see if I can eliminate it this time.

bresch · 2020-08-24T09:17:37Z

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

+	// Convert each GPS position to a local NEU offset relative to the reference position
+	// which is defined as the positon of the blended solution calculated from non offset corrected data
+	Vector2f blended_NE_offset_m;
+	blended_NE_offset_m.zero();
+	float blended_alt_offset_mm = 0.0f;
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_blend_weights[i] > 0.0f) {
+			// calculate the horizontal offset
+			Vector2f horiz_offset{};
+			get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7),
+						    (_gps_blended_state.lon / 1.0e7),
+						    (_gps_output[i].lat / 1.0e7),
+						    (_gps_output[i].lon / 1.0e7),
+						    &horiz_offset(0),
+						    &horiz_offset(1));
+
+			// sum weighted offsets
+			blended_NE_offset_m += horiz_offset * _blend_weights[i];
+
+			// calculate vertical offset
+			float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt);
+
+			// sum weighted offsets
+			blended_alt_offset_mm += vert_offset * _blend_weights[i];
+		}
+	}


Are we not already doing the correction here: https://github.com/PX4/Firmware/pull/14447/files#diff-5c37c83ace9fdd92b81d362d265d2f21R503-R525 ?

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

JacobCrabill · 2020-09-02T16:47:19Z

I'll give this a review soon

dagar · 2020-09-25T20:54:40Z

In order to prevent this from dragging out even longer I'm going to revert this branch to the last "good" tested state and get it rebased and merged immediately. Having it merged unblocks further GpsDriver updates and helps with multi-ekf. I'll then restore the cleanup fixes in a new PR we can test incrementally.

…_position

dakejahl mentioned this pull request Mar 20, 2020

updated to sensor_gps_s PX4/PX4-GPSDrivers#53

Merged

dakejahl force-pushed the pr-sensors_vehicle_gps_position branch 2 times, most recently from d0c3171 to a6237ba Compare March 20, 2020 20:07

dagar mentioned this pull request Mar 21, 2020

[question] Mavlink GPS #14445

Closed

This was referenced Mar 21, 2020

[bugfix] cannode GPS #14450

Merged

EKF: Improve GPS blending; Allow >2 GPSs #14355

Closed

move GPS aggregation and blending to sensors #13584

Closed

dakejahl changed the title ~~[WIP] Move GPS aggregation and blending to sensors~~ Move GPS aggregation and blending to sensors Mar 21, 2020

dakejahl force-pushed the pr-sensors_vehicle_gps_position branch from 10bdb4e to d4f89ca Compare March 21, 2020 03:13

dakejahl requested a review from dagar March 21, 2020 03:13

LorenzMeier previously approved these changes Mar 21, 2020

View reviewed changes

LorenzMeier requested review from bresch and CarlOlsson March 21, 2020 11:00

dakejahl dismissed LorenzMeier’s stale review via 243a919 March 21, 2020 16:58

dakejahl force-pushed the pr-sensors_vehicle_gps_position branch from c3a0696 to 243a919 Compare March 21, 2020 16:58

dakejahl force-pushed the pr-sensors_vehicle_gps_position branch from 243a919 to 89e785b Compare March 21, 2020 17:49

JacobCrabill reviewed Mar 22, 2020

View reviewed changes

dagar mentioned this pull request Jun 20, 2020

Update submodule devices to latest Sat Jun 20 00:38:47 UTC 2020 #15151

Closed

dagar force-pushed the pr-sensors_vehicle_gps_position branch from 6d57642 to 27e4597 Compare August 23, 2020 18:07

dagar force-pushed the pr-sensors_vehicle_gps_position branch 3 times, most recently from bc1dca8 to 07ff168 Compare August 23, 2020 19:10

dagar requested review from JacobCrabill and dagar August 23, 2020 21:03

move GPS aggregation and blending ekf2 -> sensors

ee42faf

- N x sensor_gps => vehicle_gps_position - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

dagar force-pushed the pr-sensors_vehicle_gps_position branch from 07ff168 to ee42faf Compare August 23, 2020 22:30

bresch requested changes Aug 24, 2020

View reviewed changes

Update src/modules/sensors/vehicle_gps_position/CMakeLists.txt

f5bf1b1

Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>

dagar force-pushed the pr-sensors_vehicle_gps_position branch from 25ff582 to 614facf Compare August 24, 2020 21:18

dagar force-pushed the pr-sensors_vehicle_gps_position branch from 614facf to ab0217b Compare September 26, 2020 02:08

Merge remote-tracking branch 'px4/master' into pr-sensors_vehicle_gps…

9575815

…_position

dagar force-pushed the pr-sensors_vehicle_gps_position branch from ab0217b to 9575815 Compare September 26, 2020 02:28

dagar merged commit a244883 into master Sep 26, 2020

dagar deleted the pr-sensors_vehicle_gps_position branch September 26, 2020 03:28

dagar mentioned this pull request Sep 27, 2020

sensors/vehicle_gps_position: untangle and remove unnecessary state #15827

Merged

lukegluke mentioned this pull request Jan 10, 2021

EKF without baro sensor can stop fusion any height permanently PX4/PX4-ECL#951

Open

Move GPS aggregation and blending to sensors #14447

Move GPS aggregation and blending to sensors #14447

Conversation

dakejahl commented Mar 20, 2020

TSC21 commented Mar 20, 2020 • edited

dakejahl commented Mar 20, 2020

TSC21 commented Mar 20, 2020

mrpollo commented Mar 20, 2020

dakejahl commented Mar 20, 2020 • edited

dakejahl commented Mar 20, 2020 • edited

JacobCrabill commented Mar 20, 2020

mhkabir commented Mar 20, 2020 • edited

dakejahl commented Mar 21, 2020 • edited

dakejahl commented Mar 21, 2020

LorenzMeier commented Mar 21, 2020

LorenzMeier left a comment

Choose a reason for hiding this comment

dakejahl commented Mar 21, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

lukegluke commented Aug 9, 2020 • edited

dagar commented Aug 23, 2020

dagar commented Aug 23, 2020

dagar commented Aug 23, 2020

dagar commented Aug 23, 2020

dagar commented Aug 23, 2020 • edited

dagar commented Aug 23, 2020

bresch commented Aug 24, 2020

CarlOlsson commented Aug 24, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

JacobCrabill commented Sep 2, 2020

dagar commented Sep 25, 2020

TSC21 commented Mar 20, 2020 •

edited

dakejahl commented Mar 20, 2020 •

edited

dakejahl commented Mar 20, 2020 •

edited

mhkabir commented Mar 20, 2020 •

edited

dakejahl commented Mar 21, 2020 •

edited

lukegluke commented Aug 9, 2020 •

edited

dagar commented Aug 23, 2020 •

edited