Yaw(n)

I think I’ve finished writing the code to support GPS tracking: a target GPS location is stored prior to the flight; the flight takes off pointing in any direction, and once it has learned it’s own GPS takeoff position, it heads towards the target, updating the direction to the target each time it gets a new GPS fix for where it is, and finally landing when it’s current position matches the target GPS position.

There’s a lot of testing to be done before I can try it live.  The new autopilot code is pretty much a rewrite from scratch.  It now has multiple partial flight plans:

  • take-off
  • landing
  • file defined direction + speed + time
  • GPS initial location finding
  • GPS tracking to destination
  • Scanse abort.

It switches between these depending on either external Scanse or GPS inputs, or the flight plan section completing e.g. the GPS flight plan reaches its destination or the file-based flight plan has completed the various time-based lateral movements and hovers (both of which will then swap to the landing flight plan).  Luckily, most of this testing can be carried out passively.

The first test preempts all of these; as per yesterday’s post, I should be able to use my magnetometer combined with its magnetic declination to find Hermione’s initial orientation compared with true (i.e. GPS) north.  At the same time, I could use the magnetometer to provide long term yaw values to fused with the integrated yaw rate from the gyrometer.  Here’s what I got from two sequential passive tests:

Compass & integrated Gyro yaw

Compass & integrated Gyro yaw

I’m not hugely impressed with the results of either.

  • The difference between compass and integrated gyro yaw doesn’t match as tightly as I was expecting – in its current state, I won’t be using the compass direction as a long term fuse with the integrated gyro yaw unless I can improve this.
  • The blobs in the lower pair are compass orientation values as she’s sitting on the ground – half way through I rotate her clockwise roughly 90°.  The rotation angle is pretty good as are the direction (NW -> NE -> SE, but I don’t like the distributed density of the blobs as she sat still on the ground at each location – I think I’ll have to use an average value for the starting orientation value passed to the autopilot for GPS tracking processing.

GPS + compass + yaw control code

Coding this combination of features has taken me several days, covering complex interaction depending on whether each is installed and how they are configured.  This has required a rework of the GPS processing and Flight Plan objects to ensure they interact cleanly, along with enhanced configuration parameters and use of compass.  Here’s the overview.

yaw control compass gps control
yaw control Defines always forward flight with yaw to turn. Defines flight plan X, Y as N, S, E & W; fused to prevent integrated IMU yaw rate drift. No interactions without compass
compass Defines flight plan X, Y as N, S, E & W; fused to prevent integrated IMU yaw rate drift. Defines NSEW during flight Defines direction of flight between waypoints.
gps control No interaction without compass Defines direction of flight between waypoints. Defines flight plan waypoints; tracks locations during flights.

I _think_ I’ve got all bases covered now in the code, but there’s a lot of new pieces to be tested individually, and in all possible combinations, passively first, and then in real flights. For the first time in this project, I’m going to need to plan my testing so I don’t miss a combination until it’s too late.

On the plus side, somehow after her last video, Hermione’s shoulder joint broke while I was testing higher video frame sizes and rates, and the camera stopped working.  Until the replacement parts arrive, there’s a lot of spare time for thinking and coding, and also occasionally tickling my fancy – more on the latter anon.

GPS + compass plan

Here’s what I’ll be attempting to achieve with GPS and compass data integrated into the code – its data’s available now, but just logged.

  • In a large, open field, I’ll take Hermione to a random point, and she’ll record the GPS location; this will be the destination for the flight.
  • I’ll then carry her to any other position in that field and pop her down on the ground pointing in any direction i.e. not necessarily towards the destination.
  • As part of the current take-off procedure, she already records her GPS take-off location; from this and the destination GPS location, she’ll build up a flight plan in much the same format as she reads from file now: from the calculated distance and direction between the GPS points, she’ll convert these to a fixed velocity vector and flight period, wrapped in the stand take-off, hover and landing flight plan sequence.
  • During the initial hover of the flight. she’ll rotate so she is pointing towards the target destination based upon the compass input.
  • Once actually heading towards this destination, compass + yaw fuse to ensure she’d always pointing in the direction she’s flying
  • At the same time, the fused combination of the double integrated acceleration, down facing video combined and GPS track the current location, comparing it to the predetermined destination position and thus tightly tracking her way to the destination.

All the above isn’t difficult unless I’m missing something hidden in plain site; a large part is in preflight checks, getting the destination and takeoff GPS points and compass orientation.  The changes to the flight based on this target are pretty much in place already.  Just minor changes are required to fuse the compass into the integrated yaw rate both for post-takeoff hover to point her towards the destination, and to maintain that during the flight to that destination.

This breaks down into several easy safe stages:

  • the automated flight plan file creation can be done in the back garden with the motors not even powered up.
  • Initially the flight plan will only cover take-off, hover with reorientation and landing, just to check she’s pointing the right way
  • then the full flight plan maintains further incremental yaw as zero while she flies forwards towards the target for the time at the speed defined in the flight plan.

I’d better stop blogging and get on with it.


After a little more thought, I’m going to break it down further.

  1. I’ll still take Hermione around the park collecting a destination GPS location along with possible intermediate way points.
  2. Once at the arbitrary starting point, her initial GPS location is also recorded as part of her take-off routine.
  3. Together these GPS locations are passed to a sub-class of the FlightPlan() object to produce a flight plan structure identical in looks to the file based flight plan used now.  The only difference is the coordinate system is GPS North, South, East and West in meters rather than piDrone take-off orientation of forward, backward, left and right.
  4. The compass will be used to determine the initial orientation WRT NSEW (magnetic) and set the base level yaw angle thus aligning it with the earth frame GPS flight plan (note to self – consider adding gain as well as the current offsets to the compass calibration; also what’s the angular error between compass and GPS north here?)
  5. The flight then runs using only the existing yaw control code – compass and GPS information are just logged as now.

This intermediate steps have clarified a few uncertainties and I definitely do now need to get on with it.

First lateral flight plan.

For the first time in 4 years, I tried a lateral flight plan, fairly confident in the belief that stable hover in a headwind is no different to intentional movement forward in no wind.  The flight plan was:

  • Climb at 30cm/s for 2s
  • Hover for 1s
  • Move forwards at 30cm/s for 2s
  • Hover for 1s
  • Move backwards at 30s/s for 2s
  • Hover for 1s
  • Descend at 30cm/s for 2s

Was I right?  You tell me: