/// initialize's position and feed-forward velocity from current pos and velocity
void AC_Loiter::init_target()
{
const Vector3f& curr_pos = _inav.get_position();
const Vector3f& curr_vel = _inav.get_velocity();
sanity_check_params();
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(LOITER_VEL_CORRECTION_MAX);
_pos_control.set_accel_xy(_accel_cmss);
_pos_control.set_leash_length_xy(LOITER_POS_CORRECTION_MAX);
_predicted_accel = _desired_accel;
// update angle targets that will be passed to stabilize controller
float roll_cd, pitch_cd;
_pos_control.accel_to_lean_angles(_predicted_accel.x, _predicted_accel.y, roll_cd, pitch_cd);
_predicted_euler_angle.x = radians(roll_cd*0.01);
_predicted_euler_angle.y = radians(pitch_cd*0.01);
// set target position
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
// set vehicle velocity and acceleration to current state
_pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
_pos_control.set_desired_accel_xy(_desired_accel.x, _desired_accel.y);
// initialise position controller
_pos_control.init_xy_controller();
}
/// init_target to a position in cm from ekf origin
void AC_Loiter::init_target(const Vector3f& position)
{
sanity_check_params();
// initialise pos controller speed, acceleration
_pos_control.set_max_speed_xy(LOITER_VEL_CORRECTION_MAX);
_pos_control.set_max_accel_xy(_accel_cmss);
// initialise desired acceleration and angles to zero to remain on station
_predicted_accel.zero();
_desired_accel = _predicted_accel;
_predicted_euler_angle.zero();
// set target position
_pos_control.set_xy_target(position.x, position.y);
// set vehicle velocity and acceleration to zero
_pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.set_desired_accel_xy(0.0f,0.0f);
// initialise position controller if not already active
if (!_pos_control.is_active_xy()) {
_pos_control.init_xy_controller();
}
}
