Ardupilot如何实现避障代码分析

目录

摘要

本节主要记录自己学ardupilot的避障代码过程，首先看下怎么注册避障传感器，然后怎么获取避障数据，最后怎么实现避障控制。

---

---

1.注册避障模块实现初始化

**1》》》》》》》》》》**
void Copter::setup() 
{
    cliSerial = hal.console;

    //加载初始化参数表到 in var_info[]s
    AP_Param::setup_sketch_defaults();

    // 初始化无人机的结构布局
    StorageManager::set_layout_copter();

    //注册传感器
    init_ardupilot();

    // 初始化 main loop 任务
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));

    // 初始化 main loop 任务
    perf_info_reset();
    fast_loopTimer = AP_HAL::micros();
}
**2》》》》》》》》》》**
void Copter::init_ardupilot()
{

  init_proximity();//避障函数初始化

}
    **3》》》》》》》》》》**
void Copter::init_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
    g2.proximity.init();
    g2.proximity.set_rangefinder(&rangefinder);
#endif
}

------------------------------------------------------------
               首先看下 g2.proximity.init();
------------------------------------------------------------
void AP_Proximity::init(void)
{
    if (num_instances != 0)  //没有传感器就直接返回
    {
        //初始化号召等待2s-------- init called a 2nd time?
        return;
    }
    for (uint8_t i=0; i<PROXIMITY_MAX_INSTANCES; i++) 
    {
        detect_instance(i); //传感器识别
        if (drivers[i] != nullptr) //我们为这个实例加载了一个驱动程序，所以它必须存在（尽管它可能不健康）
        {
            // we loaded a driver for this instance, so it must be present (although it may not be healthy)
            num_instances = i+1;
        }

        //初始化状态标志-----initialise status
        state[i].status = Proximity_NotConnected;
    }
}

------------------------------------------------------------
          继续看下识别我们用的哪种传感器 detect_instance(i);
------------------------------------------------------------
void AP_Proximity::detect_instance(uint8_t instance)
{
    uint8_t type = _type[instance];
    if (type == Proximity_Type_SF40C)   //激光测距模块
    {
        if (AP_Proximity_LightWareSF40C::detect(serial_manager)) 
        {
            state[instance].instance = instance;
            drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], serial_manager);
            return;
        }
    }
    if (type == Proximity_Type_MAV) 
    {
        state[instance].instance = instance;
        drivers[instance] = new AP_Proximity_MAV(*this, state[instance]);
        return;
    }
    if (type == Proximity_Type_TRTOWER) 
    {
        if (AP_Proximity_TeraRangerTower::detect(serial_manager)) 
        {
            state[instance].instance = instance;
            drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], serial_manager);
            return;
        }
    }
    if (type == Proximity_Type_RangeFinder) //测距仪器
    {
        state[instance].instance = instance;
        drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
        return;
    }
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    if (type == Proximity_Type_SITL) 
    {
        state[instance].instance = instance;
        drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); //创建实例
        return;
    }
#endif
}
///////////////////////////////////////////////
可以看到这里主要是选择哪种类型的避障模块
///////////////////////////////////////////////

1》这里看下支持的驱动类型：

//2》假如我们使用的是Proximity_Type_RangeFinder，则要创建对象

drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);

3》用到的参数

