Plane: add input shaping to roll and pitch controllers by IamPete1 · Pull Request #32743 · ArduPilot/ardupilot

IamPete1 · 2026-04-11T16:22:05Z

Summary

This adds input shaping to the roll and pitch controllers with new accel and jerk limit params for each. This should result in much smoother flight.

Classification & Testing (check all that apply and add your own)

Checked by a human programmer
Non-functional change
No-binary change
Infrastructure change (e.g. unit tests, helper scripts)
Automated test(s) verify changes (e.g. unit test, autotest)
Tested manually, description below (e.g. SITL)
Tested on hardware
Logs attached
Logs available on request

Description

This uses copters "real time s-curves" to shape the angle demands using the existing rate limits and new acceleration and jerk limits. This adds a new ATIS log message that shows the result of the input shaping. Currently this logs all the time (under the attitude bit), but once were happy with it we may want to turn it off by defualt.

Through testing we need to find good default values for the accel and jerk limits. The user can disable the new input shaping by setting either accel or jerk limit to zero.

Example of "raw" roll angle and shaped roll angle:

Some existing features are still missing:

roll indecision handling for large roll errors
roll wrap for inverted flight.
Absolute velocity constraint, currently the shaped velocity is limited by the rate limit params, but this is then added to the angle control velocity this total is not constrained. Copter uses a hard 2x limit for this (I think).
Testing with quadplane assistance and manual mode to ensure the input shaping is reset correctly.
Shape rate commands with acceleration and jerk.

IamPete1 · 2026-04-11T19:18:46Z

@lthall One thing I have noticed with having a jerk parameter is that if the acceleration limit is much larger than the jerk limit the shape_pos_vel_accel method essentially gets stuck. The inner velocity loop gain k_v approaches 0. Copter defines the jerk limit as accel_max / input_tc so when k_v is calculated as jerk_max / accel_lim its is just 1/input_tc because of the 0 to 1 recommended range on input_tc everything stays nicely in sync. Basically shape_pos_vel_accel does not deal well with a pure jerk limit.

I don't think its really a fundamental problem, but it does make me think moving to a time constant like copter will stop users from causing themselves issues, although I think the separate jerk limit is conceptually easier to understand.

IamPete1 · 2026-04-11T20:05:14Z

+
+    // Calculate angle error and apply gain
+    angle_err_deg = angle_target_deg - get_measured_angle();
+    const float desired_rate = (angle_err_deg / gains.tau) + rate_target_deg;


Adding the rate feed forward here results in significantly more overshoot. Just angle_err_deg / gains.tau is much better.

I think if you can get a good tune this is not such a issue. I'm not sure of a way we can mitigate this without also loosing some of the benefits.

lthall · 2026-04-12T23:42:20Z

@lthall One thing I have noticed with having a jerk parameter is that if the acceleration limit is much larger than the jerk limit the shape_pos_vel_accel method essentially gets stuck. The inner velocity loop gain k_v approaches 0. Copter defines the jerk limit as accel_max / input_tc so when k_v is calculated as jerk_max / accel_lim its is just 1/input_tc because of the 0 to 1 recommended range on input_tc everything stays nicely in sync. Basically shape_pos_vel_accel does not deal well with a pure jerk limit.

Yes, this algorithm was not designed with the intention of providing a Jerk limit primarily. It works on the assumption that the primary limit is Acceleration and that Jerk will be set high enough that it is primarily limiting the acceleration, not the velocity. I don't think this is a problem provided we provide clear guidance.

tridge · 2026-04-21T00:09:46Z

@IamPete1 main thing I'd like to see is an analysis of the impact on upset recovery, we want to ensure this doesn't re-introduce the problem with slow upset recovery. For example, maybe disable when inverted?
Another one worth thinking about is what happens with changes to estimator, eg. lane switch, switch to/from DCM with sudden attitude estimate changes

IamPete1 · 2026-05-17T15:52:48Z

main thing I'd like to see is an analysis of the impact on upset recovery, we want to ensure this doesn't re-introduce the problem with slow upset recovery. For example, maybe disable when inverted?

The pitch controller will not apply rate limits if inverted and the roll controller will not apply rate limits if the in_recovery flag is set. However, the accel/jerk limits are always active. So may result in a slower recovery (depending on the gains). Upset recovery in "normal" flight is not affected it would only be after a reset or after running in rate only mode. They are the cases where the target attitude is set to the current attitude. I think it would be:

On mode change from manual or acro
On completion of a altitude wait way-point where the glider pullup code is not used
On exit from a vtol mode.
On exit from a nav scripting period

One way to fix this would be to constrain the attitude resets to be within the normal attitude envelope in those cases.

Another one worth thinking about is what happens with changes to estimator, eg. lane switch, switch to/from DCM with sudden attitude estimate changes

I have added a checker the plane and a reset function in the controllers. I have not yet tested it thoroughly.

