I've been playing around with the ArduPlane software 2.68 for a few weeks now and think I have found a few bugs in the navigation code that were preventing it from tracking accurately between waypoints in windy conditions. After correcting these bugs and adding enhancements in the navigation.pde file, the tracking accuracy has improved and the plane compensates much better for windy conditions. In summary I found that:

1) In navigation.pde when flying between way-points in auto the update_crosstrack function which compensates for cross-wind and applies a heading change proportional to cross-track position error was not being called. Instead the target_bearing_cd was being used - basically it was just pointing the nose at the next waypoint.

2) In navigation.pde => update_crosstrack() the wind correction was being applied in the wrong order. The heading to reduce position error should be calculated first and then corrected for cross-wind, not the other way round as was being done by the 2.68 baseline code. 

3) The position error control loop was missing a damping (derivative) term which was causing some snaking unless position error gain was turned right down. What is required is a gain from cross-track velocity to demanded heading

These were fixed and flown in gusty winds of between 15 and 18 kts with my Skywalker X-5 and the tracking was good considering the conditions and early state of autopilot tune (see attached image)

Previous attempts in similar wind condtions gave poor tracking. The following is a summary of the changes I made to navigation.pde. (I have made mods to other files to enable in-flight tuning of the new crosstrack_vel_gain gain). A gain of 1 was all that was needed to provide a smooth track capture.

In static void navigate()

I replaced the line nav_bearing_cd = target_bearing_cd; with update_crosstrack();

In static void update_crosstrack(void) moved the crosswind correction to be applied last and added a cross-track velocity gain term as follows:

static void update_crosstrack(void) // Paul Riseborough (experimental)
// Crosstrack Error - modified to add cross track velocity compensation to reduce overshoot and snaking
// ----------------
// Only apply cross-track error corrections when within +-45 degrees of the destination waypoint and if greater than the min tracking dfistance to go
// Otherwise just point the nose at the next waypoint.
if (wp_totalDistance >= g.crosstrack_min_distance &&
abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
// meters we are off track line (positive to the left)
crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance;
// metres per sec we are moving away from track line (positive to the left)
float crosstrack_vel_error = sin(radians(wrap_180_cd(crosstrack_bearing_cd - g_gps->ground_course)*0.01)) * g_gps->ground_speed*0.01;
nav_bearing_cd = crosstrack_bearing_cd + constrain(crosstrack_error * g.crosstrack_gain + crosstrack_vel_error*g.crosstrack_vel_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
nav_bearing_cd = target_bearing_cd; // Point nose at next waypoint
// if we are using a compass for navigation, then adjust the
// heading to account for wind
if (g.crosstrack_use_wind && compass.use_for_yaw()) {
Vector3f wind = ahrs.wind_estimate();
Vector2f wind2d = Vector2f(wind.x, wind.y);
float speed;
if (ahrs.airspeed_estimate(&speed)) {
Vector2f nav_vector = Vector2f(cos(radians(nav_bearing_cd*0.01)), sin(radians(nav_bearing_cd*0.01))) * speed;
Vector2f nav_adjusted = nav_vector - wind2d;
nav_bearing_cd = degrees(atan2(nav_adjusted.y, nav_adjusted.x)) * 100;


You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • B1

    Have been playing with the Hexicopter lately. I need to get back onto fixed wing again.!!

    Once I figure out how to change it I am keen to give this a go in the X-8. 

    need to play with the files when I am more awake, doesn't seem to want to work after midnight!! I'm doing something wrong or missed something,... I'll try again tomorrow then hopefully out for a test fly in the afternoon! 


  • Does anyone know if your improvements have made it into the master code for 2.69 or 2.70?

  • Wow! This sounds like great work, I'm surprised no one else has commented. Will you be getting this added to the main code base? Where I live it's always windy, so being able to fly in strong winds is important.

    I'm a little surprised by point 1 - I've noticed that my plane does not just aim at the next waypoint in windy conditions, it appears to be pointing into the wind and doing some crab walking to the next waypoint! Perhaps one of the devs can comment on these changes, they seem to highlight some fundamental errors with navigation.


  • Moderator

    My nav with crosstrack never worked correctly, so I always just turned the gain down to 0.

    Keen to try this, Paul, can you post the files you made changes to?

This reply was deleted.