Sub: limit poshold xy velocity to PILOT_SPEED to avoid bounceback #28205
Conversation
ArduSub/mode_poshold.cpp
Outdated
| } | ||
|
|
||
| // behavior is similar to Sub::get_pilot_desired_climb_rate | ||
| float Sub::get_pilot_desired_horizontal_rate(RC_Channel *channel) const |
There was a problem hiding this comment.
wouldn't it make sense to keep it alongside get_pilot_desired_climbrate() in attitude.cpp ?
There was a problem hiding this comment.
Good point, I'll move it.
clydemcqueen
force-pushed
the
sub_poshold_bounce
branch
from
d923fce to
35ef428
Compare
| @@ -406,6 +403,7 @@ class Sub : public AP_Vehicle { | |||
| float get_roi_yaw(); | |||
| float get_look_ahead_yaw(); | |||
| float get_pilot_desired_climb_rate(float throttle_control); | |||
| float get_pilot_desired_horizontal_rate(RC_Channel *channel) const; | |||
There was a problem hiding this comment.
not a blocker of course but personally I'd stick with the word "speed". introducing a synonym, "horizontal rate" just adds confusion
| position_control->update_xy_controller(); | ||
| } | ||
|
|
||
| motors.set_forward(forward_out); |
There was a problem hiding this comment.
This looks like a good change to me because previously the pilot input was essentially added on top of the position controller output. That would lead to the position controller fighting the user which is terrible and a pattern we try to avoid.
The change in behaviour for users is that they won't be able to control the vehicle laterally in poshold mode anymore unless the EKF has a good position estimate.
Still, looks like an improvement!
There was a problem hiding this comment.
The change in behaviour for users is that they won't be able to control the vehicle laterally in poshold mode anymore unless the EKF has a good position estimate.
That's an interesting point. what if we move body_rates_cm_s up one level and apply that to thrusters if posititon_ok() is false?
Sensors are not perfect and I think users should keep control when they fail.
There was a problem hiding this comment.
That's an interesting point. what if we move
body_rates_cm_sup one level and apply that to thrusters ifposititon_ok()is false?
Ah, good catch, and good idea. Let me play around with this a bit.
|
tested and working well on my pool. but I'm concerned about more challenging conditions for DVLs |
Thanks for testing! It is good to know that the pilot experience feels good when the sensor is working. I'll work on the sensor failure case a bit. |
|
In other vehicles the way we handle loss of position estimate is to change the flight mode. So when the EKF failsafe triggers, the vehicle can be switched to Land, AltHold, etc. We don't expect a mode to be controlled both with and without a position sensor. |
clydemcqueen
force-pushed
the
sub_poshold_bounce
branch
from
35ef428 to
1950941
Compare
|
The code now falls back to manual control (limited by PILOT_SPEED) if positioning fails. I tested this in SITL The position estimates can get a little wonky just before the EKF failsafe triggers, but this is the current behavior. All-in-all I think that this is an improvement over the current situation. LMK what you think. |
|
It's up to you guys of course. Rover's Acro mode for skid steering vehicle also does this kind of thing where the throttle is completely manual if the vehicle doesn't have a good position estimate, otherwise it runs the forward-back speed controller. |
clydemcqueen
force-pushed
the
sub_poshold_bounce
branch
from
1950941 to
943ed3b
Compare
clydemcqueen
force-pushed
the
sub_poshold_bounce
branch
from
943ed3b to
85f9179
Compare
|
I tested this on a DVL-rigged ROV last week and it worked well. The A50 is rock solid in all of the situations we dive in, so I didn't see the failsafe kick in. LMK if you'd like to see additional changes. Thanks. |
This PR limits horizontal velocity in poshold mode, even when the pilot is using the forward and lateral sticks to reposition the sub. This eliminates the possibility of a PSC-caused bounceback. This is verified by an autotest.
The PR also adds a PILOT_SPEED parameter, so there are now 3 WPNAV_SPEED* parameters and 3 PILOT_SPEED* parameters. Previously, poshold was using WPNAV_SPEED for xy and PILOT_SPEED_UP/DN for z, this PR changes it to use PILOT_SPEED* for both xy and z.
(Circle mode also uses WPNAV_SPEED for xy and PILOT_SPEED_UP/DN for z, but I would argue that it should be using WPNAV_SPEED* for both xy and z. This can be a separate PR.)
There are 2 additional small fixes:
Sub::desired_climb_rateModePoshold::requires_GPSreturns true, soSub::set_modewill callsub.position_okbefore accepting a mode switch. (ModePoshold::init()function also callssub.position_ok, so this doesn't change behavior.)This should probably be tested on real hardware.
Fixes #28120