浅谈APM系列-----update_flight_mode（ModeBrake）

update_flight_mode（ModeBrake）

这里只看ModeBrake。

位置：X:\ardupilot\ArduCopter\mode.cpp

// update_flight_mode - calls the appropriate attitude controllers based on flight mode（根据飞行模式调用适当的姿态控制器）
// called at 100hz or more
void Copter::update_flight_mode()
{
    // Update EKF speed limit - used to limit speed when we are using optical flow（当我们使用光流时，用来限制速度）
    ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);//只是重新计算了一下这两值

    flightmode->run();//阅读  201808181419
	/*需要这里的flightmode，会随着飞行模式的切换调用不同的代码*/
}

刹车模式控制代码，最开始的地方。 

位置：X:\ardupilot\ArduCopter\mode_brake.cpp

// brake_run - runs the brake controller（运行制动控制器）
// should be called at 100hz or more
void Copter::ModeBrake::run()
{
    // if not auto armed set throttle to zero and exit immediately（如果不是自动控制的，将油门控制为零，立即退出）
    if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
        wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
        zero_throttle_and_relax_ac();
        pos_control->relax_alt_hold_controllers(0.0f);
        return;
    }

    // relax stop target if we might be landed（如果我们能降落，放松停止目标）
    if (ap.land_complete_maybe) {
        wp_nav->loiter_soften_for_landing();
    }

    // if landed immediately disarm（如果着陆立即解除）
    if (ap.land_complete) {
        copter.init_disarm_motors();
    }

    // set motors to full range（将电动机设置为全范围）
    motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // run brake controller（运行制动控制器）
    wp_nav->update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);

    // call attitude controller（调用控制器的态度）
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain());

    // body-frame rate controller is run directly from 100hz loop

    // update altitude target and call position controller（更新高度目标和呼叫位置控制器）
    pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
    pos_control->update_z_controller();

    if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
        if (!copter.set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
            copter.set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT);
        }
    }
}

---

 

/// init_brake_target - initializes stop position from current position and velocity
void AC_WPNav::init_brake_target(float accel_cmss)
{
    const Vector3f& curr_vel = _inav.get_velocity();
    Vector3f stopping_point;

    // initialise position controller
    _pos_control.init_xy_controller();

    // initialise pos controller speed and acceleration
    _pos_control.set_speed_xy(curr_vel.length());
    _pos_control.set_accel_xy(accel_cmss);
    _pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
    _pos_control.calc_leash_length_xy();

    _pos_control.get_stopping_point_xy(stopping_point);

    // set target position
    init_loiter_target(stopping_point);
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.h

/// init_xy_controller - initialise the xy controller
    ///     sets target roll angle, pitch angle and I terms based on vehicle current lean angles
    ///     should be called once whenever significant changes to the position target are made
    ///     this does not update the xy target
    void init_xy_controller(bool reset_I = true);

---

 

/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
///     should be called whenever the speed, acceleration or position kP is modified
void AC_PosControl::calc_leash_length_xy()
{
    if (_flags.recalc_leash_xy) {
        _leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP());
        _flags.recalc_leash_xy = false;
    }
}

 

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{
    float leash_length;

    // sanity check acceleration and avoid divide by zero
    if (accel_cms <= 0.0f) {
        accel_cms = POSCONTROL_ACCELERATION_MIN;
    }

    // avoid divide by zero
    if (kP <= 0.0f) {
        return POSCONTROL_LEASH_LENGTH_MIN;//100.0f  // minimum leash lengths in cm
    }

    // calculate leash length（计算刹车长度）
    if(speed_cms <= accel_cms / kP) {
        // linear leash length based on speed close in（基于速度接近的线性皮带长度）
        leash_length = speed_cms / kP;
    }else{
        // leash length grows at sqrt of speed further out（刹车长度以更快的速度增长）
        leash_length = (accel_cms / (2.0f*kP*kP)) + (speed_cms*speed_cms / (2.0f*accel_cms));
    }

    // ensure leash is at least 1m long(确保刹车至少有1米长)
    if( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {
        leash_length = POSCONTROL_LEASH_LENGTH_MIN;
    }

    return leash_length;
}

 

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// get_stopping_point_xy - calculates stopping point based on current position, velocity, vehicle acceleration
///     distance_max allows limiting distance to stopping point
///     results placed in stopping_position vector
///     set_accel_xy() should be called before this method to set vehicle acceleration
///     set_leash_length() should have been called before this method
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();//位置控制的P参数

    // 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
	// 如果速度低于10厘米/s，kP很低或者加速度为0，避免除以0
    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;
    }

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

