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