Ardupilot 绕圈模式分析（待修改篇-1）

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lixiaoweimashixiao/article/details/81911457

目录

摘要

本文主要记录ardupilot的绕圈模式的实现过程，欢迎批评指正，微信lxw15982962929

---

1.绕圈模式初始化

bool Copter::circle_init(bool ignore_checks)
{
    if (position_ok() || ignore_checks) //位置定位ok，没有解锁执行初始化
    {
        circle_pilot_yaw_override = false;

        //初始化无人机的飞行速度和加速度-------initialize speeds and accelerations
        pos_control->set_speed_xy(wp_nav->get_speed_xy());
        pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
        pos_control->set_jerk_xy_to_default();
        pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
        pos_control->set_accel_z(g.pilot_accel_z);

        //初始化绕圈控制，根据设定的绕圈中心------initialise circle controller including setting the circle center based on vehicle speed
        circle_nav->init();

        return true; //返回1，才能进行绕圈控制
    }else
    {
        return false; //返回0，绕圈控制无作用
    }
}

---

分析其中的函数： circle_nav->init();

---

   
void AC_Circle::init()
{
    // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles)
	//初始化位置控制器（根据车辆当前倾斜角度设置目标滚转角、俯仰角和I项）
    _pos_control.init_xy_controller();

    //将初始位置目标设为合理停车点--------set initial position target to reasonable stopping point
    _pos_control.set_target_to_stopping_point_xy();
    _pos_control.set_target_to_stopping_point_z();

    //获取阻止点-----------------------get stopping point
    const Vector3f& stopping_point = _pos_control.get_pos_target();

    //在停止点之前设置圆心到圆弧半径-------- set circle center to circle_radius ahead of stopping point
    _center.x = stopping_point.x + _radius * _ahrs.cos_yaw();
    _center.y = stopping_point.y + _radius * _ahrs.sin_yaw();
    _center.z = stopping_point.z;

    //计算速度------calculate velocities
    calc_velocities(true);

    //从无人机器航向设置初始角度------set starting angle from vehicle heading
    init_start_angle(true);
}

需要注意的参数：

const AP_Param::GroupInfo AC_Circle::var_info[] = {
    // @Param: RADIUS
    // @DisplayName: Circle Radius
    // @Description: Defines the radius of the circle the vehicle will fly when in Circle flight mode
    // @Units: cm
    // @Range: 0 10000
    // @Increment: 100
    // @User: Standard
    AP_GROUPINFO("RADIUS",  0,  AC_Circle, _radius, AC_CIRCLE_RADIUS_DEFAULT), //绕圈半径设置

    // @Param: RATE
    // @DisplayName: Circle rate
    // @Description: Circle mode's turn rate in deg/sec.  Positive to turn clockwise, negative for counter clockwise
    // @Units: deg/s
    // @Range: -90 90
    // @Increment: 1
    // @User: Standard
    AP_GROUPINFO("RATE",    1, AC_Circle, _rate,    AC_CIRCLE_RATE_DEFAULT), //绕圈角速度是20deg/s

    AP_GROUPEND
};

----------

---

2.绕圈模式控制

---

---