---

位置：X:\ardupilot\libraries\AC_WPNav\AC_WPNav.h

/// init_loiter_target to a position in cm from ekf origin
    ///     caller can set reset_I to false to preserve I term since previous time loiter controller ran.  Should only be false when caller is sure that not too much time has passed to invalidate the I terms
    void init_loiter_target(const Vector3f& position, bool reset_I=true);

位置：X:\ardupilot\libraries\AC_WPNav\AC_WPNav.cpp

/// init_loiter_target in cm from home
void AC_WPNav::init_loiter_target(const Vector3f& position, bool reset_I)
{
    // initialise position controller
    _pos_control.init_xy_controller(reset_I);

    // initialise pos controller speed, acceleration and jerk
    _pos_control.set_speed_xy(_loiter_speed_cms);//设置XY方向的速度，并重新计算刹车距离
    _pos_control.set_accel_xy(_loiter_accel_cmss);//设置XY方向的加速度，并重新计算刹车距离
    _pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);

    // set target position
    _pos_control.set_xy_target(position.x, position.y);//设置XY方向的目标位置

    // initialise feed forward velocity to zero（初始化前馈速度为0）
    _pos_control.set_desired_velocity_xy(0,0);

    // initialise desired accel and add fake wind
    _loiter_desired_accel.x = 0;
    _loiter_desired_accel.y = 0;

    // initialise pilot input
    _pilot_accel_fwd_cms = 0;
    _pilot_accel_rgt_cms = 0;
}

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// init_xy_controller - initialise the xy controller
///     sets target roll angle, pitch angle and I terms based on vehicle current lean angles
///     should be called once whenever significant changes to the position target are made
///     this does not update the xy target
void AC_PosControl::init_xy_controller(bool reset_I)
{
    // set roll, pitch lean angle targets to current attitude
    _roll_target = _ahrs.roll_sensor;
    _pitch_target = _ahrs.pitch_sensor;

    // initialise I terms from lean angles
    if (reset_I) {
        // reset last velocity if this controller has just been engaged or dt is zero
        lean_angles_to_accel(_accel_target.x, _accel_target.y);
        _pid_vel_xy.set_integrator(_accel_target);
    }

    // flag reset required in rate to accel step
    _flags.reset_desired_vel_to_pos = true;
    _flags.reset_rate_to_accel_xy = true;
    _flags.reset_accel_to_lean_xy = true;

    // initialise ekf xy reset handler（初始化ekf xy重置处理程序）
    init_ekf_xy_reset();
}

/// initialise ekf xy position reset check
void AC_PosControl::init_ekf_xy_reset()
{
    Vector2f pos_shift;
    _ekf_xy_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
}

---

位置：X:\ardupilot\ArduCopter\mode.cpp

void Copter::Mode::zero_throttle_and_relax_ac()
{
#if FRAME_CONFIG == HELI_FRAME
    // Helicopters always stabilize roll/pitch/yaw
    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, copter.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, copter.g.throttle_filt);
#endif
}

 

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

// Set output throttle and disable stabilization（设置油门输出，禁止自稳）
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{
    _throttle_in = throttle_in;//设置姿态控制器油门输入为0
    _motors.set_throttle_filter_cutoff(filter_cutoff);
    if (reset_attitude_control) {//TRUE
        relax_attitude_controllers();
    }
    _motors.set_throttle(throttle_in);//0
    _angle_boost = 0.0f;
}

 

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

