MC position control - Add acceleration/thrust feedforward to velocity PID controller

[bresch]

Describe problem solved by the proposed pull request

Current state:
The model controlled by the velocity PID controller is contains one integrator which removes steady-state error when a constant setpoint is applied. To track a ramp setpoint, an integrator (I term of the PID controller) increases the order of the closed-loop system and remove the steady-state error.

Problems:

• The controller cannot track a parabolic setpoint
• To reduce the steady-state error during acceleration, the gain of the integrator has to be quite high, which causes an overshoot at the end of the ramp. This can be attenuated by a jerk-limited setpoint, but isn't enough.
• The drone does not feel responsive and the setpoints are tweaked as an attempt to hide bad tracking (setpoint resets, increase jerk and acceleration limits). This leads to hazardous behaviors.

Solution proposed by this PR

A feedforward on the controller is commonly used in industry to improve tracking. Since some flight tasks already produce acceleration setpoints, they can easily be fed as feedforwards. With a feedforward, the feedback controller is then only needed to remove the steady-state error and reject disturbances.

Describe possible alternatives

• Increase the order of the system by 1 (add an other integrator)
• A different type of controller.

Tests

SITL tests:
Without feedforward (now):

With feedforward (this PR):

FYI @RomanBapst @MaEtUgR

[MaEtUgR]

Nice! Good idea. I was looking into a similar approach but by switching the velocity controller to output a desired acceleration that has a proper unit and then mapping this output and the feed-forward term to the attitude setpoint with a slightly different strategy. But this is a nice first step. Did you test on a real vehicle yet? I can test my draft acceleration stick input flight task with this pr.

[MaEtUgR]

I quickly tried and controlling the drone with that task works but it loses altitude and does weir twitches so I have to check where that's coming from.

[MaEtUgR]

Just rebased to do further tests.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[jlecoeur]

@bresch this looks like a good improvement, what is missing to merge?

[bresch]

replaced by #12072