GPS Blending (attempt2) #5841

rmackay9 · 2017-03-09T04:37:01Z

This is an updated version of @priseborough's earlier PR: #5642

This PR adds the following:

GPS_AUTO_SWITCH parameter has new Blend ("2") option which blends the input from 2 GPSs into a 3rd "virtual" GPS. The blending is performed within the AP_GPS library. If successful the "primary_gps" is changed to the 3rd GPS (aka the virtual GPS). The existing options "0" and "1" are not functionally changed by this PR.
EKF2/EKF3 resets the horizontal and vertical height estimates if the primary GPS is changed.
DataFlash logging of 3rd GPS (GPS3, GPA3 messages)
Pre-arm check that GPSs (if present) are within 50m of each other (3 dimensional distance is used)
Pre-arm check that blending is healthy (if GPS_AUTO_SWITCH is set to 2)
SITL receives a new SIM_GP2_GLITCH vector for easier testing. One downside is that the glitch only applies to the virtual Ublox GPS. These commits can be removed from the PR if they're unacceptable.
a few unrelated commits to the AP_GPS and AP_NavEKF2/3 libraries to remove unused methods, fix comments. These can be moved to a separate PR if that would help with the review.

Previous explanation of the blending method from Paul:
GPS_AUTO_SWITCH type 2 is a soft switching/blending method using a weighted average of physical receivers to create a third 'virtual' GPS where:

The method is scaleable to more than 2 GPS receivers
A 'blended' location and velocity is calculated using a weighted average of the two receivers
Weights are calculated using the ratio of reported error variances.
A filtered location offset correction is calculated for each receiver relative to the 'blended' location and used to correct the output from each receiver.
If less than 2 GPS units are available the first GPS instance detected will be used.
If there is not enough error reporting to support calculation of the blending weights, the first receiver with the equal highest solution status value will be used . This can happen if one of the two receivers does not provide a position or speed accuracy.
The existing hard switch logic has been left untouched and is selected with GPS_AUTO_SWITCH = 1 the same as before.

This has been tested in SITL and flight tested.

If the GPS receiver selection changes and we are using GPS for height, the vertical position will be reset to the new GPS height measurement. correct output observer history when doing a GPS height reset

Add GPS and GPA reporting for blended solution Allow GPS library to manage reporting for the blended solution

callers always specify an instance

No functional change

The frontend implementation was in the backend file No functional change

Fix pre-existing bug in hard switch logic Update GPS_AUTO_SWITCH description protect against zero accuracy estimates returned by GPS drivers

Also minor spelling and parameter documentation fix

highest_supported_status will always return FIX_3D for blended or invalid instance setHIL_Accuracy checks instance is 2 or less send_mavlink_gps2_raw uses num_instances variable directly to avoid confusion with num_sensors

These functions are slightly long and make the .h file hard to read. Also saves a small amount of flash space. No functional change

backends become friends so they can continue to access parameters held in frontend get_rate_ms made private because only used by frontend Also moved static arrays higher in cpp file

Copter's GPS checks will override this

Allows removing some duplicate code

WickedShell

A couple of changes that were outlined here.

A weired scenario that I'm not sure if we need to explicitly handle it, but if you have 1 3D GPS and 1 2D GPS, you will weigh the speed/height accuracies from the 3D gps as perfect, but if the 2D GPS is providing what it thinks is a good accuracy then it will be blending its horizontal position over the other GPS's position. This is an extremely unlikely scenario however, particularly because if you can only acquire a 2D fix it's unlikely that it is a terribly good position that it found.

Wrote most of the comments before talking to @rmackay9 on mumble, and I think I deleted the comments that are no longer valid/handled but it's possible there are a couple here that have already been addressed and I've missed deleting them (or I later found the answer to it and forgot to delete it).

WickedShell · 2017-03-10T01:24:49Z

libraries/AP_Arming/AP_Arming.cpp

