MPC: ORBIT: reduce jerk when transitioning into orbit

[Claudio-Chies]

Solved Problem

When transitioning from flying towards the orbit to orbiting, the vehicle can jerk quite alot around the axis pointing towards the orbit center (usually roll)

The Problem is the transition into the circling is implemented as a step response.

Solution

With this PR i also add a ramp of 30 samples to the initial start of the orbit maneuvre, by this the initial jerk is greatly reduced, and the transition is smoother
ADD PICTURE

Changelog Entry

For release notes:

Bugfix: Reduced Jerk when transitioning from approach to orbit circle to orbit

Alternatives

Test coverage

• SITL Flightreview
  Tested also with higher velocities and changing radiuses, the problems described by Mathieu are not present in this fix

[bresch]

I tried with higher speeds and after changing the orbit radius the drone struggles a bit to settle back on the circle. This is probably because the jerk-limited trajectory makes the orbit controller oscillate.

[Claudio-Chies]

Good catch, i'll try do find a fix on monday

[MaEtUgR]

I settled on this Orbit setpoint generation after many tests with position setpoints, smoothing, ... and this is the best implementation I found so far. The only catch which I never got to solving is accelerating into orbiting at the beginning. I think it only requires some kind of 1 dimensional acceleration phase for the velocity. Feeding everything through the trajectory generator in cartesian coordinates with position setpoints brings back all the issues I already went through during initial testing.

Note that @junwoo091400 went through a similar process making the follow me mode and also ended up smoothing on the tangential dimension.

[Claudio-Chies]

Couldnt we use a linear ramp on the existing implementation?
This way we would flatten the initial jerk quite alot, and we wouldnt require the smoothing

[MaEtUgR]

Couldnt we use a linear ramp on the existing implementation?

Sure, that might work well. Could for example ramp in the rotation velocity using

PX4-Autopilot/src/lib/mathlib/math/Functions.hpp

Line 152 in a9cdb36

const T interpolate(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)

[bresch]

The orbit is made of 2 components:

• reference trajectory used as feedforward
• controller to bring the drone back on the circle

IMO, the slew-rate you want to add should be applied on the reference trajectory an not on everything. Also, instead of using an arbitrary time of 30dt to ramp up the velocity, why not limiting it using the acceleration parameter?

[Claudio-Chies]

@bresch
good point, i now implemented the ramping with the use of a slew rate, as discussed with @MaEtUgR