// Ensure attitude controller have zero errors to relax rate controller output（确保姿态控制器有零错误，以放松速率控制器输出）
void AC_AttitudeControl::relax_attitude_controllers()
{
    // TODO add _ahrs.get_quaternion()
    _attitude_target_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());//重新设置姿态控制的目标四元数
    _attitude_target_ang_vel = _ahrs.get_gyro();//重新设置姿态控制的目标角速度
    _attitude_target_euler_angle = Vector3f(_ahrs.roll, _ahrs.pitch, _ahrs.yaw);//设置姿态控制的目标欧拉角

    // Set reference angular velocity used in angular velocity controller equal
    // to the input angular velocity and reset the angular velocity integrators.
    // This zeros the output of the angular velocity controller.
    _rate_target_ang_vel = _ahrs.get_gyro();//设置速度环，目标角速度
    get_rate_roll_pid().reset_I();
    get_rate_pitch_pid().reset_I();
    get_rate_yaw_pid().reset_I();//设置三个轴方向的PID的积分项为0
}

 

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// relax_alt_hold_controllers - set all desired and targets to measured
void AC_PosControl::relax_alt_hold_controllers(float throttle_setting)
{
    _pos_target.z = _inav.get_altitude();
    _vel_desired.z = 0.0f;
    _flags.use_desvel_ff_z = false;
    _vel_target.z = _inav.get_velocity_z();
    _vel_last.z = _inav.get_velocity_z();
    _accel_feedforward.z = 0.0f;
    _accel_last_z_cms = 0.0f;
    _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;
    _flags.reset_accel_to_throttle = true;
    _pid_accel_z.set_integrator((throttle_setting-_motors.get_throttle_hover())*1000.0f);
}

---

回到最初，继续追代码。

位置：X:\ardupilot\libraries\AC_WPNav\AC_WPNav.cpp

/// loiter_soften_for_landing - reduce response for landing
void AC_WPNav::loiter_soften_for_landing()
{
    const Vector3f& curr_pos = _inav.get_position();

    // set target position to current position
    _pos_control.set_xy_target(curr_pos.x, curr_pos.y);
    _pos_control.freeze_ff_xy();
}

 

---

位置：X:\ardupilot\libraries\AC_WPNav\AC_WPNav.cpp

// update_brake - run the stop controller - gets called at 400hz
void AC_WPNav::update_brake(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
    // calculate dt
    float dt = _pos_control.time_since_last_xy_update();//距离上次计算的时间差

    // run at poscontrol update rate.
    if (dt >= _pos_control.get_dt_xy()) {//控制执行的频率

        // send adjusted feed forward velocity back to position controller（将调整后的前馈速度发送回位置控制器）
        _pos_control.set_desired_velocity_xy(0.0f, 0.0f);
        _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, false);
    }
}

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
    // compute dt
    uint32_t now = AP_HAL::millis();
    float dt = (now - _last_update_xy_ms)*0.001f;
    _last_update_xy_ms = now;

    // sanity check dt - expect to be called faster than ~5hz
    if (dt > POSCONTROL_ACTIVE_TIMEOUT_MS*1.0e-3f) {
        dt = 0.0f;
    }

    // check for ekf xy position reset
    check_for_ekf_xy_reset();

    // check if xy leash needs to be recalculated
    calc_leash_length_xy();

    // translate any adjustments from pilot to loiter target
    desired_vel_to_pos(dt);

    // run position controller's position error to desired velocity step
    pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);

    // run position controller's velocity to acceleration step
    rate_to_accel_xy(dt, ekfNavVelGainScaler);

    // run position controller's acceleration to lean angle step
    accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);
}

---

G:\ardupilot\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// check for ekf position reset and adjust loiter or brake target position
void AC_PosControl::check_for_ekf_xy_reset()
{
    // check for position shift
    Vector2f pos_shift;
    uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
    if (reset_ms != _ekf_xy_reset_ms) {
        shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);
        _ekf_xy_reset_ms = reset_ms;
    }
}

G:\ardupilot\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// check for ekf position reset and adjust loiter or brake target position
void AC_PosControl::check_for_ekf_xy_reset()
{
    // check for position shift
    Vector2f pos_shift;
    uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
    if (reset_ms != _ekf_xy_reset_ms) {
        shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);
        _ekf_xy_reset_ms = reset_ms;
    }
}

---

