Replace the Attitude Command Model with the improved Real Time S-Curve algorithm #31849
+116
−121
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
lthall
force-pushed
the
20260104_NewCommandModel
branch
from
add2f8c to
208ba8e
Compare
lthall
force-pushed
the
20260104_NewCommandModel
branch
4 times, most recently
from
94ddcf4 to
7ee49bf
Compare
lthall
force-pushed
the
20260104_NewCommandModel
branch
from
3cd2f28 to
b44d4e8
Compare
lthall
force-pushed
the
20260104_NewCommandModel
branch
from
e04d231 to
e383b0c
Compare
lthall
force-pushed
the
20260104_NewCommandModel
branch
2 times, most recently
from
f2c8d40 to
717a136
Compare
lthall
force-pushed
the
20260104_NewCommandModel
branch
5 times, most recently
from
498a9d8 to
ca431d8
Compare
IamPete1
reviewed
Jan 25, 2026
| _ang_vel_target_rads.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target_rads.z, ang_vel_target.z, radians(_ang_vel_yaw_max_degs), _dt_s); | ||
| attitude_command_model(wrap_PI(attitude_error_angle.x), 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s); | ||
| attitude_command_model(wrap_PI(attitude_error_angle.y), 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s); | ||
| attitude_command_model(wrap_PI(attitude_error_angle.z), 0.0, _ang_vel_target_rads.z, _ang_accel_target_rads.z, radians(_ang_vel_yaw_max_degs), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); |
Member
There was a problem hiding this comment.
Currently yaw uses _input_tc this changes it to _rate_y_tc
| ang_vel_to_euler_rate(_attitude_target, _ang_accel_target_rads, euler_accel_target_rads); | ||
| attitude_command_model(wrap_PI(euler_roll_angle_rad - _euler_angle_target_rad.x), 0.0, _euler_rate_target_rads.x, euler_accel_target_rads.x, fabsf(euler_rate_max_rads.x), euler_accel.x, _input_tc, _dt_s); | ||
| attitude_command_model(wrap_PI(euler_pitch_angle_rad - _euler_angle_target_rad.y), 0.0, _euler_rate_target_rads.y, euler_accel_target_rads.y, fabsf(euler_rate_max_rads.y), euler_accel.y, _input_tc, _dt_s); | ||
| float yaw_rate_max_rads = euler_rate_max_rads.z; |
Member
There was a problem hiding this comment.
Suggested change
| float yaw_rate_max_rads = euler_rate_max_rads.z; | |
| float yaw_rate_max_rads = fabsf(euler_rate_max_rads.z); |
ABS needs to be done here for the slew yaw min to make sense.
| // translate the roll pitch and yaw acceleration limits to the euler axis | ||
| // translate the roll pitch and yaw rate and acceleration limits to the euler axis | ||
| const Vector3f euler_accel = euler_accel_limit(_attitude_target, Vector3f{get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()}); | ||
| const Vector3f euler_rate_max_rads = euler_accel_limit(_attitude_target, Vector3f{radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), radians(_ang_vel_yaw_max_degs)}); |
Member
There was a problem hiding this comment.
I think the slew yaw limit should be done before the conversion to euler axis?
| _euler_rate_target_rads.z = constrain_float(_euler_rate_target_rads.z, -slew_yaw_max_rads, slew_yaw_max_rads); | ||
| yaw_rate_max_rads = MIN(yaw_rate_max_rads, slew_yaw_max_rads); | ||
| } | ||
| attitude_command_model(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), 0.0, _euler_rate_target_rads.z, euler_accel_target_rads.z, fabsf(yaw_rate_max_rads), euler_accel.z, _input_tc, _dt_s); |
Member
There was a problem hiding this comment.
Suggested change
| attitude_command_model(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), 0.0, _euler_rate_target_rads.z, euler_accel_target_rads.z, fabsf(yaw_rate_max_rads), euler_accel.z, _input_tc, _dt_s); | |
| attitude_command_model(wrap_PI(euler_yaw_angle_rad - _euler_angle_target_rad.z), 0.0, _euler_rate_target_rads.z, euler_accel_target_rads.z, yaw_rate_max_rads, euler_accel.z, _input_tc, _dt_s); |
| ang_vel_limit(_ang_vel_target_rads, radians(_ang_vel_roll_max_degs), radians(_ang_vel_pitch_max_degs), slew_yaw_max_rads); | ||
| attitude_command_model(attitude_error.x, 0.0, _ang_vel_target_rads.x, _ang_accel_target_rads.x, radians(_ang_vel_roll_max_degs), get_accel_roll_max_radss(), _input_tc, _dt_s); | ||
| attitude_command_model(attitude_error.y, 0.0, _ang_vel_target_rads.y, _ang_accel_target_rads.y, radians(_ang_vel_pitch_max_degs), get_accel_pitch_max_radss(), _input_tc, _dt_s); | ||
| attitude_command_model(attitude_error.z, heading_rate_rads, _ang_vel_target_rads.z, _ang_accel_target_rads.z, radians(_ang_vel_yaw_max_degs), get_accel_yaw_max_radss(), _rate_y_tc, _dt_s); |
Member
There was a problem hiding this comment.
This one is currently _input_tc
lthall
force-pushed
the
20260104_NewCommandModel
branch
from
ca431d8 to
ddad33e
Compare
lthall
force-pushed
the
20260104_NewCommandModel
branch
from
ddad33e to
f53f7e2
Compare
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
This updates the existing Sqrt Controller used for the attitude command model with the new Real Time S-Curve algorithm.
This results in a jerk limited response on both leaving the current attitude and approaching the new set point.
Current problems are all related to lack of strict handling of command model parameters.
I still have a little work to ensure the acceleration state is recorded correctly and converted to the correct frame before use.
I have separated the comment improvements here #31867