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();
}
  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Gkbytes

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值