G:\ardupilot\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration
///     should be called whenever the speed, acceleration or position kP is modified
void AC_PosControl::calc_leash_length_xy()
{
    if (_flags.recalc_leash_xy) {
        _leash = calc_leash_length(_speed_cms, _accel_cms, _p_pos_xy.kP());
        _flags.recalc_leash_xy = false;
    }
}

G:\ardupilot\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float AC_PosControl::calc_leash_length(float speed_cms, float accel_cms, float kP) const
{
    float leash_length;

    // sanity check acceleration and avoid divide by zero
    if (accel_cms <= 0.0f) {
        accel_cms = POSCONTROL_ACCELERATION_MIN;
    }

    // avoid divide by zero
    if (kP <= 0.0f) {
        return POSCONTROL_LEASH_LENGTH_MIN;//100.0f  // minimum leash lengths in cm
    }

    // calculate leash length（计算刹车长度）
    if(speed_cms <= accel_cms / kP) {
        // linear leash length based on speed close in（基于速度接近的线性皮带长度）
        leash_length = speed_cms / kP;
    }else{
        // leash length grows at sqrt of speed further out（刹车长度以更快的速度增长）
        leash_length = (accel_cms / (2.0f*kP*kP)) + (speed_cms*speed_cms / (2.0f*accel_cms));
    }

    // ensure leash is at least 1m long(确保刹车至少有1米长)
    if( leash_length < POSCONTROL_LEASH_LENGTH_MIN ) {
        leash_length = POSCONTROL_LEASH_LENGTH_MIN;
    }

    return leash_length;
}

 

---

G:\ardupilot\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// desired_vel_to_pos - move position target using desired velocities（使用期望速度移动位置目标）
void AC_PosControl::desired_vel_to_pos(float nav_dt)
{
    // range check nav_dt
    if( nav_dt < 0 ) {
        return;
    }

    // update target position（更新目标位置）
    if (_flags.reset_desired_vel_to_pos) {
        _flags.reset_desired_vel_to_pos = false;
    } else {
        _pos_target.x += _vel_desired.x * nav_dt;
        _pos_target.y += _vel_desired.y * nav_dt;
    }
}

---

 