IamPete1 · 2026-05-17T15:54:02Z

This now uses the existing time constant as the axis time constant and adds a new angle gain for each axis.

Georacer

Really nice to have this feature! Perhaps now we'll be able to assess whether the roll/pitch angle tracking is well-tuned based on actual angle reference tracking and not angle rise times.

For now this is a review based on visual inspection. I'll test it in a 2nd pass.

Some additional comments:

IIUC tau (aka *2SRV_TCONST) is getting shadowed by the new *ANGLE_P parameter when input shaping is enabled. Is this correct?
Instead it's used as a gain for the jerk limit, which is a different usage, not covered by its parameter description.

It's not a problem for a first-pass implementation, but shouldn't we keep only one of the two? And/or deduplicate the usage of tau?

Georacer · 2026-05-24T20:02:08Z

-    float get_rate_out(float desired_rate, float scaler);
-    virtual float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) = 0;


Nice! I never liked those method names anyway!

Georacer · 2026-05-24T21:31:46Z

        "RFNS", "QBBBBfffffIf", "TimeUS,InUse,InRng,IRCnt,Ms0OK,Ms0,Dst,Cor,Cor0,CorL,TimeCL,HE", "s----mmmmmsm", "F----00000C0", true },
 #endif  // AP_RANGEFINDER_ENABLED
+
+// @LoggerMessage: ATIS


How about logging Demanded, Target, Actual like we do in Copter? These now exist and have the same meaning, no?

Yeah, we certainly could. This was really a first pass at logging which logs everything.

I had a think about this, I decided I like it better like this. This only logs the shaped angle and the angle error. If we did add demanded and actual that would be four extra fields which are already in the ATT message. The angle stuff is also redundant for VTOL modes, the velocity and acceleration logged is useful in all modes.

Although I could certainty see the case that we might want to log the shaped target in ATT since that is what people are used to logging. Now to get the full picture you need target and actual from ATT and the shaped target from ATIS.

Open to ideas.

I have improved the field descriptions.

IamPete1 · 2026-05-24T22:03:08Z

IIUC tau (aka *2SRV_TCONST) is getting shadowed by the new *ANGLE_P parameter when input shaping is enabled. Is this correct? Instead it's used as a gain for the jerk limit, which is a different usage, not covered by its parameter description.

It's not a problem for a first-pass implementation, but shouldn't we keep only one of the two? And/or deduplicate the usage of tau?

Sort of, the way the maths works results in the same rate demand from the error path using the old method as the feed forward path and the new method. So by re-using the same param we matain the same "feel". The angle gain can then be used to tune the disturbance rejection. So we do need both, but your totally right that I should update the TCONST param description.

IamPete1 · 2026-05-27T23:02:31Z

I have done a plane version of the kinematic tool that I hope will help explain how the new code will behave in comparison to the current code.

https://firmware.ardupilot.org/Tools/WebTools/KinematicTool/plane/

…attitude controlers

…ide of input shaping

…altitude to settle

… not changed

…HRS switch

IamPete1 · 2026-06-13T21:48:28Z

+    }
+
+    // Apply gain using sqrt controller
+    float desired_rate = sqrt_controller(angle_err_deg, angle_gain, accel_max * 0.5, dt);


@lthall I have copied this from copter, why use half the accel limit? With the input shaping that means we could attempt 1.5 times the accel limit.

Maybe I should add that I don't think there is really any right or wrong answer here, you could make arguments for using 1, or you could make a argument to limit the input shaped path to 0.5 too. 0.5 here seems like a nice middle ground. Just want to make sure I'm not missing something.

IamPete1 · 2026-06-13T21:53:33Z

I'm now quite happy with this, I think we just need to build testing hours. Looking for feedback on user facing stuff like param names and usage and logging. I did wonder if we should take the opportunity to get rid of the "2SRV" stuff.

peterbarker · 2026-06-15T23:52:19Z

https://drive.google.com/drive/folders/1OBmknLZ5DC9vaqqY6qwTZvfdjW_vwtL9

IamPete1 · 2026-06-16T00:34:28Z

Looks like the roll issue was triggered by a change in time constant from 0.05 to 0.6.

This is a change in jerk limit from 10000 deg/s^3 to 833.33 deg/s^3. This happend when the vehicle was close to its max accel of 500 deg/s^2.

This mean it took ages to slow down again, hence the roll over.

Looks like this is still a risk (also for copter). It also explains why I was unable to reproduce in SITL, you have to change the param when there was a large acceleration demand.

peterbarker · 2026-06-25T02:12:48Z

Failed to fly this due to rebase issue

IamPete1 requested review from Georacer, lthall and tridge April 11, 2026 16:22

IamPete1 added NeedsTesting Plane labels Apr 11, 2026

IamPete1 commented Apr 11, 2026

View reviewed changes

Hwurzburg added the WikiNeeded needs wiki update label Apr 11, 2026

