Coding: Flying 3D Spline Curves between Waypoints with ArduCopter 3.0

Figure 1: Mission Planner waypoints and telemetry track after 3 spline circuits

I wanted my ArduCopter to fly smooth continuous curves between waypoints for some nicer aerial video, so I coded up this SplineNav class. Each spline segment is a cubic polynomial curve defined in 3D space, with 1st and 2nd derivatives continuous wherever two segments meet at a waypoint. This gives a nice smooth transition between curve segments during flight (Figure 1). It also saves time and battery, since abrupt speed and direction changes are avoided.

Figure 2: Autopilot logged track after 3 spline circuits

The SplineNav class, like CIRCLE mode, calls the loiter controller, which is what allows it to precisely control position along the curve using GPS and inertial navigation (thanks to the brand new ArduCopter 3.0 firmware). Unlike CIRCLE mode, in SplineNav the autopilot takes full control of setting altitude as well, so it can fly complex 3D curves. However, it also references your WPNAV_SPEED, WPNAV_SPEED_DN, and WPNAV_SPEED_UP parameter settings to avoid flying too fast, or climbing or descending too quickly.

For this video I wanted a slightly more wild ride, so I increased my WPNAV_SPEED_DN to 250 cm/s, and WPNAV_SPEED_UP to 350 cm/s. I left WPNAV_SPEED at 500 cm/s for now, but will try increasing it later. Figure 2 shows the 3D track in Google Earth. If you reduce these UP/DN speed parameters you will get the same 3D track for these waypoints, but at the steep up and down locations SplineNav will reduce its travel speed along the 3D curve.

Video

I shot this SplineNav demonstration video with my ArduPhantom. The GoPro is mounted on a Hummer 2-axis brushless gimbal designed for DJI Phantom.

Source Code

Here's the source code for developers and brave testers to test and suggest fixes and improvements. There's a .PDE file and an .H file, both of which go in your ArduCopter 3.0 sketch folder:

SplineNav.h
SplineNav.pde

Follow the directions in the SplineNav Readme to make the ArduCopter 3.0 code call SplineNav. Then compile with the special ArduPilot version of the Arduino IDE, and upload to your copter. Set your waypoints with Mission Planner, or with the channel 7 switch, and go test out SplineNav.

Warnings

Make sure your loiter is rock solid before you test, because SplineNav relies on the loiter controller. Also, the first time you test a new set of waypoints, you should probably keep WPNAV_SPEED low, perhaps just 200 cm/s or so. That way you'll have enough time to take over control in case the spline curve intersects any solid objects!