ekf2: Allow use of RTK GPS heading #10355

priseborough · 2018-08-28T20:38:52Z

Adds feature requested by #10284

Is inactive with default parameters.

Testing

See PX4/PX4-ECL#497 for details of previous testing using replay.

Testing this branch requires setting EKF2_AID_MASK = 129 and EKF2_MAG_TYPE = 5. That will force the EKF to wait until GPS yaw is available before performing the yaw alignment. If EKF2_MAG_TYPE = 0, then the yaw will be set using the magnetomer intially and then reset to the GPS yaw when it becomes available.

Testing using replay was repeated and documented in the comments below.

dagar · 2018-08-28T20:44:11Z

src/modules/ekf2/ekf2_main.cpp

@@ -691,6 +691,14 @@ void Ekf2::run()
 	vehicle_status_s vehicle_status = {};
 	sensor_selection_s sensor_selection = {};

+	// get heading offset required for use of GPS yaw
+	param_t handle = param_find("GPS_YAW_OFFSET");


Add this to vehicle_gps_position instead of accessing the parameter directly?

We could do, but that will increase log size unnecessarily

If that's enough to push it over the edge we could consider a low rate (nearly static) metadata message as well. That could also contain the position.

Are you able to explain the reasons for not wanting to adopt the approach used here of checking the parameter value on startup? It seems like the simplest way to achieve the required result.

Generally it ends up being a bad idea to cross module boundaries. These parameters are internal data of that driver or module, not a system wide setting.

In the simple intended use case I'm sure this is fine, but how does it scale when there are multiple GPS receivers, or GPS from UAVCAN, Mavlink, or some mix of all of the above. What about the GPS being connected to one system, but running the estimator on another with messages passed via FastRTPS?

Yes I'm jumping ahead, but these are the reasons to try and keep things as modular as possible as we go along .

@dagar Can you show me an example of a 'low rate (nearly static) metadata message'

I don't think we have a good example at the moment, but it's an idea I want to pursue for the high rate messages like IMU. This isn't quite the same, but think of sensor_combined. The IMU is published (and logged) at high rate, but we publish the corresponding device ids separately in sensor_selection only when changed.

Is this GPS message with yaw still fairly low rate? If so I wouldn't worry about it for now.

Idea

sensor_gyro (0, 1, 2) - contains x, y, z values at 250 Hz - 2kHz sensor_gyro_info (0, 1, 2) - contains metadata for the corresponding instance (device id, health, clipping, etc)

dagar · 2018-08-28T20:44:27Z

src/modules/ekf2/ekf2_main.cpp

@@ -980,6 +990,8 @@ void Ekf2::run()
 				_gps_state[1].lat = gps.lat;
 				_gps_state[1].lon = gps.lon;
 				_gps_state[1].alt = gps.alt;
+				_gps_state[1].yaw_offset = gps_yaw_offset;
+				_gps_state[1].fix_type = gps.fix_type;


fix_type line repeated

dagar · 2018-08-28T20:44:41Z

src/modules/ekf2/ekf2_main.cpp

@@ -980,6 +990,8 @@ void Ekf2::run()
 				_gps_state[1].lat = gps.lat;
 				_gps_state[1].lon = gps.lon;
 				_gps_state[1].alt = gps.alt;
+				_gps_state[1].yaw_offset = gps_yaw_offset;


yaw (gps.heading) missing?

Will fix. Yaw not supported by GPS blending at the moment but will be adding support in a subsequent PR that enables a yaw output and non yaw output receiver to be used together.

priseborough · 2018-08-28T21:12:42Z

Results from replay of log file
https://logs.px4.io/plot_app?log=d9bc48f9-3270-4ff9-8141-c33a006f9f9d

Note: The jump in heading innovations when turning are the result of the bug in PX4/Firmware that was present when the original log data was generated. This was been subsequently fixed upstream by
#10346

Replay log here: https://logs.px4.io/plot_app?log=67bf7874-8a52-4c56-9baf-eb9d46985dc1

priseborough · 2018-08-28T21:18:14Z

