# yaw control.

Currently, yaw angle PID target stays set at zero throughout the flight; the piDrone always faces the same direction regardless of the way it’s travelling.  Next step on the way to ultimately tracking between GPS points is to have her point in the direction she’s travelling; it’s not actually necessary for my girls; the Mavic Pro does it as the forward facing camera video is streamed back to it’s remote control so I get an FPV (first-person view – i.e. as though I was a pilot sitting on it).

My intention is that the yaw angle tracks the earth frame flight plan velocity vector, so the piDrone points the way it _should_ be going.  This is roughly what the Mavic does.

I though it would be trivial, and added the one new line of code, and then I realised the gotchas which led to me blogging the details.  There are 3 problems.

• Conversion of the lateral velocity vector to an angle.  tangent only covers ±90°; this means the 1/1 and -1/-1 vectors both come out as 45° angles.  Also 1/0 would throw a exception rather than 90°.  Luckily math.atan2(x, y) resolves this spitting out angles of ±180°.  Thats my yaw angle PID target resolved.
• Alignment of the integrated gyro input in the same scale as above.  If the piDrone flies 2.25 clockwise loops around the circle, the integrated gyro will read 810° when it needs to read 90° – doing yaw = yaw % 180° should sort this out.  That’s the yaw angle PID input sorted
• Finally if the yaw PID input is 179° and the yaw PID target is -179°, the PID (target – input) needs to come out at +2° not -358° i.e. the angle must always be <= |180°|.  I’ve sorted this out in the code by adding a custom subclass overriding the default error = (target – input):
```####################################################################################################
#
# PID algorithm subclass to cope with the yaw angles error calculations.
#
####################################################################################################
class YAW_PID(PID):

def Error(self, input, target):
#-------------------------------------------------------------------------------------------
# An example in degrees is the best way to explain this.  If the target is -179 degrees
# and the input is +179 degrees, the standard PID output would be -358 degrees leading to
# a very high yaw rotation rate to correct the -358 degrees error.  However, +2 degrees
# achieves the same result, with a much lower rotation rate to fix the error.
#-------------------------------------------------------------------------------------------
error = target - input
return (error if abs(error) < math.pi else (error - 2 * math.pi * error / abs(error)))
```

Now I just need the spring wind and torrential rain showers to ease up for an hour.

# Irritations and Innovations

No breakthroughs to report but:

• Zoe is now running indoors safely with or without motion fusion installed
• Without the fusion, she drifts horizontally and continues to rise during hover phase: this suggests the value for gravity at takeoff has drifted during the flight, perhaps temperature related?  It’s only about 15°C in our house currently which is outside the range she works well in.  First test is to add a blob of blue tack on the IMU so it isn’t cooled by the breeze from the props.
• With fusion, her height is much better, but she swings laterally around her takeoff point – the Garmin LiDAR lite is doing it’s job well but there’s some tuning required for the lateral motion from the Raspberry Camera.  Also it’s dark in the play room, even with the lighting on, so I’m going to add LED lighting under her motors to give the camera better site.  She’s flying over an IKEA LEKPLATS play mat, but ours seems very faded, so I’ll be getting her a new one.
• I’ve added a whole bunch of safety trip wires so that, for example, if she’s 50cm above where the flight plan says she should be, the flight dies.  Together these make her much safer for flights indoors.
• I’ve added enhanced scheduling to prioritise IMU over camera input when the IMU FIFO is reading half-full; this is to prevent FIFO overflows as camera processing sometimes takes a while, and the overflows have been happening a lot recently.
• I’ve also added another couple of pairs of PIDs – I’m not sure how I got away without them before.  The equivalent controls yaw perfectly, but the pitch and roll angles were missing, skipping straight to the rotation rates instead.
• distance (target – input) =PID=> corrective velocity target
• velocity (target – input) =PID=> corrective acceleration target
• acceleration target => angular target (maths to choose an angle for a desired acceleration)
• angle (target – input) =PID=> corrective rotation target
• rotation (target – input) =PID=> PWM output

Together all these changes require a lot of tuning, tinkering and testing; I hope to report back with a video when there’s something worth sharing.

# A-maze-ing

My distance / direction PIDs are now at nearly perfect tuning, and along the way, I’ve also learned a lot about how to tune the three tiers of PIDs in the horizontal motion hierarchy; it’s been slow, hence the lack of posts.

