New SplineNav Version 0.2 on a 63 km/h Tour of Chinese Lotus Pond

I've now finished coding up SplineNav version 0.2. It's almost a complete rewrite from version 0.1. Besides flying smoother and using processor resources more efficiently, it now restricts maximum speed to prevent lag. For example, if you're flying fast into a strong headwind, then altitude may get low due to insufficient downward thrust, and position may also start lagging target, resulting in corner cutting. SplineNav now tracks this, and continuously and smoothly adjusts target speed to allow the copter to keep up without major altitude loss. So it's now completely safe to crank up the speed settings and fly SplineNav really fast.


SplineNav Waypoints

I collected the waypoints for this video while flying FPV, and flipping the channel 8 switch at each point I wanted to record. Then I loaded the waypoints into Mission Planner, and made some small adjustments (Figure 1). I also checked them by flying SplineNav at low speed first, and watching via FPV how close it got to the trees.

Figure 1: Waypoints recorded during FPV flight and adjusted in Mission Planner

After the low speed check I felt comfortable to fly it at max speed, although it was still very nerve racking, because it was zipping by a few meters from those weeping willow trees at about 60 km/h. I'm not sure I could have reacted in time to prevent my copter going to the bottom of the pond had a GPS error caused it to brush the willow branches and careen out of control!


Flight Log Track

After the flight, I loaded the log data into Google Earth and exported a KML file to overlay on the map, using Mission Planner's handy KML Overlay feature (Figure 2).

Figure 2: Purple track is GPS recorded track during SplineNav flight

As you can see, the GPS track indicates it hit all the waypoints very precisely, expect it slightly missed waypoint 8, which I attribute to the copter having just flown right next to a large building which could have caused GPS signal reflections.

The waypoints had a range of altitudes set, for a more interesting flight. Here is the flight profile from the data logs, imported into Google Earth:

Altitude profile generated in Google Earth from flight log data

Hardware Used

Airframe: ArduPhantom (DJI Phantom case, stock motors, ESC, and battery)
Autopilot: 3DR APM 2.5
Gimbal: Hummer 2-axis brushless gimbal for DJI Phantom and GoPro 3
Camera: GoPro Hero 3 Silver
GPS: 3DR ublox LEA-6H
Telemetry: 3DR 433 MHz
R/C: FlySky TH9X(ER9X FW) + 2.4GHz FrSky DJT module + V8R7-II rx
FPV: ImmersionRC 5.8GHz 600mA + FatShark Predator goggles



This test was done using the excellent new ArduCopter 3.0.1 release code, that I then modified to include and call the SplineNav code.

The latest SplineNav code, already integrated into my own branch of ArduCopter 3.0.1, is available here:


SplineNav 0.2 Firmware Installation

Warning: Only install SplineNav if your copter is already working well with ArduCopter Version 3.0.1, and if you're experienced enough to test fly it safely.

1. Download the code with this link: and extract the zip file.

2. In the special Ardupilot version of Arduino, go to File -> Preferences and set your sketch directory to the path of the "SplineNav-SplineNav-0.2" directory from the extracted zip archive.

3. Restart Arduino, and choose File -> Sketchbook -> ArduCopter from the menu.

4. From the ArduPilot menu, make sure your HAL Board is set correctly.

5. Connect your copter's APM via USB, and from the Tools menu make sure the serial port is set correctly.

6. Click the Upload arrow button and wait for the code to compile and upload to your APM.

7. Set your waypoints (either with Mission planner or with the channel 7 or 8 switch), then go fly!

Note: Since there is not yet any SPLINENAV mode in Mission Planner, SplineNav for now just commandeers CIRCLE mode. So switch to CIRCLE mode on your transmitter when you're ready to fly your waypoints with SplineNav.



Here are the speed and acceleration parameters I used for this video (set in Mission Planner):

WPNAV_SPEED: 2000 cm/s
My copter can't fly 2000 cm/s, but SplineNav correctly kept the speed adjusted to what my copter can actually handle, and according to the GPS data it reached a maximum velocity of 1760 cm/s (63 km/hour).

WPNAV_SPEED_UP: 350 cm/s
WPNAV_SPEED_DN: 450 cm/s
WPNAV_ACCEL: 500 cm/s/s

Also, the following parameters are #defines in the splinenav.h source code, but hopefully they will eventually become configurable parameters:

Higher tension splines curve more tightly at waypoints, but straighter in between waypoints. A tension value of 2 makes it a Catmull-Rom spline. I found that slightly lower tensions tend to give nice loose curves for smooth aerial video.

SPLINE_JERK: 500.0 cm/s/s/s
Jerk is the maximum rate that SplineNav increases or decreases acceleration as it flies the curve.

This makes SplineNav loop the waypoints forever until you exit out into another mode.

Views: 2628

3D Robotics
Comment by Chris Anderson on July 19, 2013 at 4:43am

Super impressive! Thanks so much for sharing that. 

Comment by David Dewey on July 19, 2013 at 4:56am

Thanks Chris. And thanks to 3DR and all the developers who's hard work makes it possible to do cool stuff like this with ArduCopter.

Comment by RickP on July 19, 2013 at 5:14am

