Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AC_AttitudeControl: attitude controller quaternion fix #17198

Conversation

lthall
Copy link
Contributor

@lthall lthall commented Apr 16, 2021

This PR fixes a problem with the feed forward angular velocity calculation highlighted in issue #17059.

We have also taken this opportunity to redefine the parameter names to make these algorithms easier to read and review.

The last three commits are the first from Hs293Go. These commits focus on replacing the vector rotation to again improve readability and reduce operations.

This PR also sets up the calculations in preparation for thrust vector attitude PR to follow.

@rmackay9 rmackay9 changed the title 20210414 pr attitude controller quaternion fix AC_AttitudeControl: attitude controller quaternion fix Apr 17, 2021
@lthall
Copy link
Contributor Author

lthall commented Apr 19, 2021

Included with #17222

@lthall lthall closed this Apr 19, 2021
@peterbarker peterbarker reopened this Apr 20, 2021
tridge
tridge previously requested changes Apr 20, 2021
libraries/AP_Math/quaternion.cpp Outdated Show resolved Hide resolved
@peterbarker peterbarker self-assigned this Apr 20, 2021
@peterbarker peterbarker force-pushed the 20210414_PR_AttitudeControllerQuaternionFix branch from 8fab76f to 08e1962 Compare April 20, 2021 06:42
@peterbarker peterbarker force-pushed the 20210414_PR_AttitudeControllerQuaternionFix branch from 08e1962 to 36092a2 Compare April 21, 2021 00:41
@peterbarker
Copy link
Contributor

I've reordered the patches in this PR to move the variable renames to the end. The delta was the same.

I've also added a patch on top to make the variable renaming consistent; I used this to do that:

git reset --hard ; for i in AC_AttitudeControl.cpp  AC_AttitudeControl.h AC_AttitudeControl_Heli.cpp AC_AttitudeControl_Multi.cpp AC_AttitudeControl_Sub.cpp AC_AttitudeControl_TS.cpp; do perl -pe '
  s/_attitude_target_euler_angle/_euler_angle_target/g;
  s/_attitude_target_euler_rate/_euler_rate_target/g;
  s/_attitude_target_quat/_attitude_target/g;
  s/attitude_target_quat/attitude_target/g;
  s/att_vehicle_thrust_vec/att_body_thrust_vec/g;
  s/attitude_vehicle_quat/attitude_body/g;
  s/_sysid_ang_vel_body/_rate_sysid_ang_vel/g;
  s/_rate_target_ang_vel/_ang_vel_body/g;
  s/attitude_target_ang_vel_quat/ang_vel_target/g;
  s/desired_ang_vel_quat/ang_vel_body_feedforward/g;
  s/to_to_from_quat/rotation_target_to_body/g;
  s/thrust_vec_correction_quat/thrust_vector_correction/g;
  s/attitude_target_update_quat/attitude_target_update/g;
  s/attitude_error_vector/attitude_error/g;
  s/_attitude_target_ang_vel/_ang_vel_target/g;
  s/_rate_sysid_ang_vel/_sysid_ang_vel_body/g;
    ' -i libraries/AC_AttitudeControl/$i; done && git diff crap/leonard-rebased

I applied that to the pre-rename commit, took the diff and applied the relevant hunks on top of the rename patch and discarded any regressions compared to the existing variable rename patch (mostly comments)

Matrix3f att_from_rot_matrix; // rotation from the current body frame to the inertial frame.
attitude_vehicle_quat.rotation_matrix(att_from_rot_matrix);
Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);
Matrix3f att_vehicle_rot_matrix; // rotation from the inertial frame to the vehicle body frame.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder what the difference is between "inertial frame" and "body frame"

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It may be clearer to say "rotation from NED to the vehicle body frame."

Copy link
Contributor

@rmackay9 rmackay9 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TBH, much of the changes are beyond me but I don't see any obvious errors, thanks!

@peterbarker peterbarker merged commit a3c329b into ArduPilot:master Apr 21, 2021
@peterbarker
Copy link
Contributor

Merged, thanks!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

6 participants