ardupilot
ardupilot copied to clipboard
ArduPilot
Copter: fix unconstrained avoidance backup speed
This fix is the result of a log where home changed altitude resulting in a huge breach of the max altitude fence. There was no fence action configured, however avoidance was enabled resulting in the vehicle trying to back away from the max altitude fence at 15 m/s. The configured climb rate limits were +-2.5 m/s.
Firstly this fixes the avoidance to constrain the velocity to AVOID_BACKUP_SPD parameter.
Secondly this constrains Z backup to half the set pos controller speeds. This helps keep the speeds in balance since the the avoid backup speed is probably set with XY speed in mind and could be too large for Z.
I have not tested this yet. We might want a minimal fix to just apply the backup speed in avoid for a backport.
Rebased and added a auto test which fails on master and passes with this PR.
I have reworked to take a pos control pointer from copter which allows the constrain to pos control limits to be done inside AC_Avoid resulting in a much smaller change.
AC_Avoid is also used by rover which will not set the pointer so will not try and use pos control.
A alternative method might be to add a pos control singleton.
I'm slightly surprised that the vertical speed limits weren't being applied in AC_PosControl but perhaps that's because we are not using "input shaping"
I'm slightly surprised that the vertical speed limits weren't being applied in AC_PosControl but perhaps that's because we are not using "input shaping"
The pos controller does not apply any limits, its on the caller to restrict what they ask for. I do think it should (https://github.com/ArduPilot/ardupilot/pull/24100) this is a case where having hard pos control limits would have been a good idea.
We discussed this on the call and agreed to use a new AVOID_BACKZ_SPD parameter.
Reworked to separate the speed limit into horizontal and vertical components. I also found I had omitted to apply the Z limit in the 3D case which I have now fixed.
@peterbarker, I think we can merge this unless you're comment above re the autotest is a blocker... so up to you.