IamPete1 force-pushed the FW_input_shaping branch from e93147d to b96a132 Compare April 17, 2026 02:13

IamPete1 commented Apr 17, 2026

View reviewed changes

Comment thread ArduPlane/Parameters.cpp Outdated

IamPete1 force-pushed the FW_input_shaping branch 3 times, most recently from bb04225 to 56426f3 Compare April 18, 2026 15:34

IamPete1 added the DevCallTopic label Apr 20, 2026

CraigElder removed the DevCallTopic label Apr 21, 2026

peterbarker added this to Spring Valley Testing Apr 21, 2026

tridge requested changes Apr 21, 2026

View reviewed changes

Comment thread ArduPlane/Parameters.cpp Outdated

peterbarker removed this from Spring Valley Testing May 16, 2026

IamPete1 force-pushed the FW_input_shaping branch from 56426f3 to fcb5e90 Compare May 17, 2026 14:50

github-actions Bot added the Scripting label May 17, 2026

IamPete1 force-pushed the FW_input_shaping branch from cf4dd4e to ff2313f Compare May 24, 2026 18:54

Georacer suggested changes May 24, 2026

View reviewed changes

peterbarker added this to Spring Valley Testing May 24, 2026

IamPete1 mentioned this pull request May 27, 2026

kinematicTool: add fixedwing version ArduPilot/WebTools#326

Merged

IamPete1 added 13 commits June 13, 2026 16:25

APM_Control: FixedWing: add angle P gain

319c44e

Scripting: Applets: revert param: add new fixedwing tuneing params

2de3ca7

AP_AHRS: make active_EKF_type function public

4896eec

Plane: check for changes in ahrs type, core or yaw resets and inform …

3772fc3

…attitude controlers

APM_Control: FixedWing: add ahrs reset method

09f7bec

APM_Control: AP_FW_Controller: move turn coordination pitch rate outs…

8186fe8

…ide of input shaping

APM_Control: AP_FW_Controller: default angle gain to zero and use 1/tau

84ac8a8

APM_Control: AP_FW_Controller: don't reset input shaping to offset value

8fe2fd7

Tools: autotest: Plane: GuidedAttitudeNoGPS: reduce takeoff alt by 30m

51da77b

Tools: autotest: Plane: SetHomeAltChange allow slightly longer for …

eaf2a0c

…altitude to settle

SITL: add AHRS angle offset params

4d590cb

AP_AHRS: SIM: apply angle offset from params

d00c601

Plane: check_ahrs_reset only report primary core change if type has…

d229c29

… not changed

IamPete1 force-pushed the FW_input_shaping branch from 35776ad to 3edd9d6 Compare June 13, 2026 15:25

IamPete1 added 4 commits June 13, 2026 17:55

Tools: autotest: Plane: add check for step less input shaping after A…

56edb70

…HRS switch

Plane: use FW controllers angle gain method

b6a7e8d

APM_Control: AP_FW_Controller: add get_angle_p method

686abdd

Tools: autotest: quadplane: TerrainAvoidApplet: turn off input shaping

7718b99

IamPete1 force-pushed the FW_input_shaping branch from 3edd9d6 to 7718b99 Compare June 13, 2026 16:55

IamPete1 added 2 commits June 13, 2026 22:41

APM_Control: AP_FW_Controller: rename methods per review

d722c1e

Plane: Logging: ATIS: Improve field descriptions

8208c01

IamPete1 commented Jun 13, 2026

View reviewed changes

IamPete1 added the DevCallTopic label Jun 13, 2026

IamPete1 requested a review from Georacer June 13, 2026 21:50

CraigElder removed the DevCallTopic label Jun 15, 2026

IamPete1 mentioned this pull request Jun 16, 2026

Tools: autotest: Copter: add test for time-constant change during a maneuver #33464

Open

9 tasks

		float get_rate_out(float desired_rate, float scaler);
		virtual float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode) = 0;

Uh oh!

Conversation

IamPete1 commented Apr 11, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Classification & Testing (check all that apply and add your own)

Description

Uh oh!

IamPete1 commented Apr 11, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

lthall commented Apr 12, 2026

Uh oh!

Uh oh!

tridge commented Apr 21, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Uh oh!

IamPete1 commented May 17, 2026

Uh oh!

IamPete1 commented May 17, 2026

Uh oh!

Georacer left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

IamPete1 commented May 24, 2026

Uh oh!

IamPete1 commented May 27, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

IamPete1 commented Jun 13, 2026

Uh oh!

peterbarker commented Jun 15, 2026

Uh oh!

IamPete1 commented Jun 16, 2026

Uh oh!

peterbarker commented Jun 25, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

8 participants

IamPete1 commented Apr 11, 2026 •

edited

Loading

IamPete1 commented Apr 11, 2026 •

edited

Loading

tridge commented Apr 21, 2026 •

edited

Loading

IamPete1 commented May 27, 2026 •

edited

Loading