From 8a42a29ef72ef8f5ad4cf8afb5b513cd6a2f8eda Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Mon, 13 May 2024 19:33:26 -0400 Subject: [PATCH] AC_AttitudeControl: add comments for system ID disturbance input code --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 2 ++ libraries/AC_AttitudeControl/AC_PosControl.h | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index af47fea367f..afad2ab3ffb 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -643,6 +643,7 @@ void AC_PosControl::update_xy_controller() // Position Controller const Vector3f &curr_pos = _inav.get_position_neu_cm(); + // determine the combined position of the actual position and the disturbance from system ID mode Vector3f comb_pos = curr_pos; comb_pos.xy() += _disturb_pos; Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, comb_pos); @@ -655,6 +656,7 @@ void AC_PosControl::update_xy_controller() // Velocity Controller const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); + // determine the combined velocity of the actual velocity and the disturbance from system ID mode Vector2f comb_vel = curr_vel; comb_vel += _disturb_vel; Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), comb_vel, _dt, _limit_vector.xy()); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 942965d0de8..0681900efad 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -462,8 +462,8 @@ protected: float _jerk_max_xy_cmsss; // Jerk limit of the xy kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target float _jerk_max_z_cmsss; // Jerk limit of the z kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target float _vel_z_control_ratio = 2.0f; // confidence that we have control in the vertical axis - Vector2f _disturb_pos; - Vector2f _disturb_vel; + Vector2f _disturb_pos; // position disturbance generated by system ID mode + Vector2f _disturb_vel; // velocity disturbance generated by system ID mode // output from controller float _roll_target; // desired roll angle in centi-degrees calculated by position controller