Skip to content

Commit

Permalink
AC_AttitudeControl: add comments for system ID disturbance input code
Browse files Browse the repository at this point in the history
  • Loading branch information
bnsgeyer committed May 14, 2024
1 parent 032201e commit 5b0b2cb
Show file tree
Hide file tree
Showing 2 changed files with 4 additions and 2 deletions.
2 changes: 2 additions & 0 deletions libraries/AC_AttitudeControl/AC_PosControl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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());
Expand Down
4 changes: 2 additions & 2 deletions libraries/AC_AttitudeControl/AC_PosControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -462,8 +462,8 @@ class AC_PosControl
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
Expand Down

0 comments on commit 5b0b2cb

Please sign in to comment.