GCS_MAVLink: Allow setting global GPS origin multiple times

[ES-Alexander]

Feature request

Is your feature request related to a problem? Please describe. Sub users with DVL sensors have complained multiple times about needing to restart their vehicle in order to set the GPS origin multiple times. GCS_Common.cpp uses set_ekf_origin, which only allows setting the origin once. While that kind of restriction may make sense for automatic origin sets that occur from elsewhere within the codebase, it seems unnecessarily restrictive as the only available behaviour, especially when handling an explicit command from the user.

Describe the solution you'd like Allow the user to set the GPS origin multiple times, without restriction. If set_ekf_origin is used elsewhere in the codebase that could be handled via a 'force' flag that's set to false by default, and true in the MAVLink message handling case, but if not then that code could just be removed.

Describe alternatives you've considered

• A 'force' flag could instead be added to the MAVLink message definition, but that seems unnecessary
• Things could stay the way they are, at the cost of people continuing to get frustrated by the issue

Platform [x] All [ ] AntennaTracker [ ] Copter [ ] Plane [ ] Rover [ ] Submarine

Additional context Mentioned in this forum thread

[rmackay9]

@ES-Alexander,

I wonder if perhaps the issue might be that the DVL sensor (underwater GPS right?) is integrated as a GPS instead of a AP_VisualOdometry or AP_Beacon library. If integrated using one of these other two options we can handle position offsets and rotation differences.

[ES-Alexander]

@rmackay9

I wonder if perhaps the issue might be that the DVL sensor (underwater GPS right?) is integrated as a GPS instead of a AP_VisualOdometry or AP_Beacon library.

Nah, this is an actual DVL (underwater GPS is position based, DVL measures velocity relative to the ground). The integration is using visual odometry messages.

Should it be using something other than SET_GPS_GLOBAL_ORIGIN for setting the origin/starting point?

[rmackay9]

@ES-Alexander,

OK, great that it's using the visual odometry messages. So the issue is that you'd like to occasionally reset where the vehicle is? Perhaps you're pulling it out of the water and you know that it is at a certain location and would like to manually set it to this location? In which case I think we might need a different method rather than moving the origin.

By the way, if the vehicle had a GPS on it as well you could switch it between GPS and the DVL (or that could be done automatically with a Lua script somehow). Whenever it used the GPS it would reset to its actual position.

[ES-Alexander]

@rmackay9,

So the issue is that you'd like to occasionally reset where the vehicle is? Perhaps you're pulling it out of the water and you know that it is at a certain location and would like to manually set it to this location?

I don't actually have a DVL - I work at Blue Robotics and this is something that has been a pain point for some of our users. That does seem like the kind of use-case that would warrant resetting the GPS location, so sure :-)

Given that framing, perhaps our interface could provide an option to set the origin (once), and then send GPS position messages for subsequent location sets. That would likely play more nicely with the existing ArduPilot codebase, although I'm not sure about the philosophical consistency.

1. Primarily, what is the GPS origin actually supposed to be (why should it not be settable multiple times)
2. Beyond that, is it possible to send GPS messages without convincing the autopilot that there's a GPS connected? I imagine there would be issues with "GPS lock lost" warnings and the like if the autopilot only ever received intermittent GPS position messages sent directly from user input, and may also not trust a single GPS message, or GPS messages in general if no GPS was set up in the parameters.

if the vehicle had a GPS on it as well you could switch it between GPS and the DVL (or that could be done automatically with a Lua script somehow). Whenever it used the GPS it would reset to its actual position.

Indeed, GPS+DVL integration would be useful, and is something we have yet to implement.

For a satellite-based GPS that would require something like #10845 to be done first, in which case the GPS could be the primary sensor, which starts being ignored (and falls back to the DVL if there is one) when the vehicle starts to dive. That's been discussed somewhat here, and is on @Williangalvani's "long term to do" list. For an in-water GPS/positioning system both data streams (GPS + VISO) could potentially be sent at the same time, with the EKF doing the combining / deciding what to trust more - I'm unsure what changes (if any) would be required to support that, although it would likely at least require some stability testing to test the soundness of the EKF's judgements.

Alternatively the readings can be combined externally (like Cerulean Sonar do), where the combined position output can be sent to the autopilot as a single value. That's simpler for the EKF to deal with, but requires having another independent EKF so would be redundant if that functionality was implemented well in ArduPilot firmwares :-)

[rmackay9]

@ES-Alexander,

So the "GPS origin" is actually the EKF origin. Internally the EKF uses 3D vectors to estimate the position, velocity, etc and the position vector is an offset from the EKF origin. This position offset vector is also exposed to external consumers including the position controllers so if we were to move the EKF origin then we also need to be sure the consumers notice and handle it. Up until now the API has guaranteed that the EKF origin doesn't move once set so there would likely be a number of changes required in at least the position and navigation controllers.

The idea of sending a "GPS Location" is probably close to the right solution except that instead of sending a "GPS" location, a relative position sent via the VISION_POSITION_ESTIMATE (or GLOBAL_VISION_POSITION_ESTIMATE or ATT_POS_MOCAP) would be better. Some of these messages include a reset field so if that is incremented the vehicle will immediately jump to the new position or location (by the way we normally say "position" to mean a 3D offset from EKF origin and Location to mean (lat, lon, alt)).

Re transitions, as long as we properly feed the DVL into the EKF as either an external vision system or a beacon it should be relatively easy to get the transitions working within AP (no need for an external system).

BTW, don't get hung-up thinking that a DVL needs to be somehow made to look like a GPS. AP handles lots of different Non-GPS systems so let's find the one that fits best.

[Williangalvani]

Expanding on this.

I'm working on our companion driver for the this DVL. The integration is mainly through VISION_POSITION_DELTA messages, but we need an origin for ardupilot so it outputs usable location data (position shown in QGC).

One of the features in our integration is a world map where the user can set the current location (origin?) of the vehicle. for smaller distances I could calculate the xyx offset from origin and send a VISION_POSITION_ESTIMATE message to "force" the vehicle to a new location.

Would this work? is there an easy way to translate latlng to xy somewhere in the code?

If the user clicked the wrong hemisphere my mistake, would the ekf deal with a huge offset? would it even work? I'm concerned about the cartesian coordinates systems and globe not playing along.

As a last resort (depending on the answers to the questions above), we could allow setting the origin multiple times only on sub, and enforcing that the vehicle is disarmed when we do it.

[rmackay9]

@Williangalvani,

a world map where the user can set the current location (origin?)

For this part, "yes", I think the feature could be implemented by letting the user set the EKF origin the first time

is there an easy way to translate latlng to xy somewhere in the code?

AP_Common/Location has a conversion. I suspect the AHRS and EKF classes also have this

during the "Extreme Autopilot" project we tested having the vehicle very far from the EKF origin and I think it all works now. We need this to be able to fly around the world or float across the ocean so it should work.

I really don't think we should allow setting the EKF origin twice. It's a bit of a fundamental rule and while it might seem like the most direct solution to this particular problem, so far I don't see a reason for it and I'm pretty sure it will create far more problems than it solves.

[Williangalvani]

@rmackay9 That worked out well.

I had a misconception that the xyz frame was actually "square", which was confusing me. I now have the code working properly using POSITION_ESTIMATE messages, thank you!

I'm closing this as it is not longer necessary.