Surface tracking in ArduSub (SURFTRAK mode)

[clydemcqueen]

We propose a surface (seafloor) tracking feature for ArduSub inspired by the surface tracking feature in ArduCopter. Surface tracking in ArduSub requires a down-facing rangefinder (e.g., the Blue Robotics Ping Sonar) and is activated by selecting the new SURFTRAK flight mode.

Surface tracking has been tested in SITL and Gazebo Garden, see the results.

I'm marking this 'draft' to begin the conversation with the maintainers.

[peterbarker]

I'm a bit confused by the addition of a mode.

You've looked at how Copter does this, with surface-tracking another way of controlling alt across several different control regimes?

[Williangalvani]

I'm a bit confused by the addition of a mode.

You've looked at how Copter does this, with surface-tracking another way of controlling alt across several different control regimes?

That was my suggestion. I really dislike the idea of having "sub-modes" not easily traceable from the GCS side (is it traceable? I haven't tried copter/place in a long time).

Sub also doesn't (yet) support Aux Functions unless I missed something (which I often do).

Not having "persistent" buttons in most controllers also makes things harder to track.

[zhrandell]

Hello all! Just jumping in briefly to share that in using our BlueROV2 to conduct standardized kelp forest video surveys, there are numerous instances where we'd want to maintain depth hold but not engage RNG_HOLD. This is partially because our BlueROV2 is slightly positively buoyant, thus we keep depth hold engaged regularly in the course of our routine operations. We would not however want depth hold to be exclusively tied to RNG_HOLD, as motoring from A to B (i.e., not surveying) across varying rocky reef topography could result in undesirable ROV altitude adjustments.

[Williangalvani]

Hi @clydemcqueen , I haven't gotten to test this yet, I'll try to do this in the next couple of days, sorry about the delay!

[ES-Alexander]

Are there plans to expand this to maintaining a range offset in an arbitrary direction? If not it seems a bit unintuitive for it to be called RNG_HOLD.

If the intent is for it to only be a vertical controller then it would seem to make more sense to call it TERRAIN_HOLD or similar, which I think is also more in line with ArduPilot's existing terminology.

I really dislike the idea of having "sub-modes" not easily traceable from the GCS side

@Williangalvani fair enough on the GCS traceability front. That said, a limited (e.g. single-axis) controller is fundamentally a sub-mode. Copter's Surface Tracking can apparently be used to replace the z controller in multiple different flight modes, which intuitively makes sense.

Perhaps 6-DoF vehicles should have independent horizontal and vertical flight modes or something, to increase flexibility and intuitive understanding of the vehicle state, while also reducing code redundancy...

[Williangalvani]

@ES-Alexander fair points. my goals are to refactor things so we can have a new "advanced" mode, where one would be able to enable/disable controllers independently (bitmask maybe?). but that will come after #23444 (which I couldn't test yet due to a leak) and some subsequent ones I'll need to decouple controllers.

[clydemcqueen]

Hi, all -- thanks for the early feedback! I am just starting to do hardware testing myself; I'll post results when I have something useful.

@Williangalvani the mode refactor looks very good. I can imagine something like class ModeRangeHold : public ModeAlthold -- that would simplify things.

I'm a fan of buttons that map to "[sub]mode X" rather than "toggle behavior X" because when the mission gets wonky there is little risk in hitting the "[sub]mode X" button one extra time for good measure.

I don't have any great ideas on the terminology. 'Surface tracking' sounds a bit funny for an underwater vehicle, but it does match terminology in copter. 'Terrain hold' might be confused with copter's 'terrain following', which requires a terrain database. I defer to the maintainers.

I'm going to rebase this periodically to keep tracking HEAD while testing continues.

[ES-Alexander]

I don't have any great ideas on the terminology. 'Surface tracking' sounds a bit funny for an underwater vehicle, but it does match terminology in copter. 'Terrain hold' might be confused with copter's 'terrain following', which requires a terrain database.

From the Copter Terrain Following docs it seems like using a downward facing lidar or sonar is also supported - not just with a pre-existing database.

[clydemcqueen]

I ran this a few times on a BlueROV2 and a Ping1 sonar rangefinder. In general it works, but I'd like to do some work to reject bad sonar readings, and to improve the pilot experience. See my notes here: clydemcqueen/ardusub_surftrak#1

[clydemcqueen]

I rebased (refactored) to use the new Sub modes; it is simpler and smaller.

I've run this in SITL and on hardware and it works nicely.

One callout is the change to AP_RangeFinder_MAVLink.cpp to drop DISTANCE_SENSOR msgs with low signal_quality. This turns out to be important for the Ping1 sonar, and possibly other sonar devices, particularly when running close to the minimum distance.

[clydemcqueen]

I am going to refactor the commits to call out the subsystems:

• Sub: add RNG_HOLD mode
• AP_JSButton: add RNG_HOLD mode
• AP_RangeFinder: ignore readings where 0 < signal_quality < 90

[clydemcqueen]

I removed the AP_RangeFinder commit at Willian's suggestion, and rebased.

[clydemcqueen]

This PR has been substantially rewritten.

The biggest change was to remove the RNG_HOLD PID controller. I used this controller to deal with long (~800ms) sensor delays, but I found that it wasn't necessary for faster sensors (~300ms delay). I was also motivated to find a solution that would also work for AUTO and GUIDED modes, since these are important use cases.

(Currently, sub doesn't support mission items with frame=ABOVE_TERRAIN, which will be necessary for AUTO and GUIDED modes. The change is pretty small, and can be handled in a different PR. See #26450.) EDIT: reference merged ABOVE_TERRAIN PR

With that in mind, I spent some time trying to tune RNG_HOLD using existing parameters, and I found that I could get good results by setting PILOT_ACCEL_Z and PSC_JERK_Z so that KPv is < 1.0. For example, PILOT_ACCEL_Z=500 cm/s/s, and PSC_JERK_Z=8.0 m/s/s/s works well. I am open to other ideas!

ardupilot/libraries/AP_Math/control.cpp

Line 301 in 15cea77

KPv = 0.5 * jerk_max / accel_max;

Tested hardware: (1) BlueROV2 heavy with a Ping1 sonar; (2) BlueROV2 heavy with a payload skid and a Water Linked DVL. This code is currently running on both, and I expect to see a lot more field testing over the next few weeks.

@Williangalvani Can you take another look?

[vshie]

I'll give this a try with Cerulean Tracker 650 and report back as rangefinder source and report back as well! Thanks @Williangalvani !

[Williangalvani]

Worked well here. the place I tested wasnt super uneven, but it keep altitude despite the terrain:

(focus on the second section, as I was giving throttle inputs before that.

[Williangalvani]

This is a great addition, thanks @clydemcqueen!

I tested it and I'm happy with how it worked. As this only touches Sub code, I'm merging it.