Ardupilot 刹车模式分析

目录

摘要

本文主要记录自己学习ardupilot飞控代码的刹车模式控制代码过程，欢迎一起交流学习。

1.初始化刹车模式

bool Copter::brake_init(bool ignore_checks)
{
    if (position_ok() || ignore_checks) 
    {

        //设置目标加速度为0--------------set desired acceleration to zero
        wp_nav->clear_pilot_desired_acceleration();

        //设置当前位置为目标位置----------set target to current position
        wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);

        //初始化垂直速度和加速度---------initialize vertical speed and acceleration
        pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
        pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE);

        // 初始化位置和目标速度---------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());
        }

        brake_timeout_ms = 0;

        return true;
    }else{
        return false;
    }
}

---

---

重点分析下这个代码
(1) 清除导航加速度——–wp_nav->clear_pilot_desired_acceleration();
AC_WPNav *wp_nav;

void clear_pilot_desired_acceleration() 
{  _pilot_accel_fwd_cms = 0.0f; 
   _pilot_accel_rgt_cms = 0.0f; 
}

（2）设置目标位置： wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);

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


----------

----------
1》》初始化位置控制器变量
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);
        _pi_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
    init_ekf_xy_reset();
}

3

》》_pos_control.get_stopping_point_xy(stopping_point);

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
//避免果速度低于10cm／s，Kp非常低或加速度为零，则避免使用当前位置除以零。
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);

}

***2》》init_loiter_target(stopping_point);***
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);
    _pos_control.set_accel_xy(_loiter_accel_cmss);
    _pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);

    //设置当前位置为目标位置---------------set target position
    _pos_control.set_xy_target(position.x, position.y);

    // initialise feed forward velocity to zero
    _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;
}

---

---

2.刹车更新代码

// 刹车更新代码，运行周期400hz,2.5ms-------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,0);
        _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, false);
    }
}

---

重点看下这代码

///更新水平位置控制，运行周期10ms---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) / 1000.0f;
    _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);
}