Sub: support ABOVE_TERRAIN frame in GUIDED mode posvel case

[clydemcqueen]

Sub Guided_PosVel mode currently assumes Location::AltFrame::ABOVE_ORIGIN. This PR adds support for Location::AltFrame::ABOVE_TERRAIN.

This new functionality is exposed two ways:

• Using MAVLink: SET_TARGET_POSITION_GLOBAL_INT.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT_INT
• Using Lua: set_target_posvel_terrain(Vector3f, Vector3f)

The motivation is to allow the development of scripts that drive the ROV at a constant xy velocity and at a constant height above the seafloor, while giving the pilot control over heading. There's a new autotest that demonstrates this.

Possible open issues / future PRs:

• Is it better to add an optional frame parameter to the set_target_posvel_NED(Vector3f, Vector3f) Lua binding?
• Sub Guided_WP mode could also support Location::AltFrame::ABOVE_TERRAIN, though this mode does not support rapid target updates

[timtuxworth]

Very interesting I'm working on something similar for Plane. Here's my draft PR for plane #27909

[clydemcqueen]

@timtuxworth Thanks for reaching out! I will read your PR and try to stay in sync where I can.

My use case doesn't depend on a GCS, instead I am using a Lua script to send targets at 10Hz. This allows the sub pilot to control heading while GUIDED mode handles the xyz motion. Here is the script that I am testing: https://github.com/clydemcqueen/ardusub_surftrak/blob/guided_above_terrain/lua/transect.lua

[timtuxworth]

Have you thought about making this a Location::AltFrame instead of uint8_t?

[clydemcqueen]

Hmmm, will all compilers map Location::AltFrame to uint8_t? I don't see this usage anywhere else in the code.

[timtuxworth]

I've used a static cast - oh I see what you've done. Makes sense.

[clydemcqueen]

We tested this w/ an A50 DVL off Seattle's Pier 62 on 21-Aug-2024 with good success. Learnings:

• We forgot to set WPNAV_ACCEL_Z to to match PILOT_ACCEL_Z, so we had some strong up/down oscillation due to sonar delay. (SURFTRAK performed as expected since it uses PILOT_ACCEL_Z).
• We learned that the lower limit for WPNAV_SPEED is 20 cm/s, vs the desired 10 cm/s. This has now been changed in master.
• The script tried to set the target position even when the rangefinder was unhealthy, resulting in a bunch of warnings in the GCS. (It looks like mavlink-router drops some messages, so the rangefinder is unhealthy more often than I expected, see ArduSub timeouts when rangefinder value > 2.5m bluerobotics/BlueOS-Water-Linked-DVL#44.) I updated the script to avoid generating warnings.

We will test again in a few weeks.

[clydemcqueen]

We tested this on a different ROV with an A50 DVL off a vessel in Elliott Bay. I still saw vertical oscillations in guided mode compared to surftrak. I tracked this down how ModeGuided::guided_set_destination_posvel was calling position_control->set_pos_offset_z_cm. I changed the code to distinguish between the first call and subsequent calls.

We will test this again in the coming weeks.