void Copter::circle_run()
{
    float target_yaw_rate = 0;
    float target_climb_rate = 0;

    // initialize speeds and accelerations
    pos_control->set_speed_xy(wp_nav->get_speed_xy());
    pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
    pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
    pos_control->set_accel_z(g.pilot_accel_z);
    
    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    //假如自动锁没有开，电机状态锁没有使能，设置油门值，最低，立即退出
    //motors->armed()主要判断遥是否解锁操作
    //ap.auto_armed判断是否自动解锁
    //ap.land_complete 判断无人机是否着陆
    //motors->get_interlock()判断电机的状态
    if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) 
    {
        // To-Do: add some initialisation of position controllers
#if FRAME_CONFIG == HELI_FRAME  //配置直升机姿态------Helicopters always stabilize roll/pitch/yaw
        // call attitude controller
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
        attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
        motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);//所有电机运转
        // 当无人机没有解锁时，不进行多旋翼的姿态角度使能-----multicopters do not stabilize roll/pitch/yaw when disarmed
        attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
        pos_control->set_alt_target_to_current_alt();//设定当前高度为目标高度
        return;
    }

    //处理遥控器输入-------------process pilot inputs
    if (!failsafe.radio) 
    {
        //从遥控器获取无人机的目标偏航角速度-----get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
        if (!is_zero(target_yaw_rate)) //is_zero(target_yaw_rate)不是空就返回1，否则返回0
        {
            circle_pilot_yaw_override = true;
        }

        // 获取目标爬升速率--------------------------get pilot desired climb rate
        target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

        //检查操作者是否要求起飞----------------------check for pilot requested take-off
        if (ap.land_complete && target_climb_rate > 0) 
        {
            //设置变量说明我们已经起飞了--------------indicate we are taking off
            set_land_complete(false);//ap.land_complete的变量改变
            //当无人机起飞了，清除积分项--------------clear i term when we're taking off
            set_throttle_takeoff();
        }
    }

    //将电机设置为全范围---------------------------set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);//设定电机的状态不在受启动的限制

    //运行绕圈控制器-----run circle controller
    circle_nav->update();

    //号召姿态控制器---------------------------------call attitude controller
    if (circle_pilot_yaw_override) 
    {
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
    }else
    {
        attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
    }

    //利用测距仪调整爬升率-------------------------------adjust climb rate using rangefinder
    if (rangefinder_alt_ok()) 
    {
        //如果测距仪是好的，使用地面跟随--------------------if rangefinder is ok, use surface tracking
        target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
    }
    //更新高度目标和运行高度位置控制器----------------------update altitude target and call position controller
    pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
    pos_control->update_z_controller();
}

---

---

（1）重点函数分析： circle_nav->update();

---

void AC_Circle::update()
{
    //计算时间--------calculate dt
    float dt = _pos_control.time_since_last_xy_update(); //位置更新时间，单位是s

    // update circle position at poscontrol update rate
    if (dt >= _pos_control.get_dt_xy()) 
    {

        //检查时间是否是合理的值------------------------double check dt is reasonable
        if (dt >= 0.2f) 
        {
            dt = 0.0f;
        }

        //斜坡最大角速度-------------------------------ramp angular velocity to maximum
        if (_angular_vel < _angular_vel_max) 
        {
            _angular_vel += fabsf(_angular_accel) * dt;
            _angular_vel = MIN(_angular_vel, _angular_vel_max);
        }
        if (_angular_vel > _angular_vel_max) 
        {
            _angular_vel -= fabsf(_angular_accel) * dt;
            _angular_vel = MAX(_angular_vel, _angular_vel_max);
        }

        //更新目标角和总的角度---------------------------update the target angle and total angle traveled
        float angle_change = _angular_vel * dt;//角度改变值
        _angle += angle_change;
        _angle = wrap_PI(_angle);
        _angle_total += angle_change;  //总的角度改变值

        // if the circle_radius is zero we are doing panorama so no need to update loiter target
        //如果绕圈半径为0，我们正在做全景，因此不需要更新悬停目标。
        if (!is_zero(_radius)) //is_zero(_radius)是空返回1，不是空返回0
        {
            //计算目标位置----calculate target position
            Vector3f target;
            target.x = _center.x + _radius * cosf(-_angle);//获取位置x
            target.y = _center.y - _radius * sinf(-_angle);//获取当前位置y
            target.z = _pos_control.get_alt_target();//获取垂直高度

            //更新位置控制器目标值------------update position controller target
            _pos_control.set_xy_target(target.x, target.y);

            //机头是180度从无人机当前位置做绕圈---------heading is 180 deg from vehicles target position around circle
            _yaw = wrap_PI(_angle-M_PI) * AC_CIRCLE_DEGX100;
        }else
        {
            //设定当前位置为绕圈位置-----set target position to center
            Vector3f target;
            target.x = _center.x;
            target.y = _center.y;
            target.z = _pos_control.get_alt_target();

            //更新位置控制器------update position controller target
            _pos_control.set_xy_target(target.x, target.y);

            //航向与角度一致，但是改成角度值------heading is same as _angle but converted to centi-degrees
            _yaw = _angle * AC_CIRCLE_DEGX100;
        }

        //更新位置控制器---------update position controller
        _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);
    }
}

---