/// pos_to_rate_xy - horizontal position error to velocity controller
///     converts position (_pos_target) to target velocity (_vel_target)
///     when use_desired_rate is set to true:
///         desired velocity (_vel_desired) is combined into final target velocity and
///         velocity due to position error is reduce to a maximum of 1m/s
void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainScaler)
{
    Vector3f curr_pos = _inav.get_position();
    float linear_distance;      // the distance we swap between linear and sqrt velocity response
    float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF

    // avoid divide by zero
    if (kP <= 0.0f) {
        _vel_target.x = 0.0f;
        _vel_target.y = 0.0f;
    }else{
        // calculate distance error
        _pos_error.x = _pos_target.x - curr_pos.x;//期望位置和实际位置的偏差
        _pos_error.y = _pos_target.y - curr_pos.y;

        // constrain target position to within reasonable distance of current location（将目标位置限制在当前位置的合理距离内）
        _distance_to_target = norm(_pos_error.x, _pos_error.y);
        if (_distance_to_target > _leash && _distance_to_target > 0.0f) {
            _pos_target.x = curr_pos.x + _leash * _pos_error.x/_distance_to_target;//重新计算了一下目标位置
            _pos_target.y = curr_pos.y + _leash * _pos_error.y/_distance_to_target;
            // re-calculate distance error（计算距离误差）
            _pos_error.x = _pos_target.x - curr_pos.x;
            _pos_error.y = _pos_target.y - curr_pos.y;
            _distance_to_target = _leash;
        }

        // calculate the distance at which we swap between linear and sqrt velocity response
		// 计算我们在线性和sqrt速度响应之间交换的距离
        linear_distance = _accel_cms/(2.0f*kP*kP);

        if (_distance_to_target > 2.0f*linear_distance) {
            // velocity response grows with the square root of the distance（速度响应随着距离的平方根增长）
            float vel_sqrt = safe_sqrt(2.0f*_accel_cms*(_distance_to_target-linear_distance));
            _vel_target.x = vel_sqrt * _pos_error.x/_distance_to_target;//计算目标速度
            _vel_target.y = vel_sqrt * _pos_error.y/_distance_to_target;
        }else{
            // velocity response grows linearly with the distance（速度响应随着距离线性增长）
            _vel_target.x = kP * _pos_error.x;//根据位置的偏差，计算期望速度
            _vel_target.y = kP * _pos_error.y;
        }

        if (mode == XY_MODE_POS_LIMITED_AND_VEL_FF) {
            // this mode is for loiter - rate-limiting the position correction
            // allows the pilot to always override the position correction in
            // the event of a disturbance

            // scale velocity within limit
            float vel_total = norm(_vel_target.x, _vel_target.y);
            if (vel_total > POSCONTROL_VEL_XY_MAX_FROM_POS_ERR) {//200
                _vel_target.x = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.x/vel_total;
                _vel_target.y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.y/vel_total;
            }

            // add velocity feed-forward（添加速度前馈）
            _vel_target.x += _vel_desired.x;
            _vel_target.y += _vel_desired.y;
        } else {
            if (mode == XY_MODE_POS_AND_VEL_FF) {
                // add velocity feed-forward
                _vel_target.x += _vel_desired.x;
                _vel_target.y += _vel_desired.y;
            }

            // scale velocity within speed limit
            float vel_total = norm(_vel_target.x, _vel_target.y);
            if (vel_total > _speed_cms) {// max horizontal speed in cm/s
                _vel_target.x = _speed_cms * _vel_target.x/vel_total;
                _vel_target.y = _speed_cms * _vel_target.y/vel_total;
            }
        }
    }
}

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// rate_to_accel_xy - horizontal desired rate to desired acceleration converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler)
{
    Vector2f vel_xy_p, vel_xy_i, vel_xy_d;//两个方向速度的，P项、I项、D项

    // reset last velocity target to current target（将最后速度目标重置为当前目标）
    if (_flags.reset_rate_to_accel_xy) {
        _vel_last.x = _vel_target.x;
        _vel_last.y = _vel_target.y;
        _flags.reset_rate_to_accel_xy = false;
    }

    // check if vehicle velocity is being overridden
    if (_flags.vehicle_horiz_vel_override) {
        _flags.vehicle_horiz_vel_override = false;
    } else {
        _vehicle_horiz_vel.x = _inav.get_velocity().x;//获取水平速度
        _vehicle_horiz_vel.y = _inav.get_velocity().y;
    }

    // feed forward desired acceleration calculation（前馈加速度计算）
    if (dt > 0.0f) {
    	if (!_flags.freeze_ff_xy) {
    		_accel_feedforward.x = (_vel_target.x - _vel_last.x)/dt;//计算前馈加速度的值
    		_accel_feedforward.y = (_vel_target.y - _vel_last.y)/dt;
        } else {
    		// stop the feed forward being calculated during a known discontinuity
    		_flags.freeze_ff_xy = false;
    	}
    } else {
    	_accel_feedforward.x = 0.0f;
    	_accel_feedforward.y = 0.0f;
    }

    // store this iteration's velocities for the next iteration（为下一次迭代存储这个迭代的速度）
    _vel_last.x = _vel_target.x;
    _vel_last.y = _vel_target.y;

    // calculate velocity error （计算速度误差）
    _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;//计算速度偏差
    _vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;

    // call pi controller
    _pid_vel_xy.set_input(_vel_error);//对两个方向的偏差进行PID

    // get p
    vel_xy_p = _pid_vel_xy.get_p();

    // update i term if we have not hit the accel or throttle limits OR the i term will reduce
	// 更新i项,如果我们没有达到accel或油门限制或者i项将会减少
    if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
        vel_xy_i = _pid_vel_xy.get_i();
    } else {
        vel_xy_i = _pid_vel_xy.get_i_shrink();
    }

    // get d
    vel_xy_d = _pid_vel_xy.get_d();

    // combine feed forward accel with PID output from velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
	// 将馈送和速度误差和PID输出的PID输出结合起来，以补偿光流量测量引起的EKF噪声
    _accel_target.x = _accel_feedforward.x + (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;//计算加速度的期望值
    _accel_target.y = _accel_feedforward.y + (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
}

 

 

---

  位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// accel_to_lean_angles - horizontal desired acceleration to lean angles（水平期望加速度到倾斜角度）
///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
    float accel_total;                          // total acceleration in cm/s/s
    float accel_right, accel_forward;
    float lean_angle_max = _attitude_control.lean_angle_max();
    float accel_max = POSCONTROL_ACCEL_XY_MAX;//980.0f

    // limit acceleration if necessary（必要时极限加速度）
    if (use_althold_lean_angle) {
        accel_max = MIN(accel_max, GRAVITY_MSS * 100.0f * tanf(ToRad(constrain_float(_attitude_control.get_althold_lean_angle_max(),1000,8000)/100.0f)));
    }

    // scale desired acceleration if it's beyond acceptable limit（如果超出可接受限度，则需要加速）
    accel_total = norm(_accel_target.x, _accel_target.y);//水平方向的目标加速度和
    if (accel_total > accel_max && accel_total > 0.0f) {
        _accel_target.x = accel_max * _accel_target.x/accel_total;
        _accel_target.y = accel_max * _accel_target.y/accel_total;
        _limit.accel_xy = true;     // unused
    } else {
        // reset accel limit flag（重置accel限制标志）
        _limit.accel_xy = false;
    }

    // reset accel to current desired acceleration（将accel重置为当前所需的加速度）
    if (_flags.reset_accel_to_lean_xy) {
        _accel_target_jerk_limited.x = _accel_target.x;
        _accel_target_jerk_limited.y = _accel_target.y;
        _accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
        _flags.reset_accel_to_lean_xy = false;
    }

    // apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
    float max_delta_accel = dt * _jerk_cmsss;

    Vector2f accel_in(_accel_target.x, _accel_target.y);
    Vector2f accel_change = accel_in-_accel_target_jerk_limited;
    float accel_change_length = accel_change.length();

    if(accel_change_length > max_delta_accel) {
        accel_change *= max_delta_accel/accel_change_length;
    }
    _accel_target_jerk_limited += accel_change;

    // lowpass filter on NE accel
    _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
    Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);//低通滤波

    // rotate accelerations into body forward-right frame（将加速度旋转到身体前向右框架）
    accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw();//向前
    accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();//向右

    // update angle targets that will be passed to stabilize controller（将传递给稳定控制器的更新角度目标）
    _pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
    float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
    _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
}