// table of user settable parameters
const AP_Param::GroupInfo AP_Proximity::var_info[] = {
    // 0 is reserved for possible addition of an ENABLED parameter

    // @Param: _TYPE
    // @DisplayName: Proximity type
    // @Description: What type of proximity sensor is connected
    // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder
    // @User: Standard
    AP_GROUPINFO("_TYPE",   1, AP_Proximity, _type[0], 0),

    // @Param: _ORIENT
    // @DisplayName: Proximity sensor orientation
    // @Description: Proximity sensor orientation
    // @Values: 0:Default,1:Upside Down
    // @User: Standard
    AP_GROUPINFO("_ORIENT", 2, AP_Proximity, _orientation[0], 0),

    // @Param: _YAW_CORR
    // @DisplayName: Proximity sensor yaw correction
    // @Description: Proximity sensor yaw correction
    // @Units: degrees
    // @Range: -180 180
    // @User: Standard
    AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], PROXIMITY_YAW_CORRECTION_DEFAULT),

    // @Param: _IGN_ANG1
    // @DisplayName: Proximity sensor ignore angle 1
    // @Description: Proximity sensor ignore angle 1
    // @Units: degrees
    // @Range: 0 360
    // @User: Standard
    AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity, _ignore_angle_deg[0], 0),

    // @Param: _IGN_WID1
    // @DisplayName: Proximity sensor ignore width 1
    // @Description: Proximity sensor ignore width 1
    // @Units: degrees
    // @Range: 0 45
    // @User: Standard
    AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity, _ignore_width_deg[0], 0),

    // @Param: _IGN_ANG2
    // @DisplayName: Proximity sensor ignore angle 2
    // @Description: Proximity sensor ignore angle 2
    // @Units: degrees
    // @Range: 0 360
    // @User: Standard
    AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity, _ignore_angle_deg[1], 0),

    // @Param: _IGN_WID2
    // @DisplayName: Proximity sensor ignore width 2
    // @Description: Proximity sensor ignore width 2
    // @Units: degrees
    // @Range: 0 45
    // @User: Standard
    AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity, _ignore_width_deg[1], 0),

    // @Param: _IGN_ANG3
    // @DisplayName: Proximity sensor ignore angle 3
    // @Description: Proximity sensor ignore angle 3
    // @Units: degrees
    // @Range: 0 360
    // @User: Standard
    AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity, _ignore_angle_deg[2], 0),

    // @Param: _IGN_WID3
    // @DisplayName: Proximity sensor ignore width 3
    // @Description: Proximity sensor ignore width 3
    // @Units: degrees
    // @Range: 0 45
    // @User: Standard
    AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity, _ignore_width_deg[2], 0),

    // @Param: _IGN_ANG4
    // @DisplayName: Proximity sensor ignore angle 4
    // @Description: Proximity sensor ignore angle 4
    // @Units: degrees
    // @Range: 0 360
    // @User: Standard
    AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity, _ignore_angle_deg[3], 0),

    // @Param: _IGN_WID4
    // @DisplayName: Proximity sensor ignore width 4
    // @Description: Proximity sensor ignore width 4
    // @Units: degrees
    // @Range: 0 45
    // @User: Standard
    AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity, _ignore_width_deg[3], 0),

    // @Param: _IGN_ANG5
    // @DisplayName: Proximity sensor ignore angle 5
    // @Description: Proximity sensor ignore angle 5
    // @Units: degrees
    // @Range: 0 360
    // @User: Standard
    AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity, _ignore_angle_deg[4], 0),

    // @Param: _IGN_WID5
    // @DisplayName: Proximity sensor ignore width 5
    // @Description: Proximity sensor ignore width 5
    // @Units: degrees
    // @Range: 0 45
    // @User: Standard
    AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity, _ignore_width_deg[4], 0),

    // @Param: _IGN_ANG6
    // @DisplayName: Proximity sensor ignore angle 6
    // @Description: Proximity sensor ignore angle 6
    // @Units: degrees
    // @Range: 0 360
    // @User: Standard
    AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity, _ignore_angle_deg[5], 0),

    // @Param: _IGN_WID6
    // @DisplayName: Proximity sensor ignore width 6
    // @Description: Proximity sensor ignore width 6
    // @Units: degrees
    // @Range: 0 45
    // @User: Standard
    AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0),

#if PROXIMITY_MAX_INSTANCES > 1
    // @Param: 2_TYPE
    // @DisplayName: Second Proximity type
    // @Description: What type of proximity sensor is connected
    // @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder
    // @User: Advanced
    AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0),

    // @Param: 2_ORIENT
    // @DisplayName: Second Proximity sensor orientation
    // @Description: Second Proximity sensor orientation
    // @Values: 0:Default,1:Upside Down
    // @User: Standard
    AP_GROUPINFO("2_ORIENT", 17, AP_Proximity, _orientation[1], 0),

    // @Param: 2_YAW_CORR
    // @DisplayName: Second Proximity sensor yaw correction
    // @Description: Second Proximity sensor yaw correction
    // @Units: degrees
    // @Range: -180 180
    // @User: Standard
    AP_GROUPINFO("2_YAW_CORR", 18, AP_Proximity, _yaw_correction[1], PROXIMITY_YAW_CORRECTION_DEFAULT),
