From e9ff670df5ab6cdba0d1d2cddbd940e4ee6eb044 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 29 Jul 2019 17:26:03 +0930 Subject: [PATCH] AC_AttitudeControl: Support System ID tests --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 3 ++ .../AC_AttitudeControl/AC_AttitudeControl.h | 35 ++++++++++++++++++- .../AC_AttitudeControl_Multi.cpp | 11 ++++-- 3 files changed, 45 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 752ef21a63..c24277b12a 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -710,6 +710,9 @@ void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, att_from_quat.rotation_matrix(att_from_rot_matrix); Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f); + // the dot product is used to calculate the current lean angle for use of external functions + _thrust_angle = acosf(constrain_float(Vector3f(0.0f,0.0f,1.0f) * att_from_thrust_vec,-1.0f,1.0f)); + // the cross product of the desired and target thrust vector defines the rotation vector Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index afc2d99d03..af857c1e85 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -193,6 +193,24 @@ public: // Set z-axis angular velocity in centidegrees/s void rate_bf_yaw_target(float rate_cds) { _rate_target_ang_vel.z = radians(rate_cds * 0.01f); } + // Set x-axis system identification angular velocity in degrees/s + void rate_bf_roll_sysid(float rate) { _rate_sysid_ang_vel.x = rate; } + + // Set y-axis system identification angular velocity in degrees/s + void rate_bf_pitch_sysid(float rate) { _rate_sysid_ang_vel.y = rate; } + + // Set z-axis system identification angular velocity in degrees/s + void rate_bf_yaw_sysid(float rate) { _rate_sysid_ang_vel.z = rate; } + + // Set x-axis system identification actuator + void actuator_roll_sysid(float command) { _actuator_sysid.x = command; } + + // Set y-axis system identification actuator + void actuator_pitch_sysid(float command) { _actuator_sysid.y = command; } + + // Set z-axis system identification actuator + void actuator_yaw_sysid(float command) { _actuator_sysid.z = command; } + // Return roll rate step size in radians/s that results in maximum output after 4 time steps float max_rate_step_bf_roll(); @@ -212,7 +230,7 @@ public: float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw() / _p_angle_yaw.kP(); } // Return angular velocity in radians used in the angular velocity controller - Vector3f rate_bf_targets() const { return _rate_target_ang_vel; } + Vector3f rate_bf_targets() const { return _rate_target_ang_vel + _rate_sysid_ang_vel; } // Enable or disable body-frame feed forward void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled = enable_or_disable; } @@ -244,6 +262,9 @@ public: // Return configured tilt angle limit in centidegrees float lean_angle_max() const { return _aparm.angle_max; } + // Return tilt angle in degrees + float lean_angle() const { return degrees(_thrust_angle); } + // Proportional controller with piecewise sqrt sections to constrain second derivative static float sqrt_controller(float error, float p, float second_ord_lim, float dt); @@ -381,9 +402,21 @@ protected: // velocity controller. Vector3f _rate_target_ang_vel; + // This is the the angular velocity in radians per second in the body frame, added to the output angular + // attitude controller by the System Identification Mode. + // It is reset to zero immediately after it is used. + Vector3f _rate_sysid_ang_vel; + + // This is the the unitless value added to the output of the PID by the System Identification Mode. + // It is reset to zero immediately after it is used. + Vector3f _actuator_sysid; + // 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_angle; + // The angle between the target thrust vector and the current thrust vector. float _thrust_error_angle; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 2257e0d1ae..d25998f3e6 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -327,17 +327,22 @@ void AC_AttitudeControl_Multi::rate_controller_run() // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration) update_throttle_rpy_mix(); + _rate_target_ang_vel += _rate_sysid_ang_vel; + Vector3f gyro_latest = _ahrs.get_gyro_latest(); - _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll)); + _motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); - _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch)); + _motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); - _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw)); + _motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z); _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()); + _rate_sysid_ang_vel.zero(); + _actuator_sysid.zero(); + control_monitor_update(); }