Example #1
0
/// update_spline - update spline controller
void AC_WPNav::update_spline()
{
    // exit immediately if this is not a spline segment
    if (_flags.segment_type != SEGMENT_SPLINE) {
        return;
    }

    // calculate dt
    uint32_t now = hal.scheduler->millis();
    float dt = (now - _wp_last_update) / 1000.0f;

    // reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
    if (dt > 0.095f) {
        // double check dt is reasonable
        if (dt >= 1.0f) {
            dt = 0.0;
        }
        // capture time since last iteration
        _wp_last_update = now;

        // advance the target if necessary
        advance_spline_target_along_track(dt);
        _pos_control.trigger_xy();
    }else{
        // run position controller
        _pos_control.update_pos_controller(false);
    }
}
Example #2
0
/// update_spline - update spline controller
bool AC_WPNav::update_spline()
{
    // exit immediately if this is not a spline segment
    if (_flags.segment_type != SEGMENT_SPLINE) {
        return false;
    }

    bool ret = true;

    // calculate dt
    float dt = _pos_control.time_since_last_xy_update();
    if (dt >= 0.2f) {
        dt = 0.0f;
    }

    // advance the target if necessary
    if (!advance_spline_target_along_track(dt)) {
        // To-Do: handle failure to advance along track (due to missing terrain data)
        ret = false;
    }

    // freeze feedforwards during known discontinuities
    if (_flags.new_wp_destination) {
        _flags.new_wp_destination = false;
        _pos_control.freeze_ff_z();
    }

    // run horizontal position controller
    _pos_control.update_xy_controller();

    _wp_last_update = AP_HAL::millis();

    return ret;
}
Example #3
0
/// update_spline - update spline controller
void AC_WPNav::update_spline()
{
    // exit immediately if this is not a spline segment
    if (_flags.segment_type != SEGMENT_SPLINE) {
        return;
    }

    float dt = _pos_control.time_since_last_xy_update();

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

        // advance the target if necessary
        advance_spline_target_along_track(dt);

        // freeze feedforwards during known discontinuities
        // TODO: why always consider Z axis discontinuous?
        if (_flags.new_wp_destination) {
            _flags.new_wp_destination = false;
            _pos_control.freeze_ff_xy();
        }
        _pos_control.freeze_ff_z();

        // run horizontal position controller
        _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f);

        _wp_last_update = hal.scheduler->millis();
    }
}
Example #4
0
/// update_spline - update spline controller
bool AC_WPNav::update_spline()
{
    // exit immediately if this is not a spline segment
    if (_flags.segment_type != SEGMENT_SPLINE) {
        return false;
    }

    float dt = _pos_control.time_since_last_xy_update();
    bool ret = true;

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

        // advance the target if necessary
        if (!advance_spline_target_along_track(dt)) {
            // To-Do: handle failure to advance along track (due to missing terrain data)
            ret = false;
        }

        // freeze feedforwards during known discontinuities
        // TODO: why always consider Z axis discontinuous?
        if (_flags.new_wp_destination) {
            _flags.new_wp_destination = false;
            _pos_control.freeze_ff_xy();
        }
        _pos_control.freeze_ff_z();

        // run horizontal position controller
        _pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_ONLY, 1.0f, false);

        _wp_last_update = AP_HAL::millis();
    }
    return ret;
}