Discussions

I've been working with APMs in both an EZ* and a ParkZone T-28.  The EZ* is flying very well and I'm tuning the T-28. 

Both airplanes have the same behavior when crossing a waypoint.  The Mission Planner track display shows the track going right to the waypoint and then starting a turn to the new waypoint heading.  This is confusing when I look at verify_nav_wp().  That routine appears to return "true" when the airplane enters the waypoint radius circle.  Here's the code I'm looking at, the distance test is bold--

static bool verify_nav_wp()
{
    hold_course = -1;
    update_crosstrack();
    if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
        //SendDebug_P("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
        //SendDebugln(command_must_index,DEC);
        char message[30];
        sprintf(message,"Reached Waypoint #%i",command_must_index);
        gcs.send_text(SEVERITY_LOW,message);
        return true;
    }
    // add in a more complex case
    // Doug to do
    if(loiter_sum > 300){
        gcs.send_text_P(SEVERITY_MEDIUM,PSTR("<verify_must: MAV_CMD_NAV_WAYPOINT> Missed WP"));
        return true;
    }
    return false;
}

At first look, this seems like this code would cause the waypoint selection to move to the next index as soon as wp_distance goes within the circle.  But that's not what I see happening in actual flight.

Full-size airplanes use a technique they call "GPS Steering" (GPSS).  This is an autopilot algorithm that uses air speed and heading change to estimate how far the turn should start before actually crossing the waypoint.  This allows a clean turn from the old track onto the new track.  The APM code seems to suggest this intent but my experience is that the next waypoint turn starts only when the wp_distance has gone to zero.

I'm asking about this because leading the turn to a new waypoint will reduce crosstrack error (no "jug-handle") and that will improve snaking while getting onto the next segment.

Can anyone say if I'm misreading the APM code or is it supposed to lead the turn and just not doing it?