Ardupilot 브레이크 모드 분석

카탈로그

카탈로그

요약

1.브레이크 모드 초기화

2.브레이크 업데이트 코드

요약

본고는 주로 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) 내비게이션 가속도 지우기 – wpnav->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) 목표 위치 설정:wpnav->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);
}