Fix trapezoid profile when stepping down

[pfankhauser]

[FGiraldez]

@pfankhauser do you remember the exact problem?

[pfankhauser]

Nope, but can easily be recreated in sim.