#endif

    AP_GROUPEND
};

5》在missionplanner中显示参数，

4》》》》》》》》g2.proximity.set_rangefinder(&rangefinder);

//  RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270}; //测距使用
void set_rangefinder(const RangeFinder *rangefinder) 
{ 
        _rangefinder = rangefinder; 
}

---

---

2.更新避障数据模块

  SCHED_TASK(update_proximity,     100,     50), //更新近距离传感器，避障使用

1》》》》》》》》》》》》》》》》》》》分析代码

void Copter::update_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
    g2.proximity.update();
#endif
}

2》》》》》》》》》》》》分析代码g2.proximity.update();

void AP_Proximity::update(void)
{
    for (uint8_t i=0; i<num_instances; i++) 
    {
        if (drivers[i] != nullptr) 
        {
            if (_type[i] == Proximity_Type_None) 
            {
                //允许用户在运行时禁用接近传感器----- allow user to disable a proximity sensor at runtime
                state[i].status = Proximity_NotConnected;
                continue;
            }
            drivers[i]->update();//更新数据
        }
    }

    //编制主实例-第一传感器返回良好数据---------work out primary instance - first sensor returning good data
    for (int8_t i=num_instances-1; i>=0; i--) 
    {
        if (drivers[i] != nullptr && (state[i].status == Proximity_Good)) 
        {
            primary_instance = i;
        }
    }
}

3》》》》》》》我们分析drivers[i]->update();//更新数据

//这里由于 virtual void update() = 0;是纯虚函数，我们以AP_Proximity_RangeFinder为例进行讲解
void AP_Proximity_RangeFinder::update(void)
{
    //如果没有测距仪目标立即退出-------------exit immediately if no rangefinder object
    const RangeFinder *rngfnd = frontend.get_rangefinder();
    if (rngfnd == nullptr) 
    {
        set_status(AP_Proximity::Proximity_NoData);
        return;
    }

    //查看所有测距仪----------------------look through all rangefinders
    for (uint8_t i=0; i<rngfnd->num_sensors(); i++) 
    {
        if (rngfnd->has_data(i)) 
        {
            //检查水平测距仪------------check for horizontal range finders
            if (rngfnd->get_orientation(i) <= ROTATION_YAW_315) 
            {
                uint8_t sector = (uint8_t)rngfnd->get_orientation(i);  //获取方向
                _angle[sector] = sector * 45;                          //获取角度
                _distance[sector] = rngfnd->distance_cm(i) / 100.0f;   //获取距离
                _distance_min = rngfnd->min_distance_cm(i) / 100.0f;   //最小距离
                _distance_max = rngfnd->max_distance_cm(i) / 100.0f;   //最大距离
                _distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max);  //是否在限制的范围
                _last_update_ms = AP_HAL::millis();                    //获取上次时间
                update_boundary_for_sector(sector);
            }
            //检查向上测距仪----------check upward facing range finder
            if (rngfnd->get_orientation(i) == ROTATION_PITCH_90) 
            {
                _distance_upward = rngfnd->distance_cm(i) / 100.0f;
                _last_upward_update_ms = AP_HAL::millis();
            }
        }
    }

    //检查超时并设置健康状态--------- check for timeout and set health status
    if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS)) 
    {
        set_status(AP_Proximity::Proximity_NoData);
    } else 
    {
        set_status(AP_Proximity::Proximity_Good);
    }
}

备注：这里要注意的是：这些值应该传到避障控制函数中，怎么传过去的。

---

---

3.避障控制初始化

《1》avoid_adsb_init(ignore_checks)

case AVOID_ADSB:
         success = avoid_adsb_init(ignore_checks);  //避障模块
            break;

《2》bool Copter::avoid_adsb_init(const bool ignore_checks)

bool Copter::avoid_adsb_init(const bool ignore_checks)
{
    //重用引导模式-----re-use guided mode
    return guided_init(ignore_checks);
}

《3》bool Copter::guided_init(bool ignore_checks)

bool Copter::guided_init(bool ignore_checks)
{
    if (position_ok() || ignore_checks) 
    {
        //初始化偏航----------------------initialise yaw
        set_auto_yaw_mode(get_default_auto_yaw_mode(false));//自动偏航设置
        //开始在位置控制模式-------------start in position control mode
        guided_pos_control_start();
        return true;
    }else
    {
        return false;
    }
}