As far as the PIDs are concerned, the key is that the I-gain on all but the vertical velocity and yaw PIDs is now set to 1/100th of the P-gain; from the still existing wobbles it could probably do with going lower yet, though I think I may still need some to manage windier conditions.

The remaining horizontal drift is likely to be unfixable without the Scanse Sweep or PX4FLOW; here it’s just using double integration of (acceleration – gravity) which frankly, I’m amazed it performs as well as it does!

The yaw still needs some attention; currently, it too has two PIDs (angle and rotation), and I think that may be one too many and the latter can be removed.  At the same time, since the input is the integration of the gyro, then it’s never going to be much better until I get the compass working – it’s continuing to not play ball.

So for now, focus is on minor PID tuning, sorting out the yaw PIDs and getting the compass working, but before then, I think she’s ready to finally try some intentional horizontal motion; I have a flight plan put together than has her tracing a horizontal square at fixed height – 2 second segments each of take-off, forward, left, back, right and finally landing.  Forecast for today is good, so I’ll try that later, and post the results unless they are shamefully embarrassing!

Oh, and the reason behind the title of this post?  It’s now confirmed that the distance PIDs are working, so that a combination of PX4FLOW for 3 axis velocities, along with Scanse Sweep object avoidance could be used successfully to wander around a simple maze successfully  The premature but cunning plan to build a back-garden maze is already underway.

# Distance / Direction PIDs

I’ve added the distance + direction (DD) PIDs: the targets come from the integrated velocity flight plan, the inputs come from the acceleration integrated twice and fused with LEDDAR height for the Z axis.  The outputs feed the existing velocity PIDs’ targets with a max speed of 0.5m/s set for safety reasons during the initial testing.

First few test flights were a super-sky-soarer (3m off the ground in less than a second before I could react to kill!); a couple of careless bugs fixed (and a rebuilt chassis – falling from 3m leads to a harder impact than the lower PCB can cope with even with the super shock-resistant Tarot legs installed).

It’s still a β version with more tuning – I suspect removing the PID I-gain from the velocity PIDs.  However, in principle, it’s working, and so I’ve updated GitHub.  By default the DD processing is commented out – look around line number 2319.  Simply uncomment them to convert the velocity PID targets from the flight plan to the DD PID output.

# One dream sleep later…

…and I have a speculative cause for Chloe’s LEDDAR loss of modbus communication.  Spoiler: there’s a video at the end.

It always happened after a step change in the flight plan, either to-takeoff or hover; are these causing power spike(s) which cause a LEDDAR reboot?  So I looked at the stats.  Acceleration was very spiky throughout, looking like the 13″ wide, 5.5 pitch (1355 or 13/5.5) props were actually over-compensating, over-and under-shooting for each motion processing – see graph, and count the peaks and troughs – about 100 per second as expected; pretty good confirmation of the speculation.

Overpowered props

Clearly PID tuning was needed, unless…perhaps Chloe was still tuned for the expensive T-motor 12/4.0 props.  A quick swap and flight later, and the noise was down in the stats, and more importantly, the LEDDAR communications wasn’t lost.

And the consistent drift to the left?  I woke up wondering what the flight plan said, and sure enough, she was supposed to drift left.  Oops, pilot error!

# Left (a)drift

I flew Phoebe earlier with LEDDAR working well.  She repeatedly drifted left, and self-corrected as can be seen in the graph below.  You can see her repeatedly drifting and stopping.  This is the expected behaviour due to the top level PID controlling velocity not distance: drift is stopped, but not reversed.  On that basis, I’ve updated the code on GitHub.

Left (a)drift

To get her back to where she took off from, I need another set of ‘pseudo-PIDs’ to recognise and correct the distance drifted.  I’m going to keep this as simple as possible:

• I’ll continue to use my velocity flight plan – integrating this over time will provide the ‘target’ for the distance PIDs in the earth reference frame
•  I’ll integrate the velocity (rotated back to earth frame) over time to get the distance PID ‘input’ – although this is double integration of the accelerometer, it should be good enough short-term based upon the close match between the graph and the flight I watched.
• critically, I’ll be using fixed velocity output from the distance ‘pseudo PID’, rotated back to the quad-frame as the inputs to the existing velocity PIDs – the input and target of the distance ‘pseudo PID’ only provide the direction, but not speed of the correction.

This should be a relatively simple change which will have a visible effect on killing hover drift, or allowing intentional horizontal movement in the flight plan.

