diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 14d3a1fc7f32d..ef3317fe6bd2d 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -254,14 +254,15 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed) // remain very close together. // // All input functions below follow the same procedure -// 1. define the desired attitude the aircraft should attempt to achieve using the input variables -// 2. using the desired attitude and input variables, define the target angular velocity so that it should +// 1. define the desired attitude or attitude change based on the input variables +// 2. update the target attitude based on the angular velocity target and the time since the last loop +// 3. using the desired attitude and input variables, define the target angular velocity so that it should // move the target attitude towards the desired attitude -// 3. if _rate_bf_ff_enabled is not being used then make the target attitude +// 4. if _rate_bf_ff_enabled is not being used then make the target attitude // and target angular velocities equal to the desired attitude and desired angular velocities. -// 4. ensure _attitude_target, _euler_angle_target, _euler_rate_target and +// 5. ensure _attitude_target, _euler_angle_target, _euler_rate_target and // _ang_vel_target have been defined. This ensures input modes can be changed without discontinuity. -// 5. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and +// 6. attitude_controller_run_quat is then run to pass the target angular velocities to the rate controllers and // integrate them into the target attitude. Any errors between the target attitude and the measured attitude are // corrected by first correcting the thrust vector until the angle between the target thrust vector measured // trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected. @@ -270,15 +271,18 @@ void AC_AttitudeControl::landed_gain_reduction(bool landed) // attitude_desired_quat: is updated on each time_step by the integral of the body frame angular velocity void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body) { - Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat; - Vector3f attitude_error_angle; - attitude_error_quat.to_axis_angle(attitude_error_angle); + // update attitude target + update_attitude_target(); // Limit the angular velocity ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); Vector3f ang_vel_target = attitude_desired_quat * ang_vel_body; if (_rate_bf_ff_enabled) { + Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat; + Vector3f attitude_error_angle; + attitude_error_quat.to_axis_angle(attitude_error_angle); + // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration // and an exponential decay specified by _input_tc at the end. @@ -314,6 +318,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f); float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f); + // update attitude target + update_attitude_target(); + // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -365,6 +372,9 @@ void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f); float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f); + // update attitude target + update_attitude_target(); + // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -424,6 +434,9 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c float euler_pitch_rate = radians(euler_pitch_rate_cds * 0.01f); float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f); + // update attitude target + update_attitude_target(); + // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -466,6 +479,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl float pitch_rate_rads = radians(pitch_rate_bf_cds * 0.01f); float yaw_rate_rads = radians(yaw_rate_bf_cds * 0.01f); + // update attitude target + update_attitude_target(); + // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -482,7 +498,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl } else { // When feedforward is not enabled, the quaternion is calculated and is input into the target and the feedforward rate is zeroed. Quaternion attitude_target_update; - attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads * _dt, pitch_rate_rads * _dt, yaw_rate_rads * _dt}); + attitude_target_update.from_axis_angle(Vector3f{roll_rate_rads, pitch_rate_rads, yaw_rate_rads} * _dt); _attitude_target = _attitude_target * attitude_target_update; _attitude_target.normalize(); @@ -539,8 +555,9 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, } Vector3f gyro_latest = _ahrs.get_gyro_latest(); - attitude_ang_error_update_quat.from_axis_angle(Vector3f{(_ang_vel_target.x-gyro_latest.x) * _dt, (_ang_vel_target.y-gyro_latest.y) * _dt, (_ang_vel_target.z-gyro_latest.z) * _dt}); + attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt); _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error; + _attitude_ang_error.normalize(); // Compute acceleration-limited body frame rates // When acceleration limiting is enabled, the input shaper constrains angular acceleration about the axis, slewing @@ -555,6 +572,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, // Update the unused targets attitude based on current attitude to condition mode change _attitude_target = attitude_body * _attitude_ang_error; + _attitude_target.normalize(); // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -566,9 +584,6 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, _attitude_ang_error.to_axis_angle(attitude_error); _ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); _ang_vel_body += _ang_vel_target; - - // ensure Quaternions stay normalized - _attitude_ang_error.normalize(); } // Command an angular step (i.e change) in body frame angle @@ -608,6 +623,9 @@ void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust heading_rate = constrain_float(heading_rate, -slew_yaw_max_rads, slew_yaw_max_rads); } + // update attitude target + update_attitude_target(); + // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -660,6 +678,9 @@ void AC_AttitudeControl::input_thrust_vector_heading(const Vector3f& thrust_vect float heading_rate = constrain_float(radians(heading_rate_cds * 0.01f), -slew_yaw_max_rads, slew_yaw_max_rads); float heading_angle = radians(heading_angle_cd * 0.01f); + // update attitude target + update_attitude_target(); + // calculate the attitude target euler angles _attitude_target.to_euler(_euler_angle_target); @@ -745,6 +766,16 @@ Quaternion AC_AttitudeControl::attitude_from_thrust_vector(Vector3f thrust_vecto return thrust_vec_quat*yaw_quat; } +// Calculates the body frame angular velocities to follow the target attitude +void AC_AttitudeControl::update_attitude_target() +{ + // rotate target and normalize + Quaternion attitude_target_update; + attitude_target_update.from_axis_angle(_ang_vel_target * _dt); + _attitude_target *= attitude_target_update; + _attitude_target.normalize(); +} + // Calculates the body frame angular velocities to follow the target attitude void AC_AttitudeControl::attitude_controller_run_quat() { @@ -782,16 +813,6 @@ void AC_AttitudeControl::attitude_controller_run_quat() _ang_vel_body += ang_vel_body_feedforward; } - if (_rate_bf_ff_enabled) { - // rotate target and normalize - Quaternion attitude_target_update; - attitude_target_update.from_axis_angle(Vector3f{_ang_vel_target.x * _dt, _ang_vel_target.y * _dt, _ang_vel_target.z * _dt}); - _attitude_target = _attitude_target * attitude_target_update; - } - - // ensure Quaternion stay normalised - _attitude_target.normalize(); - // Record error to handle EKF resets _attitude_ang_error = attitude_body.inverse() * _attitude_target; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index b3968282580d5..9363287095924 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -346,6 +346,9 @@ class AC_AttitudeControl { // translates body frame acceleration limits to the euler axis Vector3f euler_accel_limit(const Quaternion &att, const Vector3f &euler_accel); + // Calculates the body frame angular velocities to follow the target attitude + void update_attitude_target(); + // Calculates the body frame angular velocities to follow the target attitude void attitude_controller_run_quat();