Fantastic work - would love to see this integrated into 3.1! If I get a chance, I'll give it a whirl on my quad and report back...

Comment by David Dewey on July 19, 2013 at 5:23am

Thanks, definitely looking forward to getting some test reports from users!

Comment by Rob_Lefebvre on July 19, 2013 at 6:13am

Very nice David.  Your test here shows the amazing possibility this present for videography.  You can pre-fly a route FPV, saving waypoints as you go, just as you did.  Then have the copter fly the mission over and over, very repeatedly, while actors or whatnot on the ground do their thing.  

Now, the video, did you speed it up?  Something about it, maybe the notchy yaw movements, seemed like it wasn't running at regular speed?

Comment by David Dewey on July 19, 2013 at 7:25am

No, there is no speed up done to the video, it's 1x speed. The notchy yaw movements are a big problem with SplineNav, and will definitely need to be resolved in order to get good footage. I think there are several causes of this problem:

1. The way that the SplineNav code controls yaw is probably not ideal. Hopefully some experienced developers can give me advice on improving it.

2. In this particular flight, the speed is near the limits of what this little copter can obtain. I think being near max throttle like this decreases yaw stability, because some other flights where I had WPNAV_SPEED set a little lower didn't have nearly as much yaw jitter.

3. This was a breezy day, which created a fair bit of turbulence near treetops and close to buildings. It wouldn't affect a larger quad very much, but this ArduPhantom is pretty small and light. Perhaps changing my PID tuning from the default values might help a bit though?

4. The 2-axis gimbal can only stabilize camera pitch and roll, not yaw.

Any suggestions to help improve this yaw issue would be greatly appreciated!

Comment by Rob_Lefebvre on July 19, 2013 at 7:40am

How fast are you calculating the yaw?  Is it being run at 10Hz or something?  And what are the inputs/outputs of the system?  Are you using GPS ground heading raw out of the GPS to set the yaw?  Because I think that is only updated at 5Hz or something.  Maybe that's what we're seeing.

I'm very interested in having smooth yaw, because with a heli, you really want the tail to follow behind nicely, or it can get really juddery at high speed.  I worked with Randy to create the yaw_look_ahead mode for yaw control, but I'm not totally happy with it yet. It also uses the GPS ground heading as an input, and I think it's just not going to be good enough.  I'd prefer to use the new INAV Lat/Lon numbers, and run it through trig at 10Hz to calculate heading.  20Hz would be even better.

So that's a start, is a good input to the yaw controller.  Without that, there's no hope.  Once you have that, might need to look at some damping and/or slew rate control.  Currently yaw_look_ahead uses a pretty crude slew rate controller, which I'd like to make better.  We have a deadband of 3m/s, to avoid it going crazy at low speeds while the GPS ground track is jumping around.  But once it gets outside of the DZ, it uses a single slew rate.  So it looks somewhat un-natural.  I want to make the slew rate variable by ground speed. Basically, the faster it's flying, and the higher the difference between current yaw head and the travel heading, the faster it will slew around.  I want it to feel like natural wind drag on the tail.

I spoke with Randy about this a bit last night, and I'd like to make another yaw mode, #4, which will use yaw_look_ahead during auto missions. And then from there, work to perfect it.  I'm not sure if this can tie in with your work or not.  It sounds like you are managing yaw from within your spline nav code?

Comment by Gary Mortimer on July 19, 2013 at 7:49am

Blimey top banana

Comment by David Dewey on July 19, 2013 at 8:04am

The way that my SplineNav currently does the yaw is probably too crude, it's something that I just haven't got around to fixing yet, but it's next on my list. It currently just sets the variable yaw_look_at_WP to a point 500 meters ahead in the direction of the spline derivative (but with the z-axis of said derivative zeroed out). I haven't even checked to see how often ArduCopter updates the yaw to face the yaw_look_at_WP, but it's probably not often enough.

So I will definitely look into this, and again any suggestions on how to best code it would be appreciated. 

Comment by Gerard Toonstra on July 19, 2013 at 8:07am

Wouldn't the gyro work very well here?

As you stated, the readout frequency of the GPS direction, which only after a derivative becomes the yaw rate is very noisy, which you can only get rid of by advanced filtering, which require time to pass, which is not available.

The magnetometer comes to mind as a replacement of GPS direction, but that depends on the quality of calibration and the amount of metal on board and other interferences. I'd attempt to fuse the magnetometer and GPS direction and/or define a threshold where one replaces the other.

These gyro readings need to be considered in 3D space with projections on the 2D plane (pitch/roll) to make it work correctly.

In my strategy, the objective is to achieve a consistent yaw rate (smooth curve) and there are formulas for determining what the yaw rate should be given a certain forward ground velocity to achieve a certain curve with a particular radius. The yaw rate allows you to fly consistently for a couple of seconds, after which you can use the GPS (and filtering) to recalculate updates for this yaw rate (curve replanning), which allows you to deal with unknown externalities (wind,disturbances, etc).

The disadvantage is that you still deal with the noisy direction vector from GPS, so I'd use external sensors like the magnetometer to dampen (and quicken up) those. The magnetometer has a higher refresh rate. You can also use the yaw rate together with the last determined direction to calculate a "dead reckoning" of what your current yaw really is.


You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service