After that, I’ll add the compass for yaw control so Phoebe is always facing the way she’s going, but that’s for another day.

# Fried Pi (again)

I was going  to write of the successful PID tuning pretty much stopping horizontal drift with a video to prove it; it’s true that happened except for the video because Zoe died.  She just doesn’t boot.  That means I need a new Pi Zero – there more chance of a dental extraction in the back garden flock 🙁

I guess I’ll see how Phoebe’s flying now.

# White out

I’ve been unable to get Zoe working in WAP mode with the mid-March release of Jessie.  Sometimes the hostapd starts fine, sometimes it doesn’t, and there’s some conflict between configuring the static IP address in the DHCP client config (dhcpcd.conf) and the booting of the DHCP server (udhcpd) – it starts OK according to the boot logs, but by the time I’ve logged in, it’s stopped.

So for now, I’ve dropped back to the end-January image that works, and I’ve stored off a copy of the working pre-WAP mid-March image so I can pick it up another time.

Then I did some tinkering with 0g offsets, measuring them 5 times, then twisting Zoe 90° and measuring again until I had 20 readings.  Here’s what it looks like suggesting an X offset of about 40 and a Y offset of about 200 is about right.

20 0g offsets

So I then took her out to fly several 10s flight including 6s of hover.  Some were immaculate, others less so, and the main factors (guess work) are the slope of the ground, and the weight balance, particularly if it shifts in flight because (for example) I’d not got one of the batteries tied down as tightly as possible!  Even for the imperfect flights, it was possible to see her successfully stopping the drift, and then the drift started up again, to be stopped again.  I think some PID tuning is required here as this suggests the correction is using the P part of the PID and it needs some I.

# Diagnostics from indoors flights

I collected these diagnostics from a couple of indoor flights this morning.  The flight is 7.s long:

• 1.5s to get the props spinning to hover speed on the ground
• 2s climb at 30cm/s
• 2s hover
• 2s descent at -30cm/s.

This was using the complementary filter with tau set to 5s but the GERMS stats were collected to get a better idea what’s going on.

GERMS stats

This shows two things – there’s a lot of noise in the raw data blue line, and the red trend line cleary shows an oscillation which is hiding the peaks and troughs of net acceleration / deceleration at 1.5, 3.5 and 5.5 seconds.

This set show raw accelerometer values, and the measure of gravity that comes after the raw data has been passed through the butterworth filter.  Three things of interest here:

Acceleration and Gravity

1. The grey line of raw acceleration is still noisy, but does show the peaks and troughs better at 1.5, 3.5 and 5.5s
2. The green butterworth filter line is doing a lovely job of extracting gravity from the noisy accelerometer data
3. There’s oscillation in both the X and Y accelerometer readings, best seen in the blue and orange trend lines.

The flights were perfect to look at – only these diagnostics show these subtle problems.

The next steps then are to sort increase the P gain and decrease the I gain to stop the oscillations.  With those gone, it’ll hopefully allow the GERMS stats to show only the deviations from real gravity, and thereby filter it out from the angle calculation as the complementary filter does now.

One point in passing, I have to drop the hover speed PWM value from the long standing 1500ms to 1400ms PWM; The new 4S batteries are clearly showing that 3S is not enough.

# Last but not least…

The QCISRFIFO.py code is finally up and running; this is where the standard GPIO ISR callback function puts each sample into a python Queue (FIFO) and the motion processing empties the queue periodically and does the motion processing.  The code now allows for easy setting of both the sampling rate and the number of samples per motion processing.  Testing reveals some interesting results.  With the IMU sampling rate set to 1kHz, a nominally 9 second flight (in the lab so no props are spinning) actually takes 15s.  So although the IMU was sampling at 1kHz, the GPIO code was missing some of the 50us pulse data ready interrupts from the IMU and the result sampling rate was only 608Hz i.e. 40% of samples were lost.  Dropping the IMU sampling rate to 500Hz resulted in an equivalent sampling rate of  441Hz or only 11% of samples were lost.  Dropping down to 333Hz IMU sampling led to 319Hz or 4% loss of samples.

For each I had motion processing happening every 10 samples, so at approximate 60Hz, 44Hz and 32Hz.   I think 32Hz motion processing is just about enough to maintain good stability, so I gave it a go.  Wobbly to say the least, and clearly in need of some PID tuning, but I think there is a chance this may prove to be the new way forwards.  Once I’ve done a bit more tuning I’ll try to get a video done, hopefully tomorrow.