-
Notifications
You must be signed in to change notification settings - Fork 17.2k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Add EKF support for multiple GPS receivers #5642
Conversation
Thanks Paul |
I'm currently investigating why disconnecting GPS2 eventually causes GPS1 data to stop updating. |
b160222
to
828f5a5
Compare
I'd definitely like to get this into AC3.5 |
@priseborough, this has a WIP label on it but maybe that's out of date? Are you happy for this to go into master now? |
Was this reviewed by anyone? @WickedShell? @rmackay9 By the way I don't think we should be including new things in the middle of beta testing 3.5. For 3.5.1 it would be fine, but putting new things now just delays more the final 3.5 release. |
@rmackay9 I've been flying it regularly so am happy for it to go in if it has been reviewed so it can get some testing with different hardware. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A couple of questions, but there is some bad logging happening that at a minimum must be corrected, as well as some docs errors.
I need to reread the final steps of blending still, as I went a bit glassy eyed towards the end. I can kind of see why you tacked on an extra entry to the state/status arrays but it also seems to complicate the setup. I'm wondering if a _blended_state() would be more useful in the end rather then having to special case all the indexing for which GPS to utilize.
libraries/AP_GPS/AP_GPS.cpp
Outdated
// @Param: BLEND_MASK | ||
// @DisplayName: Multi GPS Blending Mask | ||
// @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 3. | ||
// @Values: 5:Default |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Values and bitmask shouldn't be used in the same parameter documentation, in this case Values should be removed.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
will fix
} | ||
if (state[i].status > state[primary_instance].status) { | ||
// we have a higher status lock, change GPS | ||
primary_instance = i; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is not really a new fault, but the last_instance_swap_ms should be updated here.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
will fix
min_ms = state[i].last_gps_time_ms; | ||
} | ||
if (_rate_ms[i] > max_rate_ms) { | ||
max_rate_ms = _rate_ms[i]; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Does it matter that the _rate_ms is only the expected update rate? I primarily ask as the only people who will have this correct will be u-blox users, no other driver uses it, which means that every other driver is just configured to the rate it is hard coded to request or it receives (IE you can get 10Hz out of SBF, but the parameter won't reflect that unless a user realizes they should make it match.).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It does matter if the GPS is slower than the parameter - eg the parameter was set to 100ms and the GPs was updating at 200. It would likely result in the data from that GPS being ignored by the blending algorithm due to failing the data delayed check at line 1001.
Given that the default parameter value is 200msec, I don't think this is a problem as we would not want to use a GPS receiver that outputted slower than that anyway. If a user sets the parameter to a different value, then they are responsible for checking that their GPS is capable of that rate.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@priseborough So my concern on this actually is that there are drivers right now where the driver requests 10Hz updates regardless of the parameter, and no user knows that they should be setting this parameter. I guess we could have some of those drivers instead call the set parameter to override to 10Hz which might work, but this is a very weak to incorrect assumption for any driver other then u-blox. (Indeed I was actually planning to hide this parameter inside the ublox layer with the parameter hiding stuff @tridge was talking about).
Would instead tracking the update time between instances in the AP_GPS layer as an actual calculated number be fine for your use case? One of the last things with #2879 is to actually track this in the AP_GPS layer so we can publish if it's healthy or not. We could track this as a delta update time that you could look into. If that's fine I can get that sent in relatively soon.
libraries/AP_GPS/AP_GPS.cpp
Outdated
if (_blend_mask & USE_SPD_ACC) { | ||
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { | ||
if (state[i].status >= GPS_OK_FIX_3D) { | ||
if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why check for speed_accuracy > 0.0f? Isn't that already assured via the have_speed_accuracy flag? It's a safe check, but I just don't see why you need it.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
will fix
@@ -783,6 +783,8 @@ 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), \ | |||
"GPS3", "QBIHBcLLefffB", "TimeUS,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,U" }, \ | |||
{ LOG_GPA_MSG, sizeof(log_GPA), \ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You need a GPA3, or you will be writing the blended GPS accuracies to IMU log entries.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
will fix
libraries/AP_GPS/AP_GPS.h
Outdated
@@ -349,12 +361,14 @@ class AP_GPS | |||
AP_Int8 _sbas_mode; | |||
AP_Int8 _min_elevation; | |||
AP_Int8 _raw_data; | |||
AP_Int8 _gnss_mode[2]; | |||
AP_Int16 _rate_ms[2]; | |||
AP_Int8 _gnss_mode[GPS_MAX_INSTANCES]; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is a user facing control parameter, I don't see any value to the blending to be duplicating this.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This doesn't add a parameter because GPS_MAX_INSTANCES = 2
libraries/AP_GPS/AP_GPS.h
Outdated
AP_Int8 _gnss_mode[2]; | ||
AP_Int16 _rate_ms[2]; | ||
AP_Int8 _gnss_mode[GPS_MAX_INSTANCES]; | ||
AP_Int16 _rate_ms[GPS_MAX_INSTANCES]; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Same deal, why have an extra parameter that doesn't appear to be utilized.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This doesn't add a parameter because GPS_MAX_INSTANCES = 2
libraries/DataFlash/LogFile.cpp
Outdated
const struct Location &loc = gps.location(i); | ||
struct Location loc; | ||
if (i >= gps.num_sensors()) { | ||
loc = gps.location(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This seems really messy. Why not have gps.location(3) manage the blended instance directly within the GPS library.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
will fix
Correct parameter documentation Fix pre-existing bug in hard switch logic Remove unnecessary speed accuracy value check
e96490d
to
44cad05
Compare
Here are the plots for the physical (GPS and GPS2) and the virtual blended GPS3 receiver from todays testing of the updates. The accuracy reported for the blended GPS is the better of the two receivers as expected. The reported position and altitude of the blended receiver more closely follows the better receiver as expected |
Randy, addressing your points in order:
|
Watching this closely. Wanted to ask if this will have potential consequences for those of us testing/using RTK GNSS as one of two GNSS in a dual setup? I am assuming if we want to make sure we are using best absolute position - assuming RTK is working and becomes primary during flight - that we should leave the parameter GPS_AUTO_SWITCH = 1 to use the original non blended mode/code? I was not clear on how much impact RTK may have on the weighting. I know there have been other issues raised for tracking RTK status but not sure where they stand, or if they ave impact on this issue. Thanks. |
Just wondering, why not add two GPS sources to observation vector and integrate them both inherently in EKF? |
@cglusky If the RTK receiver reports speed and/or position accuracy, then the much smaller values reported will make the weighting go to close to 100% for the RTK receiver when operating in RTK mode. Check the GPA message logged for the RTK receiver and see if there are non-zero HAcc and/or SAcc values. if so, then the blending algorithm can be used. |
@EShamaev That requires adding additional states to the GPS to track the offset between receivers and additional switching logic complexity. It would also have to be added to both EKF2 and EKF3. This would blow the flash budget, increase implementation and maintenance cost and potentially add bugs to the EKF's for a feature that only benefits a small percentage of users. |
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.
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.
Looks like we're out of flash space on Pixhawk (again) |
@priseborough If someone would merge: #5810 you should be fine... |
Out of space on fmuv3 or v2, an important distinction. |
Just merged @WickedShell's fix to reduce flash consumption by chopping solo gimbal. It's a straightforward fix and no Solo's require the -v2 firmware (i.e. the cube doesn't suffer from the 1MB flash limit). We just need to make sure that people know to load the -v3 firmware when using solo. |
Awesome! |
I've re-tested this with the fixes from @priseborough and it looks good to me now. So I raised 5 points above and the results are:
I've got one extra small patch that I'd like included in the PR (rmackay9@1c96d66). |
Just found this thread and following with interest since I was asking myself the same questions for fusing 3 GPS with an EKF to get a more accurate position. Quick bonus question, would it be possible to force the GPS MAV arming/home position to a known position as a parameter and then calculate offset from this position to correct the reading from the live GPS data ? ( say we have an external very precise RTK position and we are always taking off from the exact same point every time) ? I guess it would be the same strategy using GPS offset as you are doing for the blended virtual GPS ? |
@Kiwa21 The algorithm that is used for the blending has been designed to support more than 2 GPS units by increasing the value of the ��GPS_MAX_RECEIVERS #define, although other parts of the GPS library would need to be modified to accomodate the additional receiver(s). Telling the vehicle where it is accurately at the start of the flight is pointless unless the onboard GPS receivers have an equivalent accuracy, because the positional error offset of the receivers is not constant during flight. If there is an RTK receiver on-board, then the exisiting code is already capable of using that to perform precision recovery. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think this isn't ready for master yet. Besides my inline comments, the update rate issue brought up by @WickedShell hasn't been fixed yet and it was deemed important by @priseborough.
The changes to the EKF look good though (I made a question there, but nothing really important), so maybe we should export them into a new PR that we could merge now - those changes already make our current switching algorithm better by allowing not only the EKF but the controllers know about a position change.
|
||
// @Param: BLEND_MASK | ||
// @DisplayName: Multi GPS Blending Mask | ||
// @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 3. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
by setting GPS_AUTO_SWITCH to 3 -> by setting GPS_AUTO_SWITCH to 2
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I've fix the BLEND_MASK parameter description as well in my latest commits.
@@ -65,7 +65,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:Hard,2:Soft |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think the description and values don't do a good job explaining what it does, we could improve here.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
OK, i'm fixing up the AUTO_SWITCH parameter description
@@ -65,7 +65,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:Hard,2:Soft | |||
// @RebootRequired: True | |||
// @User: Advanced | |||
AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1), |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If we are leaving the old method then we should change this default - that method isn't very good and if we have a better one we should use it as default.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@OXINARF unfortunately thats a bit orthogonal to the concept of bringing in the behavior disabled until confidence is gained on it across the board. Unless we are that confident in this?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
True, but after this is ready for master, I'm more confident in this method than the previous one but maybe that's just me. I'm fine with putting it to disabled by default too - it's just leaving it at the old method by default doesn't look good to me.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think Tridge and others will be happier if we leave the existing "UseBest" logic (that's what I'm calling it) for a while until the "Blending" has had more testing by use and beta testers.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ok, if that's what the majority prefers. My issue is that UseBest doesn't always uses the best one (and that's one of the reasons we've recommended against using it).
@@ -226,6 +242,19 @@ void AP_GPS::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_man | |||
_port[0] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 0); | |||
_port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1); | |||
_last_instance_swap_ms = 0; | |||
|
|||
// Initialise class variables used to do GPS blending | |||
memset(&_NE_pos_offset_m, 0, sizeof(_NE_pos_offset_m)); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is initializing to 0, which these variables already have on init. The only one that needs init is _omega_lpf - having these doesn't hurt but increases flash size.
@@ -28,7 +28,8 @@ | |||
maximum number of GPS instances available on this platform. If more | |||
than 1 then redundant sensors may be available | |||
*/ | |||
#define GPS_MAX_INSTANCES 2 | |||
#define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not additional 'virtual' GPS created by blending receiver data |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
does not additional 'virtual' -> does not include additional 'virtual'
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
fixed in the updated PR
} | ||
} | ||
timing[GPS_MAX_RECEIVERS].last_fix_time_ms = (uint32_t)temp_time_1; | ||
timing[GPS_MAX_RECEIVERS].last_message_time_ms = (uint32_t)temp_time_2; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If we are blending these here there's no need to be setting them to the newest value above.
AP_HAL::UARTDriver *_port[GPS_MAX_INSTANCES]; | ||
// Note allowance for an additional instance to contain blended data | ||
GPS_timing timing[GPS_MAX_RECEIVERS+1]; | ||
GPS_State state[GPS_MAX_RECEIVERS+1]; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Any reason not to use GPS_MAX_INSTANCES?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think maybe it's clearer to have the +1 there? Maybe not, I don't have a strong preference either way.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I find the +1 more clearer too, but then I think we should just scrape the GPS_MAX_INSTANCES entirely (or even better, use it instead of creating a new GPS_MAX_RECEIVERS). The vehicles should really use the num_sensors method instead of relying on using the macro anyway.
// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm | ||
#define USE_HPOS_ACC 1 | ||
#define USE_VPOS_ACC 2 | ||
#define USE_SPD_ACC 4 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nitpicking: these should be in cpp file to reduce scope.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
ok, moving them.
// Don't fuse velocity data if GPS doesn't support it | ||
if (frontend->_fusionModeGPS <= 1) { | ||
fuseVelData = true; | ||
} else { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why was this moved? Don't we want to fuse (or stop fusing) velocity data immediately?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'll leave this to @priseborough to answer.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We still want to calculate a offset corrected GPS velocity measurement for other purposes, eg in-flight state resets, even if we aren't fusing the data, hence the changed order.
if (_blend_mask & USE_HPOS_ACC) { | ||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) { | ||
if (state[i].status >= GPS_OK_FIX_2D) { | ||
if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Check for bigger than 0 was removed from speed but not from vertical and horizontal accuracy.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@priseborough wants to check that the accuracy numbers are non-zero so I'm adding the check back in for speed-accuracy. As far as I can see there's no where else that we specifically check that they are non-zero.
I forgot one thing in my review: all methods that receive an instance number need to be re-checked. At least get_lag looks like it won't work as it is accessing wrong memory positions. |
Well, I've done something which addresses many of the issues found by @OXINARF and @WickedShell but not quite all of them. The commit history is a bit messy though so perhaps @priseborough and I should do another review tomorrow before we ask anyone to look at it again. I've lightly tested it and it doesn't seem broken. |
@rmackay9 It would be great if history could be a bit squashed, it's easier to check individual commits, but not when there are several fixup commits. |
@priseborough and I did some more work on this today and have created a new PR: #5841 I'll close this one for now. Thanks for the reviews! |
Problem
The existing logic used to switch between GPS receivers can cause large unpredictable jumps in EKF position if dual GPs receivers are fitted and GPS_AUTO_SWITCH is set to 1.
Solution
Extend the GPS_AUTO_SWITCH to type 2 which is a soft switching/blending method using a weighted average of physical receivers to create a third 'virtual' GPS where:
The existing hard switch logic has been left untouched and is selected with GPS_AUTO_SWITCH = 1 the same as before.
Logging has been updated to to log the blended GPS instance if it is being calculated.
Status
This is a draft for comment. it has had limited flight and bench testing.
TODO
Improve the legacy hard switch logic to use solution status and reported accuracy (if available) rather than number of satellites. Using the number of satellites doesn't work as a metric where there are mixed receiver models (eg GPS+GLONASS and GPS only).
Bench testing of various GPS failure modes - signal fade, loss of physical connection.