Rover fast waypoints?

Does the rover code offer the “fast” waypoints option like copter does?

If you mean Write WP Fast it does. Works in SITL.

Waypoints where if the intermediate target is past the current waypoint the vehicle starts to track to the next waypoint instead of waiting to hit the current waypoint’s radius.

Basically the vehicle starts to turn earlier before those waypoints approached at high velocity compared to those approached at low velocity.

From AC_WPNav.cpp:

// check if we've reached the waypoint
    if( !_flags.reached_destination ) {
        if( _track_desired >= _track_length ) {
            // "fast" waypoints are complete once the intermediate point reaches the destination
            if (_flags.fast_waypoint) {
                _flags.reached_destination = true;
            }else{
                // regular waypoints also require the copter to be within the waypoint radius
                Vector3f dist_to_dest = curr_pos - _destination;
                if( dist_to_dest.length() <= _wp_radius_cm ) {
                    _flags.reached_destination = true;
                }
            }
        }
    }

@autoboat, no, Rover doesn’t have “fast waypoints” so it will only start to turn once it is WP_RADIUS from the waypoint. For reference, it’s navigation is more like Plane’s rather than Copter’s and even uses the same “L1 controller”. Overall Rover’s software is structured much like Copter but the highest level navigation is still done like Plane.

1 Like