ArduPilot
Plane: improve POSITION1 velocity control
For a while, I've noticed an annoying behavior during POS1. The aircraft initially can't decelerate fast enough, but then as it gets slower, it massively undershoots QPOS.TSpd. This can be seen by the aircraft pitching up to max and then suddenly pitching back down below 0 right after. I can reproduce this in RealFlight using the Titan Cobra, but I've encountered this in real life on Volanti and Ottano.
The POS1 controller target velocity is shown in purple. The actual velocity, in orange, is doing a terrible job following it. We can see that the undershoot is actually commanded by the underlying position controller's target velocity in brown (multiple brown lines, I know). The position controller's target velocity goes lower because we start winding up a position error (blue/brown lines). The pitch angle (green/brown) goes from 27 degrees pitched up to 4 degrees pitched down back-to-back.
Essentially what the controller is saying is "I'm at the right speed to reach the point X meters away, but I got here too early and I should slow down even more to get my position-vs-time curve to match up with expectations". It does a great job of getting the actual position to come back and follow that curve, but that's entirely unnecessary.
Instead, we should clear the position error when we're in POS1. The outer controller just wants the inner controller to handle velocity and acceleration.
Here's the same plot after this PR. I think this is much nicer. Now the actual velocity follows QPOS.TSpd quite well. And our attitude commands are much more reasonable. We only hit 17 degrees pitch up, and after, we relax to 3 degrees pitch up, and we don't drop the nose until we switch to POS2 (I could try to knock that out too, but I'm fine with that).
backtransition_before_and_after.zip Here are the logs from RealFlight, flown on the Titan Cobra at RF_AirStadium.
I recently learned about https://github.com/ArduPilot/ardupilot/pull/21620. That PR also fixes the issue I'm trying to solve here (among other things). It's in need of an update (it's based on 4.4dev), but ~~I can confirm it stops the controller from trying to hold a distance-vs-time curve and instead correctly follows a speed-vs-distance curve~~ (update: only works on 4.4; rebasing to 4.7 broke it somehow):
backtransition_21620_4.4dev.zip
If Pete's planning to revive that that PR and get it in before 4.7, then we don't need mine. Otherwise, I think my smaller PR is worth doing in the meantime.