《3》.1

void Copter::guided_pos_control_start()
{
    //设置为位置控制模式------- set to position control mode
    guided_mode = Guided_WP;

    //航路点与样条控制器的初始化----- initialise waypoint and spline controller
    wp_nav->wp_and_spline_init();

    //将WPNav初始化为停止点---- initialise wpnav to stopping point
    Vector3f stopping_point;
    wp_nav->get_wp_stopping_point(stopping_point);

    //不需要检查返回状态，因为不使用地形数据。---- no need to check return status because terrain data is not used
    wp_nav->set_wp_destination(stopping_point, false);

    //初始化偏航---- initialise yaw
    set_auto_yaw_mode(get_default_auto_yaw_mode(false));
}

《3》.1-1 wp_nav->wp_and_spline_init();

void AC_WPNav::wp_and_spline_init()
{
    // check _wp_accel_cms is reasonable
    if (_wp_accel_cms <= 0)
    {
        _wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
    }

    // also limit the accel using the maximum lean angle. This
    // prevents the navigation controller from trying to move the
    // target point at an unachievable rate
    float accel_limit_cms = GRAVITY_MSS * 100 * tanf(radians(_attitude_control.lean_angle_max()*0.01f));
    if (_wp_accel_cms > accel_limit_cms) 
    {
        _wp_accel_cms.set(accel_limit_cms);
    }

    //初始化位置控制器------------------initialise position controller
    _pos_control.init_xy_controller();
    _pos_control.clear_desired_velocity_ff_z();

    //初始化位置控制器的速度和加速度-------initialise position controller speed and acceleration
    _pos_control.set_speed_xy(_wp_speed_cms);
    _pos_control.set_accel_xy(_wp_accel_cms);
    _pos_control.set_jerk_xy_to_default();
    _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
    _pos_control.set_accel_z(_wp_accel_z_cms);
    _pos_control.calc_leash_length_xy();
    _pos_control.calc_leash_length_z();

    //将偏航航向初始化为当前航向目标---- initialise yaw heading to current heading target
    _flags.wp_yaw_set = false;
}

《3》.2-1 void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const

void AC_PosControl::get_stopping_point_xy(Vector3f &stopping_point) const
{
    const Vector3f curr_pos = _inav.get_position();
    Vector3f curr_vel = _inav.get_velocity();
    float linear_distance;      // the distance at which we swap from a linear to sqrt response
    float linear_velocity;      // the velocity above which we swap from a linear to sqrt response
    float stopping_dist;        // the distance within the vehicle can stop
    float kP = _p_pos_xy.kP();

    //增加速度误差到当前位置------add velocity error to current velocity
    if (is_active_xy()) 
    {
        curr_vel.x += _vel_error.x;
        curr_vel.y += _vel_error.y;
    }

    //计算当前速度矢量-------calculate current velocity
    float vel_total = norm(curr_vel.x, curr_vel.y);

    // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
    if (kP <= 0.0f || _accel_cms <= 0.0f || is_zero(vel_total)) 
    {
        stopping_point.x = curr_pos.x;
        stopping_point.y = curr_pos.y;
        return;
    }

    //计算速度从线性切换到SqRT的点----- calculate point at which velocity switches from linear to sqrt
    linear_velocity = _accel_cms/kP;

    //计算我们可以停止的距离---------------------- calculate distance within which we can stop
    if (vel_total < linear_velocity) 
    {
        stopping_dist = vel_total/kP;
    } else 
    {
        linear_distance = _accel_cms/(2.0f*kP*kP);
        stopping_dist = linear_distance + (vel_total*vel_total)/(2.0f*_accel_cms);
    }

    //约束制动距离----------------------constrain stopping distance
    stopping_dist = constrain_float(stopping_dist, 0, _leash);

    //利用速度矢量将停车距离转换为停车点----convert the stopping distance into a stopping point using velocity vector
    stopping_point.x = curr_pos.x + (stopping_dist * curr_vel.x / vel_total);
    stopping_point.y = curr_pos.y + (stopping_dist * curr_vel.y / vel_total);
}
//计算垂直阻止点
void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
{
    const float curr_pos_z = _inav.get_altitude();
    float curr_vel_z = _inav.get_velocity_z();

    float linear_distance;  // half the distance we swap between linear and sqrt and the distance we offset sqrt
    float linear_velocity;  // the velocity we swap between linear and sqrt

    // if position controller is active add current velocity error to avoid sudden jump in acceleration
    if (is_active_z()) {
        curr_vel_z += _vel_error.z;
        if (_flags.use_desvel_ff_z) {
            curr_vel_z -= _vel_desired.z;
        }
    }

    // avoid divide by zero by using current position if kP is very low or acceleration is zero
    if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) {
        stopping_point.z = curr_pos_z;
        return;
    }

    // calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
    linear_velocity = _accel_z_cms/_p_pos_z.kP();

    if (fabsf(curr_vel_z) < linear_velocity) {
        // if our current velocity is below the cross-over point we use a linear function
        stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP();
    } else {
        linear_distance = _accel_z_cms/(2.0f*_p_pos_z.kP()*_p_pos_z.kP());
        if (curr_vel_z > 0){
            stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
        } else {
            stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
        }
    }
    stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX);
}

