Send velocity and angular velocity command dynamically

Our thrust vs input signal curves likely do not hold precisely for thrust at speed.

If you’re targeting a specific desired velocity, and you have an estimate of current velocity and a method to tell the vehicle to move faster or slower, you can implement whichever control algorithm you like to control the vehicle commands based on the “error” between the desired and current velocity.

Depending on your precision and transfer time requirements different approaches may or may not be sufficient for your purpose.

A reasonably simple approach is to use just proportional control, where if the error is large you apply a large control action (e.g. if you are much slower than desired you significantly increase the commanded throttle), and if the error is small you apply a small (or zero) control action (change the commanded throttle by a small amount, or leave it as is). The amount to change by is determined by the error multiplied by a scaling factor, which is a parameter of the algorithm that you can tune.

If you need more advanced control you can use a full PID controller (proportional, integral, derivative), which is still reasonably simple (and will have many implementations available online), but has some additional tuning parameters that help to dampen oscillations and correct for long duration small errors.

Beyond that you could potentially go to a more advanced controller, but if that’s deemed to be necessary then it’s almost certainly worth implementing the control within ArduSub instead, since it has access to much higher sensor measurement frequencies, and can apply control actions with lower latency.