/// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller
void AC_PosControl::init_vel_controller_xyz()
{
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
_pid_vel_xy.reset_filter();
lean_angles_to_accel(_accel_target.x, _accel_target.y);
_pid_vel_xy.set_integrator(_accel_target);
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_accel_to_lean_xy = true;
// set target position
const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y);
set_alt_target(curr_pos.z);
// move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity(curr_vel);
// set vehicle acceleration to zero
set_desired_accel_xy(0.0f,0.0f);
// initialise ekf reset handlers
init_ekf_xy_reset();
init_ekf_z_reset();
}
/// 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()
{
// 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
_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;
_flags.reset_accel_to_lean_xy = true;
// initialise ekf xy reset handler
init_ekf_xy_reset();
}
/// init_xy_controller - initialise the xy controller
/// 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)
{
// set roll, pitch lean angle targets to 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);
_pi_vel_xy.set_integrator(_accel_target);
}
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
_flags.reset_accel_to_lean_xy = true;
// initialise ekf xy reset handler
init_ekf_xy_reset();
}
