Improvements to navigation tracking - successful test today

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);
}
else
{
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;
}
}

 

Views: 780

Attachments:

Replies to This Discussion


ModeratorPermalink Reply by Graham Dyer on February 10, 2013 at 12:59am

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?

Permalink Reply by paul riseborough on February 10, 2013 at 2:11am

Yes, the attached zip file contains the files I've modified. 

Attachments:
Permalink Reply by James masterman on February 10, 2013 at 1:00am
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.

Regards

James
Permalink Reply by paul riseborough on February 10, 2013 at 1:33am

My comment about just pointing to the next waypoint is an oversimplification (there are some other processes at work depending on the distance between waypoints and the mode it's operating in), but I was never able to get decent tracking with spaced waypoints (mine are ~500m apart) until I made the changes.

I'm new to coding in C (although I have 20+ years experience in flight controls with 10 of them on UAV's) and have also never worked in an open source environment like this before, so I need to work out what the process is (how do I submit changes, what is the peer review process, etc).

I have a number of other modifications to navigation and autopilot algorithms in the pipeline that I believe will improve tracking accuracy,but my rudimentary skills in C programming are slowing me down (the devs will probably faint when they see the horrors I've unleashed on the code)

Permalink Reply by James masterman on February 10, 2013 at 4:22pm
Thanks Paul,

It would be worth getting in contact with the dev team, seems like you have some valuable skills to offer. Try contacting Andrew Tridgell.

Regards

James

3D RoboticsPermalink Reply by Craig Elder on February 11, 2013 at 3:40pm

Hi Paul

Please join the Dev team at:

https://groups.google.com/forum/#!forum/drones-discuss

and then please submit your patch to 

https://github.com/diydrones/ardupilot 

and it will be welcomed, reviewed, and incorporated into the code.

Thanks for your contribution!

Permalink Reply by paul riseborough on February 10, 2013 at 2:18am

Oh, I  also forgot that I noticed some improved code in the attitude.ino file calc_nav_roll() function  that provides a better (in my opinion) conversion from heading error to bank angle demand. This code had been effectively bypassed so I activated it and have been happy with the result,although the differences were minor.

Permalink Reply by paul riseborough on February 10, 2013 at 2:20am

The attached zip file contains the code mods specific to the tracking improvement initiative.

Attachments:
Permalink Reply by keeyen pang on February 10, 2013 at 3:46am
I'm also flying skywalker 1900 with apm 2.0 older version firmware. But I don't have much problem with tracking between way point. Sometime the crabbing angle reach about 40 to 45 degree but it follow pretty well to the grid line. I'm thinking of upgrade it to 2.68 but now I have to be sure the latest firmware won't perform worse than the old one. More discussion needed
Permalink Reply by Cliff Gill on March 1, 2013 at 8:14am

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

Permalink Reply by paul riseborough on March 1, 2013 at 1:11pm

No, not yet. I've started working with the devs, but had to work through a few unintended consequences arising from the change. I'm on a big learning curve with installing and using the various sw development tools.

Permalink Reply by Steve Keep on March 14, 2013 at 7:06am

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! 

B2 

RSS

Social Networking

Top Content 

1 APM 2.5 from hobbyking

APM 2.5 from hobbyking
2 Mapping Mission Ride Along

Mapping Mission Ride Along
3 Looking for best laptop screen to see in the sun

Looking for best laptop screen to see in the sun
4 Alt_hold and loiter acting crazy

Alt_hold and loiter acting crazy
5 ArduPlane 2.73 released

ArduPlane 2.73 released
6 New micro-cameras emulate insects' compound eyes

New micro-cameras emulate insects' compound eyes
7 APM Copter 2.9.1.b has been released

APM Copter 2.9.1.b has been released
8 3 axis Camera Gimble connections.

3 axis Camera Gimble connections.
9 Thirty Days (#17): 12-rotor close up

Thirty Days (#17): 12-rotor close up
10 Mechanical Hands From A MakerBot: The Magic Of Robohand (From Makerbot blog news)

Mechanical Hands From A MakerBot: The Magic Of Robohand (From Makerbot blog news)

Contests

Season Two of the Trust Time Trial (T3) Contest has now begun. The fourth round is an accuracy round for multicopters, which requires contestants to fly a cube. The deadline is April 14th.

A list of all T3 contests is here. 

Groups

Advertisement

© 2013   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service