/// 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()
{
// force the xy velocity controller to run immediately
_vel_xyz_step = 3;
// set roll, pitch lean angle targets to current attitude
_roll_target = _ahrs.roll_sensor;
_pitch_target = _ahrs.pitch_sensor;
// 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_rate_lat.set_integrator(_accel_target.x);
_pid_rate_lon.set_integrator(_accel_target.y);
// flag reset required in rate to accel step
_flags.reset_desired_vel_to_pos = true;
_flags.reset_rate_to_accel_xy = true;
// set target position in xy axis
const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y);
// move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity_xy(curr_vel.x, curr_vel.y);
// record update time
_last_update_vel_xyz_ms = hal.scheduler->millis();
}
/// 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_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;
// 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;
// set target position in xy axis
const Vector3f& curr_pos = _inav.get_position();
set_xy_target(curr_pos.x, curr_pos.y);
// move current vehicle velocity into feed forward velocity
const Vector3f& curr_vel = _inav.get_velocity();
set_desired_velocity_xy(curr_vel.x, curr_vel.y);
}