Skip to content
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

Surface tracking in ArduSub (SURFTRAK mode) #23435

Merged
merged 3 commits into from
Feb 21, 2024

Conversation

clydemcqueen
Copy link
Contributor

@clydemcqueen clydemcqueen commented Apr 9, 2023

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
Copy link
Contributor

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
Copy link
Contributor

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
Copy link

zhrandell commented Apr 11, 2023

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
Copy link
Contributor

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
Copy link
Contributor

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
Copy link
Contributor

@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
Copy link
Contributor Author

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
Copy link
Contributor

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
Copy link
Contributor Author

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 clydemcqueen marked this pull request as ready for review July 18, 2023 20:26
@clydemcqueen
Copy link
Contributor Author

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
Copy link
Contributor Author

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
Copy link
Contributor Author

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

@clydemcqueen
Copy link
Contributor Author

clydemcqueen commented Jan 17, 2024

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!

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
Copy link

vshie commented Feb 6, 2024

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

@Williangalvani
Copy link
Contributor

Worked well here. the place I tested wasnt super uneven, but it keep altitude despite the terrain:
image
(focus on the second section, as I was giving throttle inputs before that.

Copy link
Contributor

@Williangalvani Williangalvani left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

@Williangalvani Williangalvani merged commit f9db039 into ArduPilot:master Feb 21, 2024
93 checks passed
@clydemcqueen clydemcqueen changed the title Surface tracking in ArduSub (RNG_HOLD mode) Surface tracking in ArduSub (SURFTRAK mode) Feb 21, 2024
@clydemcqueen clydemcqueen deleted the clyde_surftrak branch March 8, 2024 17:24
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

6 participants