From 5863b84c35d2ce51065f4a4bcc2015d59c188ae9 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 22 Aug 2018 17:30:30 +0900 Subject: [PATCH] AC_PosControl: reduce glitch on init --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 17 +++++++++-------- libraries/AC_AttitudeControl/AC_PosControl.h | 2 +- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index d63d881ab9..387b788d2b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -774,21 +774,21 @@ float AC_PosControl::get_lean_angle_max_cd() const } /// init_xy_controller - initialise the xy controller +/// this should be called after setting the position target and the desired velocity and acceleration /// sets target roll angle, pitch angle and I terms based on vehicle current lean angles /// should be called once whenever significant changes to the position target are made /// this does not update the xy target -void AC_PosControl::init_xy_controller(bool reset_I) +void AC_PosControl::init_xy_controller() { // set roll, pitch lean angle targets to current attitude + // todo: this should probably be based on the desired attitude not the current attitude _roll_target = _ahrs.roll_sensor; _pitch_target = _ahrs.pitch_sensor; // initialise I terms from lean angles - if (reset_I) { - // reset last velocity if this controller has just been engaged or dt is zero - lean_angles_to_accel(_accel_target.x, _accel_target.y); - _pid_vel_xy.set_integrator(_accel_target-_accel_desired); - } + _pid_vel_xy.reset_filter(); + lean_angles_to_accel(_accel_target.x, _accel_target.y); + _pid_vel_xy.set_integrator(_accel_target-_accel_desired); // flag reset required in rate to accel step _flags.reset_desired_vel_to_pos = true; @@ -867,7 +867,7 @@ void AC_PosControl::init_vel_controller_xyz() _roll_target = _ahrs.roll_sensor; _pitch_target = _ahrs.pitch_sensor; - // reset last velocity if this controller has just been engaged or dt is zero + _pid_vel_xy.reset_filter(); lean_angles_to_accel(_accel_target.x, _accel_target.y); _pid_vel_xy.set_integrator(_accel_target); @@ -1064,7 +1064,6 @@ void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler) // 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; @@ -1104,6 +1103,7 @@ void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float accel_right, accel_forward; // rotate accelerations into body forward-right frame + // todo: this should probably be based on the desired heading not the current heading accel_forward = accel_x_cmss*_ahrs.cos_yaw() + accel_y_cmss*_ahrs.sin_yaw(); accel_right = -accel_x_cmss*_ahrs.sin_yaw() + accel_y_cmss*_ahrs.cos_yaw(); @@ -1117,6 +1117,7 @@ void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const { // rotate our roll, pitch angles into lat/lon frame + // todo: this should probably be based on the desired attitude not the current attitude accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f); accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 404eb31f8a..4cba15852a 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -163,7 +163,7 @@ public: /// sets target roll angle, pitch angle and I terms based on vehicle current lean angles /// should be called once whenever significant changes to the position target are made /// this does not update the xy target - void init_xy_controller(bool reset_I = true); + void init_xy_controller(); /// set_accel_xy - set horizontal acceleration in cm/s/s /// leash length will be recalculated the next time update_xy_controller() is called