---

 

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_AttitudeControl.cpp

// Command {an euler roll and pitch angle} and {an euler yaw rate} with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
    // Convert from centidegrees on public interface to radians
    float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
    float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
    float euler_yaw_rate = radians(euler_yaw_rate_cds*0.01f);

    // calculate the attitude target euler angles（计算姿态目标欧拉角,这是根据上次的期望姿态的四元数计算上次期望的欧拉角）
    _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

    // ensure smoothing gain can not cause overshoot
    smoothing_gain = constrain_float(smoothing_gain,1.0f,1/_dt);

    // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors)
    euler_roll_angle += get_roll_trim_rad();//返回0

    if (_rate_bf_ff_enabled & _use_ff_and_input_shaping) {
        // translate the roll pitch and yaw acceleration limits to the euler axis（将滚动距和偏航加速度限制转换为欧拉轴）
        Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));

        // When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
        // angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
        // and an exponential decay specified by smoothing_gain at the end.
        _attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle-_attitude_target_euler_angle.x), smoothing_gain, euler_accel.x, _attitude_target_euler_rate.x, _dt);
        _attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle-_attitude_target_euler_angle.y), smoothing_gain, euler_accel.y, _attitude_target_euler_rate.y, _dt);

        // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing
        // the output rate towards the input rate.
        _attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z);

        // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward
		// 将理想姿态的欧拉角导数转换为前馈线角速度矢量
        euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);//上次期望姿态的欧拉角、期望的欧拉角速度，期望的角速度
    } else {
        // When feedforward is not enabled, the target euler angle is input into the target and the feedforward rate is zeroed.
		// 当前馈没有启用时，目标欧拉角是输入目标，前馈率为零。
        _attitude_target_euler_angle.x = euler_roll_angle;
        _attitude_target_euler_angle.y = euler_pitch_angle;
        _attitude_target_euler_angle.z += euler_yaw_rate*_dt;
        // Compute quaternion target attitude
        _attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);

        // Set rate feedforward requests to zero
        _attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
        _attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
    }

    // Call quaternion attitude controller
    attitude_controller_run_quat();
}

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

