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

AC_PosControl: reduce glitch on init

This commit is contained in:
Leonard Hall 2018-08-22 17:30:30 +09:00 committed by Randy Mackay
parent 4ceabe4838
commit 5863b84c35
2 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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);
}

2
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -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