Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: add EKF reset handling

Browse Source
This commit is contained in:
Leonard Hall 2018-01-01 10:23:29 +10:30 committed by Randy Mackay
parent 8c45345863
commit 05418d3d29
2 changed files with 35 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -474,22 +474,22 @@ void AC_AttitudeControl::attitude_controller_run_quat()
// Add the angular velocity feedforward, rotated into vehicle frame
Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
Quaternion attitude_error_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion target_ang_vel_quat = attitude_error_quat.inverse()*attitude_target_ang_vel_quat*attitude_error_quat;
Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
Quaternion desired_ang_vel_quat = to_to_from_quat.inverse()*attitude_target_ang_vel_quat*to_to_from_quat;
// Correct the thrust vector and smoothly add feedforward and yaw input
if(_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE*2.0f){
_rate_target_ang_vel.z = _ahrs.get_gyro().z;
}else if(_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*feedforward_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.x += desired_ang_vel_quat.q2*feedforward_scalar;
_rate_target_ang_vel.y += desired_ang_vel_quat.q3*feedforward_scalar;
_rate_target_ang_vel.z += desired_ang_vel_quat.q4;
_rate_target_ang_vel.z = _ahrs.get_gyro().z*(1.0-feedforward_scalar) + _rate_target_ang_vel.z*feedforward_scalar;
} else {
_rate_target_ang_vel.x += target_ang_vel_quat.q2;
_rate_target_ang_vel.y += target_ang_vel_quat.q3;
_rate_target_ang_vel.z += target_ang_vel_quat.q4;
_rate_target_ang_vel.x += desired_ang_vel_quat.q2;
_rate_target_ang_vel.y += desired_ang_vel_quat.q3;
_rate_target_ang_vel.z += desired_ang_vel_quat.q4;
}
if (_rate_bf_ff_enabled) {
@ -499,6 +499,12 @@ void AC_AttitudeControl::attitude_controller_run_quat()
_attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
_attitude_target_quat.normalize();
}
// ensure Quaternions stay normalized
_attitude_target_quat.normalize();
// Record error to handle EKF resets
_attitude_ang_error = attitude_vehicle_quat.inverse() * _attitude_target_quat;
}
// thrust_heading_rotation_angles - calculates two ordered rotations to move the att_from_quat quaternion to the att_to_quat quaternion.
@ -653,6 +659,21 @@ void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
}
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void AC_AttitudeControl::inertial_frame_reset()
{
// Retrieve quaternion vehicle attitude
// TODO add _ahrs.get_quaternion()
Quaternion attitude_vehicle_quat;
attitude_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
// Recalculate the target quaternion
_attitude_target_quat = attitude_vehicle_quat * _attitude_ang_error;
// calculate the attitude target euler angles
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
}
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{

6
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -122,6 +122,9 @@ public:
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void shift_ef_yaw_target(float yaw_shift_cd);
// handle reset of attitude from EKF since the last iteration
void inertial_frame_reset();
// Command a Quaternion attitude with feedforward and smoothing
void input_quaternion(Quaternion attitude_desired_quat);
@ -367,6 +370,9 @@ protected:
// velocity controller.
Vector3f _rate_target_ang_vel;
// This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
Quaternion _attitude_ang_error;
// The angle between the target thrust vector and the current thrust vector.
float _thrust_error_angle;