/// set_alt_target_from_climb_rate_ff - adjusts target up or down using a climb rate in cm/s using feed-forward
///     should be called continuously (with dt set to be the expected time between calls)
///     actual position target will be moved no faster than the speed_down and speed_up
///     target will also be stopped if the motors hit their limits or leash length is exceeded
///     set force_descend to true during landing to allow target to move low enough to slow the motors
void AC_PosControl::set_alt_target_from_climb_rate_ff(float climb_rate_cms, float dt, bool force_descend)
{
    // calculated increased maximum acceleration if over speed
    float accel_z_cms = _accel_z_cms;
    if (_vel_desired.z < _speed_down_cms && !is_zero(_speed_down_cms)) {
        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_down_cms;
    }
    if (_vel_desired.z > _speed_up_cms && !is_zero(_speed_up_cms)) {
        accel_z_cms *= POSCONTROL_OVERSPEED_GAIN_Z * _vel_desired.z / _speed_up_cms;
    }
    accel_z_cms = constrain_float(accel_z_cms, 0.0f, 750.0f);//速度限制

    // jerk_z is calculated to reach full acceleration in 1000ms.
    float jerk_z = accel_z_cms * POSCONTROL_JERK_RATIO;// Defines the time it takes to reach the requested acceleration

    float accel_z_max = MIN(accel_z_cms, safe_sqrt(2.0f*fabsf(_vel_desired.z - climb_rate_cms)*jerk_z));

    _accel_last_z_cms += jerk_z * dt;
    _accel_last_z_cms = MIN(accel_z_max, _accel_last_z_cms);

    float vel_change_limit = _accel_last_z_cms * dt;
    _vel_desired.z = constrain_float(climb_rate_cms, _vel_desired.z-vel_change_limit, _vel_desired.z+vel_change_limit);//修改期望的Z轴速度
    _flags.use_desvel_ff_z = true;

    // adjust desired alt if motors have not hit their limits（如果发动机没有达到极限，就调整理想的alt值）
    // To-Do: add check of _limit.pos_down?
    if ((_vel_desired.z<0 && (!_motors.limit.throttle_lower || force_descend)) || (_vel_desired.z>0 && !_motors.limit.throttle_upper && !_limit.pos_up)) {
        _pos_target.z += _vel_desired.z * dt;//修正Z轴目标位置
    }
}

---

 

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

 

/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
    // check time since last cast
    uint32_t now = AP_HAL::millis();
    if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
        _flags.reset_rate_to_accel_z = true;
        _flags.reset_accel_to_throttle = true;
    }
    _last_update_z_ms = now;

    // check for ekf altitude reset
    check_for_ekf_z_reset();

    // check if leash lengths need to be recalculated
    calc_leash_length_z();

    // call position controller（调用位置控制器）
    pos_to_rate_z();
}

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

// pos_to_rate_z - position to rate controller for Z axis
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
void AC_PosControl::pos_to_rate_z()
{
    float curr_alt = _inav.get_altitude();

    // clear position limit flags
    _limit.pos_up = false;
    _limit.pos_down = false;

    // calculate altitude error
    _pos_error.z = _pos_target.z - curr_alt;

    // do not let target altitude get too far from current altitude
    if (_pos_error.z > _leash_up_z) {
        _pos_target.z = curr_alt + _leash_up_z;
        _pos_error.z = _leash_up_z;
        _limit.pos_up = true;
    }
    if (_pos_error.z < -_leash_down_z) {
        _pos_target.z = curr_alt - _leash_down_z;
        _pos_error.z = -_leash_down_z;
        _limit.pos_down = true;
    }

    // calculate _vel_target.z using from _pos_error.z using sqrt controller
    _vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);

    // check speed limits
    // To-Do: check these speed limits here or in the pos->rate controller
    _limit.vel_up = false;
    _limit.vel_down = false;
    if (_vel_target.z < _speed_down_cms) {// max descent rate in cm/s
        _vel_target.z = _speed_down_cms;
        _limit.vel_down = true;
    }
    if (_vel_target.z > _speed_up_cms) {// max climb rate in cm/s
        _vel_target.z = _speed_up_cms;
        _limit.vel_up = true;
    }

    // add feed forward component(添加前馈组件)
    if (_flags.use_desvel_ff_z) {
        _vel_target.z += _vel_desired.z;
    }

    // call rate based throttle controller which will update accel based throttle controller targets
    rate_to_accel_z();
}

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