@bkueng Are you able to repeat the test you did here: https://logs.px4.io/plot_app?log=25380997-1d97-4e3c-8006-58fd4d4e52c5 and supply the data log?
This data will confirm the combination of recent driver changes and this PR provide adequate protection against gimbal lock and angle wrapping.

bkueng · 2018-08-29T15:25:42Z

Here are 2 logs on this branch with enabled GPS yaw fusion (logging from boot until shutdown):

The second one contains the rolling/pitching towards the end of the log.

priseborough · 2018-08-29T22:34:34Z

Inspection of the heading and GPS position and velocity innovations show that data is being fused correctly and the vehicle has a valid heading. Heading fusion stops as designed when the GPS antennas go through a bad orientation.

51d7e1e3-8562-46d3-8b3c-960e008516b5.ulg innovations

1005486e-e7f4-4088-9b20-22c48036d22a.ulg innovations - these are larger than for the previous log

One of the limitations with the interface used by this type of GPS is that there is no data to populate the s_variance_m_s field so the default observation noise of 0.5 m/s as set by the EF2_GPS_V_NOISE parameter is used at all time regardless of receiver signal quality. Something to look at for later improvement is scaling the noise using eph if the s_variance_m_s data is not populated.

priseborough · 2018-08-29T22:38:00Z

Once we get an agreed method for messaging the heading offset, then this can be merged.

priseborough · 2018-09-02T11:39:58Z

@dagar I've added the heading offset to the GPS message along with protection in the ecl library if the heading_offset is undefined.

The logs don't contain the additional heading_offset message, so the previous logs can't be used to test the change. Once you are happy with the changes, I will ask @bkueng to generate a new log when he has time.

dagar · 2018-09-02T13:49:34Z

Small failure ecl side.

beat · 2018-09-02T16:41:50Z

@priseborough Please stop using @beat as alias, as I'm not the Beat Küng you are referring to. You should be using @bkueng for Beat Küng instead. That will for sure also speed up his replies. Thanks! 😄

dagar · 2018-09-03T01:37:53Z

src/modules/ekf2/ekf2_main.cpp

@@ -2223,6 +2227,10 @@ void Ekf2::calc_gps_blend_output()
 	_gps_output[GPS_BLENDED_INSTANCE].nsats		= _gps_blended_state.nsats;
 	_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid	= _gps_blended_state.vel_ned_valid;

+	// No suppoort for blending of yaw data at the moment


typo - suppoort

bkueng

Do I see it correctly that we don't have heading fusion if we use dual gps?

bkueng · 2018-09-03T07:36:39Z

src/drivers/gps/gps.cpp

@@ -885,6 +888,7 @@ GPS::publish()
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
 		_report_gps_pos.heading = NAN;
+		_report_gps_pos.heading_offset = NAN;


This needs to be removed.

bkueng · 2018-09-03T07:37:31Z

src/drivers/gps/gps.cpp

@@ -688,6 +690,7 @@ GPS::run()

 			case GPS_DRIVER_MODE_ASHTECH:
 				_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
+				_report_gps_pos.heading_offset = heading_offset;


Can you set this unconditionally for all drivers, so that it automatically works when we add heading support to another driver?

priseborough · 2018-09-03T08:06:24Z

@bkueng That is correct. I am still working out the best way to achieve that. The easiest is to always pass through the heading from the receiver that has it. If both do, then blend it using the position accuracy ratio, given we do not have a heading accuracy value from the receiver.

bkueng · 2018-09-03T08:48:15Z

@bkueng That is correct. I am still working out the best way to achieve that. The easiest is to always pass through the heading from the receiver that has it.

I think passing it through is good enough for now.

src/modules/ekf2/ekf2_main.cpp

bkueng

@priseborough I was able to test it again, here's the log: https://logs.px4.io/plot_app?log=9bdbab10-cb54-4f9d-85e6-0b53f338235a

bkueng · 2018-09-07T14:41:00Z

src/modules/ekf2/ekf2_main.cpp

@@ -2070,9 +2070,11 @@ void Ekf2::update_gps_blend_states()

 	// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
 	uint8_t gps_best_yaw_index = 0;
+	best_weight = 0.0f


There's a semicolon missing.

priseborough · 2018-09-11T11:08:28Z

