Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: revert Add PosControl PID logging

Browse Source
This commit is contained in:
Randy Mackay 2020-09-11 15:18:15 +09:00
parent 36dee8791a
commit 4ed486bb92
2 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -896,8 +896,6 @@ void AC_PosControl::write_log()
AP::logger().Write_PSC(get_pos_target(), _inav.get_position(), get_vel_target(), _inav.get_velocity(), get_accel_target(), accel_x, accel_y); AP::logger().Write_PSC(get_pos_target(), _inav.get_position(), get_vel_target(), _inav.get_velocity(), get_accel_target(), accel_x, accel_y);
AP::logger().Write_PSCP(_vel_error, _vel_xy_p, _vel_xy_i, _vel_xy_d);
} }
/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller /// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
@ -1040,7 +1038,7 @@ void AC_PosControl::run_xy_controller(float dt)
// the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame // the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
Vector2f accel_target; Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
// check if vehicle velocity is being overridden // check if vehicle velocity is being overridden
if (_flags.vehicle_horiz_vel_override) { if (_flags.vehicle_horiz_vel_override) {
@ -1059,22 +1057,22 @@ void AC_PosControl::run_xy_controller(float dt)
_pid_vel_xy.set_input(_vel_error); _pid_vel_xy.set_input(_vel_error);
// get p // get p
_vel_xy_p = _pid_vel_xy.get_p(); vel_xy_p = _pid_vel_xy.get_p();
// update i term if we have not hit the accel or throttle limits OR the i term will reduce // update i term if we have not hit the accel or throttle limits OR the i term will reduce
// TODO: move limit handling into the PI and PID controller // TODO: move limit handling into the PI and PID controller
if (!_limit.accel_xy && !_motors.limit.throttle_upper) { if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
_vel_xy_i = _pid_vel_xy.get_i(); vel_xy_i = _pid_vel_xy.get_i();
} else { } else {
_vel_xy_i = _pid_vel_xy.get_i_shrink(); vel_xy_i = _pid_vel_xy.get_i_shrink();
} }
// get d // get d
_vel_xy_d = _pid_vel_xy.get_d(); vel_xy_d = _pid_vel_xy.get_d();
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target.x = (_vel_xy_p.x + _vel_xy_i.x + _vel_xy_d.x) * ekfNavVelGainScaler; accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
accel_target.y = (_vel_xy_p.y + _vel_xy_i.y + _vel_xy_d.y) * ekfNavVelGainScaler; accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
// reset accel to current desired acceleration // reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) { if (_flags.reset_accel_to_lean_xy) {

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -421,9 +421,6 @@ protected:
Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set
LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error LowPassFilterFloat _vel_error_filter; // low-pass-filter on z-axis velocity error
// XY PID terms for logging
Vector2f _vel_xy_p, _vel_xy_i, _vel_xy_d;
LowPassFilterVector2f _accel_target_filter; // acceleration target filter LowPassFilterVector2f _accel_target_filter; // acceleration target filter
// ekf reset handling // ekf reset handling