+        if (!gps.all_consistent(distance_m)) {
+            if (report) {
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,
+                                                 "PreArm: GPSs inconsistent by %4.1fm",


Can we fit "positions" in this text if possible, just for maximum user clarity.

One issue with making long descriptions is that, at least on the mission planner, most users will view them on the HUD which is not very wide. The message is already at it's maximum length so any additional characters are simply at the expense of characters at the end of the message.
In any case, I've changed it to "PreArm: GPS positions differ by %4.1fm"

Thanks! I admit I only ever plan these messages to be relative to the statustext length. MP could break into 2 lines anything that is to long to just fit in the HUD, but thats a separate thing, and has its own complications with doing that.

WickedShell · 2017-03-10T01:27:43Z

libraries/AP_GPS/AP_GPS.cpp

@@ -149,6 +165,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
    // @Units: milliseconds
    // @Values: 100:10Hz,125:8Hz,200:5Hz
+    // @Range: 100 200


Realistically range 50 is coming as the lower number for this, I wonder if we should just set that here now. (It's a valid value as well)

Are there actually GPSs that have a 20Hz update rate? I don't think I've ever seen one. In any case, I've now set the lower limit on this parameter description to 50. Of course, as we all know, these parameter ranges are not absolute limits. They are just guidelines to the GCS to help keep users picking reasonable values. They can be overridden on the full parameter lists.

Yeah there are, Septentrio is capable of 20Hz updates, and there are some other units on the market that do it as well. Now the relative value of these positions is a bit up for debate, but they are definitely out there and can be acquired now :)

Sorry to be a nag, you updated RATE_MS but not RATE_MS2 as well

No, that's cool. thanks for cathing that. fixed.

WickedShell · 2017-03-10T01:29:36Z

libraries/AP_GPS/AP_GPS.cpp

+    _omega_lpf = 1.0f / constrain_float(_blend_tc, 5.0f, 30.0f);
+
+    // sanity check update rate
+    for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {


Why bother with this? You have already protected against callers with the get_rate_ms() function, which means at this point you could safely remove this logic for code space reasons.

I've added this start-up override of the parameter value so that there is some small feedback to the user that the value they input for the RATE_MS was invalid.

I think it would probably show up more to the user as "the parameter system isn't saving what I set this parameter to, help it's broken!" :)

yes, if the user sets it incorrectly, I imagine that's how we will hear from them. If I go ahead with removing the code, we likely won't hear from them at all and the user will be none the wiser that what they've requested isn't being done. It's a slight "peep" of a warning on a mistake that probably isn't vehicle threatening in any case.

WickedShell · 2017-03-10T01:31:05Z

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1269,7 +1269,7 @@ AP_GPS_UBLOX::_configure_rate(void)
 {
    struct ubx_cfg_nav_rate msg;
    // require a minimum measurement rate of 5Hz
-    msg.measure_rate_ms = MIN(gps._rate_ms[state.instance], MINIMUM_MEASURE_RATE_MS);
+    msg.measure_rate_ms = MIN(gps._rate_ms[state.instance], GPS_MAX_RATE_MS);


This should just be get_rate_ms() might as well enforce this as a common interface, and again reduce code size/changes for differences.

sure, done.

WickedShell · 2017-03-10T01:31:50Z

libraries/AP_GPS/AP_GPS.h

+    AP_Int8 _min_elevation;
+    AP_Int8 _raw_data;
+    AP_Int8 _gnss_mode[GPS_MAX_RECEIVERS];
+    AP_Int16 _rate_ms[GPS_MAX_RECEIVERS];


Please add a comment indicating that this should never be directly dereferenced and instead to always utilize get_rate_ms() for any caller who needs the information.

sure, added the comment.

WickedShell · 2017-03-10T02:29:16Z

libraries/AP_GPS/AP_GPS.cpp

+{
+    // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
+    state[GPS_MAX_RECEIVERS].instance = GPS_MAX_RECEIVERS;
+    state[GPS_MAX_RECEIVERS].status = NO_GPS;


Nit pick: NO_FIX would be better.

Fixed, thanks

WickedShell · 2017-03-10T02:36:53Z

libraries/AP_GPS/AP_GPS.h

@@ -157,9 +174,8 @@ class AP_GPS
        return status(primary_instance);
    }

-    // Query the highest status this GPS supports
+    // Query the highest status this GPS supports (always reports GPS_OK_FIX_3D for the blended GPS)


Somewhat the same question asked earlier, why is the blended only 3D_GPS? Shouldn't it instead be the higher of the 2 options it could be blending?

This "highest_supported_status" is only called for the real GPSs and it's only used in order to decide whether we should send RTK info to the GCS. I think we don't want to send RTK data for the virtual blended GPS in any case so I've just added the warning and within the function protected against a dangerous access to unallocated memory.

The only reason I think that maybe the RTK should be audited is for any future user. We wouldn't ever send the blended instance anyways because it would never be index 0 or 1, which is the required index for those two.

Let's escalate this to @tridge. It doesn't seem useful to me and this PR is tough enough as it is. If @tridge says let's add it then I'll go ahead and do it.

We don't have to do it by any means, I'm just trying to hold for as consistent an API without having to read all the special cases from the header file every time :)

WickedShell · 2017-03-10T02:42:56Z

libraries/DataFlash/LogStructure.h

@@ -786,10 +786,14 @@ Format characters in the format string for binary log messages
      "GPS",  "QBIHBcLLefffB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U" }, \
    { LOG_GPS2_MSG, sizeof(log_GPS), \
      "GPS2", "QBIHBcLLefffB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U" }, \
+    { LOG_GPS3_MSG, sizeof(log_GPS), \


I know I suggested GPS/GPA 3 but should we maybe make these be GPSB/GPAB for the blended instance? That way log tools can always assume which is the blended instance?

Sure, changed to GPSB and GPAB.

WickedShell · 2017-03-10T03:12:41Z

libraries/AP_GPS/AP_GPS.cpp

-                // position shift to the controllers.
-                primary_instance = i;
-                _last_instance_swap_ms = now;
+    if (_output_is_blended) {


We should only do this if the _blend_health is considered healthy. There is a race condition where a poorly configured GPS (say 2.5Hz) could get us in/out of blending every other message from the 5Hz or higher GPS. We don't want to be swapping between the blended and non blended GPS every 200 msec's. (Particularly since we might be swapping between instance 1, 2 or blend every 200 msec's freely).

Ok, seems like a good idea, added!

WickedShell · 2017-03-10T03:14:19Z

libraries/AP_GPS/AP_GPS.cpp

@@ -65,7 +80,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
    // @Param: AUTO_SWITCH
    // @DisplayName: Automatic Switchover Setting
    // @Description: Automatic switchover to GPS reporting best lock
-    // @Values: 0:Disabled,1:Enabled
+    // @Values: 0:Disabled,1:UseBest,2:Blend
+    // @RebootRequired: True


If we fix the arming check problem I believe this can no longer be a reboot required flag?

Ok, removed.

This includes these changes: RATE_MS, RATE_MS2 parameter description Range minimum reduced to 50 _blend_health_counter is reset to 0 if blending is disabled GPS_MAX_RECEIVERS is replaced with GPS_BLENDED_INSTANCE where appropriate simplify all_consistent functions check of number of receivers calc_blended_weights fix for initial check of how many receivers we have remove unnecessary setting of GPS last time when blending fails remove RebootRequired from AUTO_SWITCH param description

This silences a warning from eclipse No functional change

Thanks to Michael DuBreuil for suggesting the fix

I think we've cleared all of @WickedShells comments..

rmackay9 and others added 24 commits March 9, 2017 11:25

SITL: add GPS2_GLITCH parameter

06a6a55

AP_HAL_SITL: add glitch for 2nd ublox

f63dcc1

AP_NavEKF2: handle position jump caused by change in GPS receiver

8e59988

If the GPS receiver selection changes and we are using GPS for height, the vertical position will be reset to the new GPS height measurement. correct output observer history when doing a GPS height reset

AP_NavEKF3: handle position jump caused by change in GPS receiver

bb948a2

If the GPS receiver selection changes and we are using GPS for height, the vertical position will be reset to the new GPS height measurement. correct output observer history when doing a GPS height reset

AP_NavEKF2: log correct GPS for replay

5b74307

AP_NavEKF3: log correct GPS for replay

ad24780

AP_NavEKF2: Reference correct derivation

830a4db

AP_NavEKF3: Reference correct derivation

7835869

DataFlash: Allow logging of blended GPS instance

150ee25

Add GPS and GPA reporting for blended solution Allow GPS library to manage reporting for the blended solution

AP_GPS_MAV: set have_accuracy as boolean

d6ad9eb

AP_GPS: remove unused highest_supported_status(void)

7f5d691

AP_GPS: remove unused get_antenna_offsets()

da6b49f

callers always specify an instance

AP_GPS: minor comment fix

12a9d8c

No functional change

AP_GPS: move time_epoch calcs into proper file

d69cefb

The frontend implementation was in the backend file No functional change

AP_GPS: Add option to blend multiple GPS receivers

29544de

Fix pre-existing bug in hard switch logic Update GPS_AUTO_SWITCH description protect against zero accuracy estimates returned by GPS drivers

AP_GPS: sanity check update rate

4d42699

Also minor spelling and parameter documentation fix

AP_GPS: check for valid instance in highest_supported_status

1a066fd

highest_supported_status will always return FIX_3D for blended or invalid instance setHIL_Accuracy checks instance is 2 or less send_mavlink_gps2_raw uses num_instances variable directly to avoid confusion with num_sensors

AP_GPS: move constructor and complex accessors to cpp file

b51a529

These functions are slightly long and make the .h file hard to read. Also saves a small amount of flash space. No functional change

AP_GPS: protect parameters from being accessed externally

58e5971

backends become friends so they can continue to access parameters held in frontend get_rate_ms made private because only used by frontend Also moved static arrays higher in cpp file

AP_GPS: add all_consistent and blend health for use as pre-arm check

7d6ad28

AP_Arming: add gps consistency and blend health check

08df8d2

AP_Arming: make gps-checks virtual

d09a561

Copter's GPS checks will override this

Copter: AP_Arming calls parent's gps checks

ca7260a

Allows removing some duplicate code

Copter: log event when primary GPS changes

d565805

rmackay9 force-pushed the prise-gpsblending3 branch from 16c7759 to d565805 Compare March 9, 2017 05:00

rmackay9 mentioned this pull request Mar 9, 2017

Add EKF support for multiple GPS receivers #5642

Closed

rmackay9 requested review from OXINARF, tridge and WickedShell March 9, 2017 05:06

rmackay9 added the AllVehicles label Mar 9, 2017

rmackay9 added EKF Enhancement GPS labels Mar 9, 2017

rmackay9 added this to the AC 3.5.0 milestone Mar 9, 2017

WickedShell previously requested changes Mar 10, 2017

View reviewed changes

rmackay9 added 2 commits March 10, 2017 15:38

AP_Arming: clarify GPS inconsistency message

6682d70

DataFlash: rename GPS3/GPA3 to GPSB/GPAB

8b47136

rmackay9 force-pushed the prise-gpsblending3 branch 3 times, most recently from c35f0ea to 81ca80f Compare March 10, 2017 07:22

rmackay9 added 5 commits March 10, 2017 17:00

AP_GPS_UBLOX: use get_rate_ms accessor

9cf2297

AP_GPS: more efficient initialisation of blending arrays

5a2c51b

AP_GPS: minor bracketing fix

ebe8040

This silences a warning from eclipse No functional change

AP_GPS: stop blending if too many blending failures

aeb2cf1

rmackay9 force-pushed the prise-gpsblending3 branch from 81ca80f to aeb2cf1 Compare March 10, 2017 08:00

rmackay9 added 3 commits March 10, 2017 17:14

AP_GPS: rename pre-arm blending health check

a94175f

AP_Arming: GPS blending health check renamed

652d64b

AP_GPS: fix blending when accuracy reported as 1mm

8efd50a

Thanks to Michael DuBreuil for suggesting the fix

rmackay9 merged commit 3e628f3 into ArduPilot:master Mar 13, 2017

WickedShell mentioned this pull request Mar 20, 2017

GPS: dual GPS switches between each other too easily #3754

Closed

rmackay9 deleted the prise-gpsblending3 branch April 3, 2020 01:24

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

GPS Blending (attempt2) #5841

GPS Blending (attempt2) #5841

rmackay9 commented Mar 9, 2017 •

edited

Loading

WickedShell left a comment

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017 •

edited

Loading

WickedShell Mar 10, 2017

WickedShell Mar 10, 2017 •

edited

Loading

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

WickedShell Mar 10, 2017

rmackay9 Mar 10, 2017

GPS Blending (attempt2) #5841

GPS Blending (attempt2) #5841

Conversation

rmackay9 commented Mar 9, 2017 • edited Loading

WickedShell 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

rmackay9 Mar 10, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

WickedShell Mar 10, 2017 • 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

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

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rmackay9 commented Mar 9, 2017 •

edited

Loading

rmackay9 Mar 10, 2017 •

edited

Loading

WickedShell Mar 10, 2017 •

edited

Loading