Loiter Flight Mode in ArduCopter

看了下loiter 控制器，且ArduCopter 的loiter mode 不是很稳定，优化一波。

// loiter_init - initialise loiter controller
bool Copter::ModeLoiter::init(bool ignore_checks)
{
    if (copter.position_ok() || ignore_checks) {
        // set target to current position
        loiter_nav->init_target();
        // initialise position and desired velocity
        if (!pos_control->is_active_z()) {
            pos_control->set_alt_target_to_current_alt();
            pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
        }
        return true;
    }else{
        return false;
    }
}


// 从当前位置和速度，初始化位置和前馈速度。
void AC_Loiter::init_target()
{
    const Vector3f& curr_pos = _inav.get_position();
    const Vector3f& curr_vel = _inav.get_velocity();

    sanity_check_params();

    // 初始化位置控制器的速度和加速度
    _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);

    // 基于当前速度和人工阻力初始化期望加速度
    float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_angle_max_cd()*0.01f));
    _predicted_accel.x = pilot_acceleration_max*curr_vel.x/_speed_cms;
    _predicted_accel.y = pilot_acceleration_max*curr_vel.y/_speed_cms;
    _desired_accel = _predicted_accel;
    // this should be the current roll and pitch angle.
    _predicted_euler_angle.x = radians(_attitude_control.get_att_target_euler_cd().x*0.01f);
    _predicted_euler_angle.y = radians(_attitude_control.get_att_target_euler_cd().y*0.01f);

    // 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();
}