AC_AttitudeControl: Support System ID tests

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;
 

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;
 

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();
 }