AC_PosControl: reduce glitch on init

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) {
+    _pid_vel_xy.reset_filter();
-        // reset last velocity if this controller has just been engaged or dt is zero
+    lean_angles_to_accel(_accel_target.x, _accel_target.y);
-        lean_angles_to_accel(_accel_target.x, _accel_target.y);
+    _pid_vel_xy.set_integrator(_accel_target-_accel_desired);
-        _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);
 }

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