Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: rename heading_quad to yaw_vec_correction_quat

Browse Source 
also rename flip_scalart local variable to feedforward_scalar
also add comments
non-functional change
This commit is contained in:
Leonard Hall 2017-12-31 14:14:54 +10:30 committed by Randy Mackay
parent 59a2445ad3
commit 1e9e12e866
1 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -466,6 +466,7 @@ void AC_AttitudeControl::attitude_controller_run_quat()
_rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector); _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);
// Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame. // Add feedforward term that attempts to ensure that roll and pitch errors rotate with the body frame rather than the reference frame.
// todo: this should probably be a matrix that couples yaw as well.
_rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z; _rate_target_ang_vel.x += attitude_error_vector.y * _ahrs.get_gyro().z;
_rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z; _rate_target_ang_vel.y += -attitude_error_vector.x * _ahrs.get_gyro().z;
@ -480,11 +481,11 @@ void AC_AttitudeControl::attitude_controller_run_quat()
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){ if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
_rate_target_ang_vel.z = _ahrs.get_gyro().z; _rate_target_ang_vel.z = _ahrs.get_gyro().z;
}else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){ }else if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE){
float flip_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE); float feedforward_scalar = (1.0f - (_thrust_error_angle-AC_ATTITUDE_THRUST_ERROR_ANGLE)/AC_ATTITUDE_THRUST_ERROR_ANGLE);
_rate_target_ang_vel.x += target_ang_vel_quat.q2*flip_scalar; _rate_target_ang_vel.x += target_ang_vel_quat.q2*feedforward_scalar;
_rate_target_ang_vel.y += target_ang_vel_quat.q3*flip_scalar; _rate_target_ang_vel.y += target_ang_vel_quat.q3*feedforward_scalar;
_rate_target_ang_vel.z += target_ang_vel_quat.q4; _rate_target_ang_vel.z += target_ang_vel_quat.q4;
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-flip_scalar) + _rate_target_ang_vel.z*flip_scalar; _rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-feedforward_scalar) + _rate_target_ang_vel.z*feedforward_scalar;
} else { } else {
_rate_target_ang_vel.x += target_ang_vel_quat.q2; _rate_target_ang_vel.x += target_ang_vel_quat.q2;
_rate_target_ang_vel.y += target_ang_vel_quat.q3; _rate_target_ang_vel.y += target_ang_vel_quat.q3;
@ -528,17 +529,21 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
} }
Quaternion thrust_vec_correction_quat; Quaternion thrust_vec_correction_quat;
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot); thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);
// Rotate thrust_vec_correction_quat to the att_from frame
thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat; thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;
// calculate the remaining rotation required after thrust vector is rotated // calculate the remaining rotation required after thrust vector is rotated transformed to the att_from frame
Quaternion heading_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat; Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;
// calculate the angle error in x and y.
Vector3f rotation; Vector3f rotation;
thrust_vec_correction_quat.to_axis_angle(rotation); thrust_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.x = rotation.x; att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y; att_diff_angle.y = rotation.y;
heading_quat.to_axis_angle(rotation); // calculate the angle error in z (x and y should be zero here).
yaw_vec_correction_quat.to_axis_angle(rotation);
att_diff_angle.z = rotation.z; att_diff_angle.z = rotation.z;
// Todo: Limit roll an pitch error based on output saturation and maximum error. // Todo: Limit roll an pitch error based on output saturation and maximum error.
@ -548,8 +553,8 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat,
// This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters. // This should be updated to be based on an angle limit, saturation, or unlimited based on user defined parameters.
if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){ if(!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()){
att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP()); att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS/_p_angle_yaw.kP());
heading_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z)); yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f,0.0f,att_diff_angle.z));
att_to_quat = att_from_quat*thrust_vec_correction_quat*heading_quat; att_to_quat = att_from_quat*thrust_vec_correction_quat*yaw_vec_correction_quat;
} }
} }