The heading_offset is 0 and the heading is NAN in that log. Did you upload the right file?

bkueng · 2018-09-11T11:30:36Z

The heading is there:

The offset is indeed 0, but the GPS_YAW_OFFSET parameter is set to 90.
The problem is that the heading offset gets reset here: https://github.com/PX4/Firmware/blob/f1d08d90dadc77c60df502533f9b1cf94a868241/src/drivers/gps/gps.cpp#L707.

priseborough · 2018-09-11T20:35:32Z

What program are you using to plot it? I am using FlightPlot v0.3.2 and it doesn't display, eg

dagar · 2018-09-19T17:41:42Z

@priseborough I don't think we're there yet, but a rebase couldn't hurt to see how close we're getting.

priseborough · 2018-09-19T21:15:40Z

We are now a lot closer on px4fmu-v2

The omnibus-f4sd build is in worse shape

dagar · 2018-09-19T21:34:03Z

@bkueng drop VTOL and FW from omnibus? We can fix any parameter interdependencies that have crept in.
https://github.com/PX4/Firmware/blob/master/cmake/configs/nuttx_omnibus-f4sd_default.cmake#L87-L93

Update: If we can drop VTOL/FW from Omnibus and merge #10530 we should have enough to unblock Paul.

bkueng · 2018-09-20T06:10:00Z

@bkueng drop VTOL and FW from omnibus? We can fix any parameter interdependencies that have crept in.