// rate_to_accel_z - calculates desired accel required to achieve the velocity target
// calculates desired acceleration and calls accel throttle controller
void AC_PosControl::rate_to_accel_z()
{
    const Vector3f& curr_vel = _inav.get_velocity();
    float p;                                // used to capture pid values for logging

    // reset last velocity target to current target
    if (_flags.reset_rate_to_accel_z) {
        _vel_last.z = _vel_target.z;
    }

    // feed forward desired acceleration calculation
    if (_dt > 0.0f) {
    	if (!_flags.freeze_ff_z) {
    		_accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt;
        } else {
    		// stop the feed forward being calculated during a known discontinuity
    		_flags.freeze_ff_z = false;
    	}
    } else {
    	_accel_feedforward.z = 0.0f;
    }

    // store this iteration's velocities for the next iteration
    _vel_last.z = _vel_target.z;

    // reset velocity error and filter if this controller has just been engaged
	// 如果这个控制器刚刚投入使用，则会出现速度错误和过滤器
    if (_flags.reset_rate_to_accel_z) {
        // Reset Filter
        _vel_error.z = 0;
        _vel_error_filter.reset(0);
        _flags.reset_rate_to_accel_z = false;
    } else {
        // calculate rate error and filter with cut off frequency of 2 Hz
        _vel_error.z = _vel_error_filter.apply(_vel_target.z - curr_vel.z, _dt);
    }

    // calculate p
    p = _p_vel_z.kP() * _vel_error.z;//P项

    // consolidate and constrain target acceleration
    _accel_target.z = _accel_feedforward.z + p;//P项 + 限流

    // set target for accel based throttle controller
    accel_to_throttle(_accel_target.z);
}

---

位置：X:\ardupilot\libraries\AC_AttitudeControl\AC_PosControl.cpp

// accel_to_throttle - alt hold's acceleration controller
// calculates a desired throttle which is sent directly to the motors
void AC_PosControl::accel_to_throttle(float accel_target_z)
{
    float z_accel_meas;         // actual acceleration
    float p,i,d;              // used to capture pid values for logging

    // Calculate Earth Frame Z acceleration（计算地球框架Z加速度）
    z_accel_meas = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f;

    // reset target altitude if this controller has just been engaged
    if (_flags.reset_accel_to_throttle) {
        // Reset Filter
        _accel_error.z = 0;
        _flags.reset_accel_to_throttle = false;
    } else {
        // calculate accel error
        _accel_error.z = accel_target_z - z_accel_meas;//计算偏差
    }

    // set input to PID
    _pid_accel_z.set_input_filter_all(_accel_error.z);
    _pid_accel_z.set_desired_rate(accel_target_z);

    // separately calculate p, i, d values for logging
    p = _pid_accel_z.get_p();

    // get i term
    i = _pid_accel_z.get_integrator();

    // ensure imax is always large enough to overpower hover throttle
    if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) {
        _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f);
    }

    // update i term as long as we haven't breached the limits or the I term will certainly reduce
    // To-Do: should this be replaced with limits check from attitude_controller?
    if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {
        i = _pid_accel_z.get_i();
    }

    // get d term
    d = _pid_accel_z.get_d();

    float thr_out = (p+i+d)*0.001f +_motors.get_throttle_hover();//悬停油门 + PID

    // send throttle to attitude controller with angle boost
    _attitude_control.set_throttle_out(thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ);
}

---

好了，今天就先到这里吧。。。。。