《3》.3 wp_nav->set_wp_destination(stopping_point, false);

bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
{
    Vector3f origin;

    // if waypoint controller is active use the existing position target as the origin
    if ((AP_HAL::millis() - _wp_last_update) < 1000)
    {
        origin = _pos_control.get_pos_target();
    } else
    {
        // if waypoint controller is not active, set origin to reasonable stopping point (using curr pos and velocity)
        _pos_control.get_stopping_point_xy(origin);
        _pos_control.get_stopping_point_z(origin);
    }

    //将原点转换为地形之上的ALT---- convert origin to alt-above-terrain
    if (terrain_alt) 
    {
        float origin_terr_offset;
        if (!get_terrain_offset(origin_terr_offset))
        {
            return false;
        }
        origin.z -= origin_terr_offset;
    }

    //设置原点和目的地-------------- set origin and destination
    return set_wp_origin_and_destination(origin, destination, terrain_alt);
}

---

---

4.避障控制更新

void Copter::update_flight_mode()往下调用

case AVOID_ADSB: //避障模式
avoid_adsb_run();
break;

---

---

《1》avoid_adsb_run();

void Copter::avoid_adsb_run()
{
    // re-use guided mode's velocity controller
    // Note: this is safe from interference from GCSs and companion computer's whose guided mode
    //       position and velocity requests will be ignored while the vehicle is not in guided mode
    guided_run();
}

《2》guided_run()函数

void Copter::guided_run()
{
    //调用正确的自动控制器------------------call the correct auto controller
    switch (guided_mode) 
    {

    case Guided_TakeOff:
        //运行起飞控制器------------------run takeoff controller
        guided_takeoff_run();
        break;

    case Guided_WP:
        //运行位置控制器------------------run position controller
        guided_pos_control_run();
        break;

    case Guided_Velocity:
        //运行速度控制器------------------run velocity controller
        guided_vel_control_run();
        break;

    case Guided_PosVel:
        //运行位置速度控制器--------------run position-velocity controller
        guided_posvel_control_run();
        break;

    case Guided_Angle:
        //运行角度控制器------------------run angle controller
        guided_angle_control_run();
        break;
    }
 }

《2》-1起飞运行代码： guided_takeoff_run();

void Copter::guided_takeoff_run()
{
    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
        // initialise wpnav targets
        wp_nav->shift_wp_origin_to_current_pos();
#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   // multicopters do not stabilize roll/pitch/yaw when disarmed
        motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
        // reset attitude control targets
        attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
        // clear i term when we're taking off
        set_throttle_takeoff();
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!failsafe.radio) 
    {
        // get pilot's desired yaw rate
        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
    }

#if FRAME_CONFIG == HELI_FRAME
    // helicopters stay in landed state until rotor speed runup has finished
    if (motors->rotor_runup_complete()) 
    {
        set_land_complete(false);
    } else {
        // initialise wpnav targets
        wp_nav->shift_wp_origin_to_current_pos();
    }
#else
    set_land_complete(false);
#endif

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // run waypoint controller
    failsafe_terrain_set_status(wp_nav->update_wpnav());

    // call z-axis position controller (wpnav should have already updated it's alt target)
    pos_control->update_z_controller();

    // call attitude controller
    auto_takeoff_attitude_run(target_yaw_rate);
}

