예제 #1
0
/// update_velocity_controller_xy - run the velocity controller - should be called at 100hz or higher
///     velocity targets should we set using set_desired_velocity_xy() method
///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
///     throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler)
{
    // capture time since last iteration
    uint32_t now = AP_HAL::millis();
    float dt = (now - _last_update_xy_ms)*0.001f;

    // call xy controller
    if (dt >= get_dt_xy()) {
        // sanity check dt
        if (dt >= 0.2f) {
            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();

        // apply desired velocity request to position target
        desired_vel_to_pos(dt);

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

        // run velocity to acceleration step
        rate_to_accel_xy(dt, ekfNavVelGainScaler);

        // run acceleration to lean angle step
        accel_to_lean_angles(dt, ekfNavVelGainScaler, false);

        // update xy update time
        _last_update_xy_ms = now;
    }
}
예제 #2
0
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
///     velocity targets should we set using set_desired_velocity_xyz() method
///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
///     throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
{
    // capture time since last iteration
    uint32_t now = hal.scheduler->millis();
    float dt = (now - _last_update_xy_ms) / 1000.0f;

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

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

    // apply desired velocity request to position target
    desired_vel_to_pos(dt);

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

    // run velocity to acceleration step
    rate_to_accel_xy(dt, ekfNavVelGainScaler);

    // run acceleration to lean angle step
    accel_to_lean_angles(dt, ekfNavVelGainScaler, false);

    // update altitude target
    set_alt_target_from_climb_rate(_vel_desired.z, dt, false);

    // run z-axis position controller
    update_z_controller();

    // record update time
    _last_update_xy_ms = now;
}
예제 #3
0
/// 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);
}
예제 #4
0
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(bool use_desired_velocity)
{
    // catch if we've just been started
    uint32_t now = hal.scheduler->millis();
    if ((now - _last_update_ms) >= 1000) {
        _last_update_ms = now;
        if (!_flags.keep_xy_I_terms) {
            reset_I_xy();
        }
        _xy_step = 0;
    }
    // reset keep_xy_I_term flag in case it has been set
    _flags.keep_xy_I_terms = false;

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

    // reset step back to 0 if loiter or waypoint parents have triggered an update and we completed the last full cycle
    if (_flags.force_recalc_xy && _xy_step > 3) {
        _flags.force_recalc_xy = false;
        _xy_step = 0;
    }

    // run loiter steps
    switch (_xy_step) {
        case 0:
            // capture time since last iteration
            _dt_xy = (now - _last_update_ms) / 1000.0f;
            _last_update_ms = now;

            // translate any adjustments from pilot to loiter target
            desired_vel_to_pos(_dt_xy);
            _xy_step++;
            break;
        case 1:
            // run position controller's position error to desired velocity step
            pos_to_rate_xy(use_desired_velocity,_dt_xy);
            _xy_step++;
            break;
        case 2:
            // run position controller's velocity to acceleration step
            rate_to_accel_xy(_dt_xy);
            _xy_step++;
            break;
        case 3:
            // run position controller's acceleration to lean angle step
            accel_to_lean_angles();
            _xy_step++;
            break;
    }
}
예제 #5
0
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
///     velocity targets should we set using set_desired_velocity_xyz() method
///     callers should use get_roll() and get_pitch() methods and sent to the attitude controller
///     throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz()
{
    // capture time since last iteration
    uint32_t now = hal.scheduler->millis();
    float dt_xy = (now - _last_update_vel_xyz_ms) / 1000.0f;

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

    // we will run the horizontal component every 4th iteration (i.e. 50hz on Pixhawk, 20hz on APM)
    if (dt_xy >= POSCONTROL_VEL_UPDATE_TIME) {

        // record update time
        _last_update_vel_xyz_ms = now;

        // sanity check dt
        if (dt_xy >= POSCONTROL_ACTIVE_TIMEOUT_MS) {
            dt_xy = 0.0f;
        }

        // apply desired velocity request to position target
        desired_vel_to_pos(dt_xy);

        // run position controller's position error to desired velocity step
        pos_to_rate_xy(true, dt_xy);

        // run velocity to acceleration step
        rate_to_accel_xy(dt_xy);

        // run acceleration to lean angle step
        accel_to_lean_angles();
    }

    // update altitude target
    set_alt_target_from_climb_rate(_vel_desired.z, _dt);

    // run z-axis position controller
    update_z_controller();
}