Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: Add PosControl PID logging

This commit is contained in:
Iampete1 2020-09-03 19:10:18 +01:00 committed by Peter Barker
parent 9753a20404
commit c109df3dd5
2 changed files with 12 additions and 7 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,6 +896,8 @@ 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_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
@ -1038,7 +1040,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
Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
Vector2f accel_target;
// check if vehicle velocity is being overridden
if (_flags.vehicle_horiz_vel_override) {
@ -1057,22 +1059,22 @@ void AC_PosControl::run_xy_controller(float dt)
_pid_vel_xy.set_input(_vel_error);
// 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
// TODO: move limit handling into the PI and PID controller
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 {
vel_xy_i = _pid_vel_xy.get_i_shrink();
_vel_xy_i = _pid_vel_xy.get_i_shrink();
}
// 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
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.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;
// reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) {

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -421,6 +421,9 @@ protected:
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
// XY PID terms for logging
Vector2f _vel_xy_p, _vel_xy_i, _vel_xy_d;
LowPassFilterVector2f _accel_target_filter; // acceleration target filter
// ekf reset handling