Sounds good. Meanwhile we can quickly disable the topic listener (#10533) to unlock this.

priseborough · 2018-10-09T03:47:04Z

I've rebased. We are still 1345 bytes short on the px4fmu-v2 target.

LorenzMeier · 2018-10-09T06:15:10Z

@bkueng Could you have a look?

bkueng · 2018-10-09T11:00:46Z

@priseborough you can disable any of the distance sensors: https://github.com/PX4/Firmware/blob/master/cmake/configs/nuttx_px4fmu-v2_default.cmake#L35

LorenzMeier · 2018-10-09T18:56:05Z

@bkueng Thanks! Do we have a more structured approach at fixing it?

TSC21 · 2018-10-17T07:18:42Z

One thing that concerns me about the module disabling: for fmu-v2, we are disabling several modules, but we are going to get to a stage where I do want to use one module in specific on the fmu-v2, I won't be able to do it because of flash overflow. Though I do see that the ekf2 and ecl modules keeping growing in size, but I don't see where one would exactly be able to, for example, ignore the build of some of each features. For example, this one in particular: not everyone would actually want to use the RTK heading, and eventhough the parameter is there, the code for this feature is going to be built. If this was a critical feature, I would say ok, this needs to go in. But I don't really see the scalability of this, while we keep adding new features to the estimator and ECL and don't have a way of filtering which features we want to build, keeping accumulating flash space without even wanting to use those same features.
IMHO, the way we add the build for these kind of features should be handled the in a similar way we do with modules and drivers we don't want to build for a certain target.

priseborough · 2018-10-17T11:47:41Z

@dagar and @bkueng Now that this has been rebased and is now passing CI given the available flash space, can one of you approve it before the that flash gets eaten up by something else .

TXS

TSC21 · 2018-10-17T11:59:03Z

@priseborough would you mind giving some feedback on my comment? Thanks

dagar · 2018-10-17T13:47:53Z

@TSC21 it's a more general problem and ecl/EKF just happens to be the most recent victim. There's quite a lot of flash that could be regained from the drivers themselves with no loss of functionality. Ping me if you're interested.

priseborough added the Admin: Enhancement (improvement) 💡 label Aug 28, 2018

priseborough requested a review from bkueng August 28, 2018 20:38

priseborough mentioned this pull request Aug 28, 2018

RTK GPS heading integration PX4/PX4-ECL#497

Merged

dagar reviewed Aug 28, 2018

View reviewed changes

priseborough force-pushed the pr-ekfGpsYaw branch 2 times, most recently from cb24f39 to 71a1edc Compare September 2, 2018 11:26

dagar reviewed Sep 3, 2018

View reviewed changes

dagar previously approved these changes Sep 3, 2018

View reviewed changes

bkueng reviewed Sep 3, 2018

View reviewed changes

priseborough dismissed dagar’s stale review via 9521ee8 September 3, 2018 07:57

AuterionWrikeBot mentioned this pull request Sep 4, 2018

Feature: RTK heading integration into EKF #10284

Closed

priseborough force-pushed the pr-ekfGpsYaw branch from c6bc114 to 2079a2a Compare September 4, 2018 07:44

bkueng reviewed Sep 6, 2018

View reviewed changes

src/modules/ekf2/ekf2_main.cpp Show resolved Hide resolved

bkueng reviewed Sep 7, 2018

View reviewed changes

priseborough force-pushed the pr-ekfGpsYaw branch from f243d20 to f1d08d9 Compare September 10, 2018 21:18

dagar mentioned this pull request Sep 19, 2018

mpu9250 in px4fmu-v2 #10521

Closed

priseborough force-pushed the pr-ekfGpsYaw branch from 386cebb to c4ca4b7 Compare September 19, 2018 07:13

priseborough force-pushed the pr-ekfGpsYaw branch from c4ca4b7 to 9e402c5 Compare September 19, 2018 20:36

dagar mentioned this pull request Sep 20, 2018

WIP: Omnibus MC only #10531

Closed

priseborough force-pushed the pr-ekfGpsYaw branch from 9e402c5 to 71a1045 Compare September 21, 2018 05:24

priseborough force-pushed the pr-ekfGpsYaw branch from 71a1045 to 5e71d58 Compare October 9, 2018 03:00

dagar mentioned this pull request Oct 16, 2018

px4fmu-v2_default disable lis3mdl #10704

Merged

priseborough added 8 commits October 17, 2018 21:36

msg: Add antenna array heading offset to GPS message

1769501

mavlink: publish GPS antenna heading offset as NAN

c5bef44

simulator: publish GPS antenna heading offset as NAN

d62e711

uavcan: publish GPS antenna heading offset as NAN

3f8378d

drivers: publish GPS antenna heading offset

5988c8d

ekf2: Allow use of RTK GPS heading

5ef3f48

msg: Add heading data to GPS blending output log data

b709c34

ecl: Use master with pr-ekfGpsYaw changes merged

9181bf1

priseborough force-pushed the pr-ekfGpsYaw branch from 5e71d58 to 9181bf1 Compare October 17, 2018 11:18

dagar approved these changes Oct 17, 2018

View reviewed changes

dagar merged commit 18f0812 into PX4:master Oct 17, 2018

ekf2: Allow use of RTK GPS heading #10355

ekf2: Allow use of RTK GPS heading #10355

Conversation

priseborough commented Aug 28, 2018 • edited by AuterionWrikeBot Loading

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

dagar Aug 30, 2018 • edited Loading

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

priseborough commented Aug 28, 2018

priseborough commented Aug 28, 2018

bkueng commented Aug 29, 2018

priseborough commented Aug 29, 2018

priseborough commented Aug 29, 2018

priseborough commented Sep 2, 2018 • edited Loading

dagar commented Sep 2, 2018

beat commented Sep 2, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bkueng left a 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

priseborough commented Sep 3, 2018

bkueng commented Sep 3, 2018

bkueng left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

priseborough commented Sep 11, 2018

bkueng commented Sep 11, 2018

priseborough commented Sep 11, 2018

dagar commented Sep 19, 2018

priseborough commented Sep 19, 2018

dagar commented Sep 19, 2018 • edited Loading

bkueng commented Sep 20, 2018

priseborough commented Oct 9, 2018

LorenzMeier commented Oct 9, 2018

bkueng commented Oct 9, 2018

LorenzMeier commented Oct 9, 2018

TSC21 commented Oct 17, 2018 • edited Loading

priseborough commented Oct 17, 2018

TSC21 commented Oct 17, 2018

dagar commented Oct 17, 2018

priseborough commented Aug 28, 2018 •

edited by AuterionWrikeBot

Loading

dagar Aug 30, 2018 •

edited

Loading

priseborough commented Sep 2, 2018 •

edited

Loading

dagar commented Sep 19, 2018 •

edited

Loading

TSC21 commented Oct 17, 2018 •

edited

Loading