《2》-2位置控制代码 guided_pos_control_run();

void Copter::guided_pos_control_run()
{
    // if not auto armed or motors not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
#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
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    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)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // run waypoint controller
    failsafe_terrain_set_status(wp_nav->update_wpnav());

    // call z-axis position controller (wpnav should have already updated it's alt target)
    pos_control->update_z_controller();

    // call attitude controller
    if (auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
        // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
    } else {
        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
    }
}

《2》-3 速度控制代码：guided_vel_control_run();

void Copter::guided_vel_control_run()
{
    // if not auto armed or motors not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
        // initialise velocity controller
        pos_control->init_vel_controller_xyz();
#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
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;
    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)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // set velocity to zero and stop rotating if no updates received for 3 seconds
    uint32_t tnow = millis();
    if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
        if (!pos_control->get_desired_velocity().is_zero()) {
            guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
        }
        if (auto_yaw_mode == AUTO_YAW_RATE) {
            set_auto_yaw_rate(0.0f);
        }
    } else {
        guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
    }

    // call velocity controller which includes z axis controller
    pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);

    // call attitude controller
    if (auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
        // roll & pitch from velocity controller, yaw rate from mavlink command or mission item
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
    } else {
        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
    }
}

《2》-4位置控制代码： guided_posvel_control_run();

void Copter::guided_posvel_control_run()
{
    // if not auto armed or motors not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
        // set target position and velocity to current position and velocity
        pos_control->set_pos_target(inertial_nav.get_position());
        pos_control->set_desired_velocity(Vector3f(0,0,0));
#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
        return;
    }

    // process pilot's yaw input
    float target_yaw_rate = 0;

    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)) {
            set_auto_yaw_mode(AUTO_YAW_HOLD);
        }
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // set velocity to zero and stop rotating if no updates received for 3 seconds
    uint32_t tnow = millis();
    if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
        guided_vel_target_cms.zero();
        if (auto_yaw_mode == AUTO_YAW_RATE) {
            set_auto_yaw_rate(0.0f);
        }
    }

    // calculate dt
    float dt = pos_control->time_since_last_xy_update();

    // update at poscontrol update rate
    if (dt >= pos_control->get_dt_xy()) {
        // sanity check dt
        if (dt >= 0.2f) {
            dt = 0.0f;
        }

        // advance position target using velocity target
        guided_pos_target_cm += guided_vel_target_cms * dt;

        // send position and velocity targets to position controller
        pos_control->set_pos_target(guided_pos_target_cm);
        pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);

        // run position controller
        pos_control->update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
    }

    pos_control->update_z_controller();

    // call attitude controller
    if (auto_yaw_mode == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
    } else if (auto_yaw_mode == AUTO_YAW_RATE) {
        // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain());
    } else {
        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
        attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
    }
}

《2》.5 guided_angle_control_run();

void Copter::guided_angle_control_run()
{
    // if not auto armed or motors not enabled set throttle to zero and exit immediately
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME  // Helicopters always stabilize roll/pitch/yaw
        // call attitude controller
        attitude_control->set_yaw_target_to_current_heading();
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
        attitude_control->set_throttle_out(0.0f,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.0f,true,g.throttle_filt);
#endif
        pos_control->relax_alt_hold_controllers(0.0f);
        return;
    }

    // constrain desired lean angles
    float roll_in = guided_angle_state.roll_cd;
    float pitch_in = guided_angle_state.pitch_cd;
    float total_in = norm(roll_in, pitch_in);
    float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), aparm.angle_max);
    if (total_in > angle_max) {
        float ratio = angle_max / total_in;
        roll_in *= ratio;
        pitch_in *= ratio;
    }

    // wrap yaw request
    float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
    float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);

    // constrain climb rate
    float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_speed_down()), wp_nav->get_speed_up());

    // get avoidance adjusted climb rate
    climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);

    // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
    uint32_t tnow = millis();
    if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
        roll_in = 0.0f;
        pitch_in = 0.0f;
        climb_rate_cms = 0.0f;
        yaw_rate_in = 0.0f;
    }

    // set motors to full range
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // call attitude controller
    if (guided_angle_state.use_yaw_rate) {
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
    } else {
        attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
    }

    // call position controller
    pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
    pos_control->update_z_controller();
}