From 05418d3d2917d96431c51fc4729ddcc63acf70ce Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 1 Jan 2018 10:23:29 +1030 Subject: [PATCH] AC_AttitudeControl: add EKF reset handling --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 37 +++++++++++++++---- .../AC_AttitudeControl/AC_AttitudeControl.h | 6 +++ 2 files changed, 35 insertions(+), 8 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index e43be0e32b..7261228602 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -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) { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index b9d4006aa9..3393a1156f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -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;