Surprise surprise…

though not the nice kind Cilla Black used to host on prime time UK TV until the turn of the millenium.

Very wonky pitch angles

Very wonky pitch angles

Although angles have been ‘sorted’ for a long while now, I took a look at them from the last test flight to try to work out why Phoebe insisted on thinking she was drifting backwards, resulting in her real world compensation of drifting forwards.

Look at the 3 green lines – they start off well (she took off with her nose up by about 10°), but around 3 seconds, the gyro (i_pitch) and the accelerometer (e_pitch) clearly have a major disagreement.  Net result of the complementary filter (c_pitch) is that Phoebe thinks her nose is pointing upwards, and as a result, she’s going to think she’s going backwards (which is exactly what yesterday’s graph shows), and so she compensates by dropping her nose, and hence start moving forwards, which is what happened yesterday in real life.

At a guess the complementary filter is now paying way too much heed to the gyro now I’ve got everything so beautiful tuned and averaged with the accelerometer readings.

Time to tweak tau in the complementary filter down from 0.5s to something a lot smaller, and see what happens although to be honest, that’s just a workaround / hack – I’d rather understand why the gyro and accelerometer suddenly disagree so much, but for the moment, I’m very short of ideas – perhaps it’s time to retire the gyro from angle calculations completely and instead use it solely for angular stability?  Hmm now, there’s a thought.

Hurry up, Phoebe

Flights are on hold at the moment until my daughter’s chicken pox clears and she’s allowed back to nursery, so I decided to have a tinker with the code’s performance.

Phoebe’s main processing loop had degraded since the last time I let her run at full speed from about 180 loops / sec (5.55ms per loop) to about 150 (6.66ms / loop).  At the same time, the sensor read was taking 60% of that 6.66ms or about 4ms per sensor read / processing.  The sensor processing is not heavy so I had to assume i2c was holding things up.

The default i2c clock frequency / data rate on the RPi is 100kbps, but the MPU6050 can operate up to 400kbps.  To do so, I created a new file i2c_bcm2708.conf in /etc/modprobe.d/ and added the line:

options i2c_bcm2708 baudrate=400000

At the same time, I increased the MPU6050 sample rate divider from 3 (250Hz data register update rate) to 1 (500Hz data register update rate).

Prior to the change, as I said, a sensor read took 4ms.  Afterwards, the code looped at 211.55 loops per second (comared to 150 previously), and only 43% of that time was spent on sensor reading leads to a 2ms sensor read – twice as fast.

Having been poking around alot in the sensor read code, I also spotted another area that could be improved: although there’s a data ready interrupt, I don’t use it; instead I loop polling the data ready register, only reading data once it transitions from low to high.  I wondered whether I could use the interrupt to save the hard-loop polling (and hence CPU) and at the same time fractionally increase the speed?

You may remember I use the RPIO library rather than the GPIO library as it offers hardware PWM to drive the ESCs.  RPIO claims to be 100% GPIO compatible, but that turns out not to be the case anymore.  The function I needed was a blocking call to waiting until there is a rising edge on the MPU6050 interrupt pin.  The GPIO API provides GPIO.wait_for_edge() which matches my needs.

So that meant using RPIO for PWM and GPIO for GPIO.  Luckily a little bit of hackery made that easy:

import RPIO.PWM as PWM
import RPi.GPIO as RPIO

No further changes to the code were needed to make the GPIO code replace the RPIO code.  A fiddly bit of config change to modify when MPU6050 interrupts are generated and cleared, and it all just worked.  Sadly, the loop speed dropped to 90Hz from the 211 previously seen which was dissapointing.  I’ve reverted to the previous high speed polling of the register as a result.  Still it was an interesting trial with interesting results.  I’ve left the interrupt code in place if I fancy another play later to track down the cause of the reduced performace – the obvious contender is the GPIO.wait_for_edge performance compared to the high speed polling.

P.S. All these tests were done with the ESCs not powered; it remains to be seen what effect this super-speedy sampling rate renders.  If nothing else, it means I can insert a lot more code (such as an RC) and retain the performance I’d had prior to these changes.

P.P.S. Just realized I’m using the complementary filter the wrong way.  It’s used for merging angles from the integrated gyro and the accelerometer passed through Euler angle conversion.  The integrated gyro is accuarate short term but drifts long term; the accelerometer is better long term but is noisy in the sort term.

Early in the project I’d heard that the gyro drifts, which is why it can’t be used long term.  I took that as read.  Now I believe that’s not correct otherwise the complementary filter still wouldn’t work, and it’s the integrated gyro that drifts (which makes complete sense to me).  That means that this performance improvement can provide more frequent gyro data, integrating more accurately, meaning it can be trusted for longer.  Hence tau can be increased to use the integrated gyro for longer whereas I’d been reducing it to include the accelerometer earlier as the noise reduced).  That also probably means I can probably increase the dlpf to let more good stuff through.

Unbalanced Complementary filter?

UPDATE: My speculation from yesterday’s blog below is almost certainly complete bollox. There is no indication from the gyro specs that the gyro drifts other than due to temperature; it’s the integration of gyro output over time which is bound to drift. The complementary filter uses the change in angle since the last sample to update the overall filtered angle – any drift gets filtered out to 50% by the tau period. Since the angular rate is not drifting, this short term update is fine – the key here is it does not use the integrated gyro as an absolute angle – it only uses the angular rate.

As you can tell, I’m starting to get a bit deperate trying to work out why Phoebe drifts and how to control it.  I have completely OTT calibration and yet she still drifts – I really am running out of ideas short of implementing a Kalman filter which I really don’t want to do as I hate to use someone else’s Magic Box solution.


Phoebe drifts, regardless of what I do, and it’s driving me nuts.

The temperature / sensor calibration I did recently was once attempt to correct this, but it’s hard to see whether that’s had any real effect – it may well be a factor but does not appear to be the dominant factor.

Which moves me on to the complementary filter – it looks unbalanced to me. Let’s have a look in more detail:

tau_fraction = tau / (tau + delta_time)
c_angle = tau_fraction * (prev_c_angle + gyro * delta_time) + (1 - tau_fraction) * euler_angle
prev_c_angle = c_angle

As a reminder, tau is a tuned period of time where the gyro and accelerometer inputs contribute equally to the overall filtered angle. For the sake of a real example, my tau is 0.5s, and the loop time is 50Hz (0.02s) leading to a tau_fraction of 96% per processing cycle.

The complementary filter is a simple fractional mix of short term accuracy in integrated gyro plus long term accuracy of accelerator Euler angles. This wipes out the long term gyro drift by increasingly using the long term stable Euler angles. To that extent, it works.

But imagine that the quad is hovering, and has been so for long enough that the gyro drift has been overwhelmed by the accelerometer Euler angles. But the gyro still has drift, and this drifted value is what’s used in the short-term reaction to events.

Now surely it would be better to use a (gyro – drift) value to update the short term changes in angles? But how to measure drift long term? Well my speculation is that it’s just this:

drift = i_angle - c_angle

where i_angle is the previously integrated angle from the gyro over the whole of the flight. As a result, it’s dominated by drift.

So removing the tau filter for the moment, and add in the drift compensation:

c_angle = (prev_c_angle + (gyro * delta_time - drift))

or

c_angle = (prev_c_angle + (gyro * delta_time) - (prev_i_angle - prev_c_angle))

or collapsing that down

c_angle = (2 * prev_c_angle) + (gyro * delta_time) - (prev_i_angle)

finally adding back the complementary filter

c_angle = tau_fraction((2 * prev_c_angle) + (gyro * delta_time) - (prev_i_angle)) + (1 - tau_fraction) * euler_angle

It seems possible this could be more accurate by removing long-term drift from short-term changes in angle processing – thus enabling longer flights. In this case, a long flight only means a few seconds.

Something else for me to try next week once the weather improves and the replacement blades arrive (don’t ask!).

Phoebe, where on earth do you think you’re going?

The YouTube motion control video got me thinking: in their setup, their sponge ball tracking systems gives them centimeter accuracy of the speed, position and orientation of the quad(s).

Phoebe uses the accelerometer with coordinates reorientated to the earth’s axes using the various angle calculations to do the same for speed and position.  She currently doesn’t track yaw ( => orientation) – I just try to minimize it using the z-axis gyro – I’ll need a compass to track it accurately.

And despite all my best attempts, she drifts and I’m really struggling to track down why, primarily due to a lack of a plan.  I’d just assumed the drift was due to bad PID tuning, or poor angles reorientating the quad accelerometer axis to earth coordinates; my approach has just been to tweak PIDs and other config parameters in search of the holy grail combination, but so far to no avail.

So in today’s tests, I’ve converted her sensor plots into a map of where Phoebe thinks she’d been compared to where I saw her go.

 

Phoebe's Flight Map

Phoebe’s Flight Map

And actually, despite having to double integrate acceleration to distance, it’s a reasonable match of the flight.

She moves forward by roughly 0.9 meters, and moves to the right (negative is reverse / right direction) by about 0.5 meter.

However…

  1. it’s clear she’s trying to correct left / right drift for a bit before she just goes off on one
  2. it’s also clear she doesn’t make a similar effort to correct the forward drift
  3. the left / right scale is out by a factor of 5 – she eventually drifted right by about 2.5 meters

The first problem suggests that once the complementary filters starts to favour the accelerometer, then drift protection starts to fail.

The second problem suggests I may have my forward / backward drift compensation the wrong way round, so she accelerates forward in compensation for forward acceleration – though I’m not convinced here.

The cause of the third problem I can’t explain, but the difference in resolution in the accelerometer axes makes it hard for consistent calculations for drift – not good

For me, I’m delighted, as by mapping her path, I’ve found lots of useful facts that previously I’ve just been speculating on.

Further thoughts a few hours later…

The consistent forward acceleration (rather than drift compensation) is due to a bug I fixed a while back that seems to have crept back in  – my bad.

The short term left / right drift compensation seems to just need PID tuning – I think oscillations like this are usually due to an excessive P, so I need to reduce that, and consider adding some I (for “return to where you a started”) and D (for “get a shift on now!”.

The longer term left / right drift’s (lack of) compensation is most interesting: the equidistant dots suggest strongly Phoebe believes she is moving to the right at constant speed and is horizontal; that matches what I saw. Bearing in mind this is sensor data, it’s used as PID feedback, so the Y axis horizontal speed PID should do something about this, yet it doesn’t appear to.  For the moment, I have no idea how this can come about so it’s frustrating – interesting too, but primarily frustrating.

I have no idea where to start about the factor of five scaling difference between the axes; I’ll just have to keep my eye on it during Monday’s testing.  Currently, the only solution is a dirty hack to multiply the left-right earth coordinate acceleration by 5 before integrating.

Finally, I’m still delighted that producing this map works, and provides a new kind of insight into flight problems.

P.S. Anyone else think this looks like an eagle soaring?

The plot thickens

on the Murder Suicide.

Ran several tests in the park – first one without any drift protection just to prove all was well; it was so then I enabled the drift protection, making sure I wasn’t in her line of flight this time.

As before, she took off and headed into the 20mph tailwind, her angle of tilt and hence her acceleration ever increasing.  This time I got detailed flight stats.  First, here’s the pitch angle she was tilting at, from integrated gyro (i_pitch), Euler accelerometer (e_pitch) and the combination of the two (c_pitch) generated by the complementary filter.

Murder Suicide Angles

Murder Suicide Angles

Ignoring the noisy Euler angles (I’ll come back to that in a bit, as there’s something hiding in there), this is a good representation of what happens: a positive pitch angle indicates Phoebe is nose up, which is what she was in order to accelerate into the wind.

Now here’s the horizontal acceleration (eax) and speed (exv) in the earth’s axis:

Murder Suicide Moverment

Murder Suicide Movement

Again, ignoring the noisy acceleration for now; you can clearly see increasing speed throughout the flight; oddly though, a positive increasing speed indicates Phoebe thinks she’s accelerating forward; that explains why she’s tilting backwards to stop that, but there’s clearly something wrong in the direction of the forward / reverse X-axis accelerometer output as she clearly was not moving forwards!  This mismatch leads to positive rather than negative feedback and hence her ever increasing tilt – the faster she thought she was going forward, the greater the reverse pitch, causing further ‘forward’ acceleration.

Also interesting, this only shows up on the X-axis.  Y is fine.  So there’s something clearly wrong with the accelerometer sensor labelling of forwards and backwards, or how it’s handled by the code.  Finally I have a clue where to start looking.

Finally, the noise: actually it’s not very convincing noise – it’s more like a 10Hz signal – the sensors are read every 10ms, so there’s 10 samples per signal confirming a 10Hz signal rather than filtered arbitrary noise.  The dlpf was set to 20Hz so it’s right that it got through.  But where has it come from?  The blades were spinning at ~80Hz, so I really can see where this 10Hz signal is coming from.  Thankfully, it’s not critical at the complementary filter has done it’s job well, and I have a clue about where to look for the noise.

Is my code too fast?

Speculation, but I’m still on the search for noise…

The main loop in my code takes on average 6ms or 170 loops per second (Hz), during which the code

  • checks the commands for what Phoebe should do
  • reads the sensors
  • calculates critical angles
  • runs the PIDs
  • updates the PWMs

and I wonder whether this updating of PWMs at 6ms intervals is causing broad-spectrum noise from the blades depending on what the updates are?  Is it this noise forcing me to use the very low (5Hz) DLPF combined with the long complementary filter (0.5s)?

Have a look at the audio spectrum analyser results I got a while back.  Although there are clear peaks showing the blade RPM , these all should be filtered out by the DLPF.  But there is also a lot of other noise which wasn’t present before the blades started spinning.

If that noise is caused by the constant PWM changes, then I could apply another filter equivalent to the complementary filter, but applied to the change in the PWM signals for each blade, set to average with a tau of say 22ms (45Hz).  Certainly ESCs can accept PWM updates at this much lower frequency, and I suspect that although many advertise they can take updates up to 400Hz, perhaps they do filtering on their outputs to limit the noise rapid PWM changes would produce.  I can’t see inside an ESC, but it seems plausible.

Backing this up is my belief nothing in a quadcopter’s flight needs to react in 6ms – if nothing else, because the quad has weight and therefore linear and angular momentum, it simply can’t react in line with such rapid and significant changes.  Add to that the fact that unless the quad hits something, there’s nothing the quad needs to react to in that time frame, and it starts to sound plausible that actually, I need Phoebe to chill-out a bit by adding that filter.

For the moment, this remains a “Hmm, I wonder”, but if I can get decent behaviour from a DLPF of 5Hz, I may well consider upping the DLPF to 10, 20Hz or more, but soften the output via my own filter; I’d rather be in full-control of what’s going on than leave it to some inpenetrable magic box.

You may wonder “Why bother?”. First, as I said, I’d rather be in control not the DLPF; second, the DLPF is applied to gyro and acclerometer, and causes delay – this is unnecessary and inaccurate for the gyro; and thirdly, if I’m right, use of the DLPF is a sticky plaster to hide a flaw; instead the code should be tailored to fit the quad reactions better.

But first I need to prove that 5Hz DLPF solves the problem.

Yet more park testing

Went to the park again today to test my new angles calculation; complete garbage – with Phoebe whizzing 25m forward before I killed the flight.  The change to the angles had left me worried about calibration, specifically for the accelerometer, and to be honest I still do, but luckily I can leave that for another day: after checking the stats, the cause is quite clearly the complementary filter.

Pitch angles

Pitch angle

As I mentioned, the flight had Phoebe whizzing forwards across the park – i.e. nose down, and that’s quite clear from the integrated gyro pitch angle (i-pitch); it’s not surprising that it’s not visible in e-pitch, the Euler angles, but the fact the complementary filter (c-pitch) had effectively ignored the i-pitch clearly shows the problem.  The tau was set to 0.25s but quite clearly from the graph the i-pitch contains critical, accurate information for at least a few seconds without obvious signs of drift.  I think I need to start playing with tau at nearer 2.5s!

Just for comparison, I’ve dug out some stats from when I first added the complementary filter; in this test, the blades were not running, I was just manually tilting Phoebe – look how the i-pitch and e-pitch track each other, while the filter output (c-pitch) cancels out the gyro integral drift (i-pitch) – all shown in shades of blue:

Sensor Angles

Sensor Angles

Hopefully with tau sorted, I can go back to checking my new angles and putting my mind at rest about whether gravity calibration is correct / useful – more on that once I’ve made a decision.

Just an afterthought: it’s clear from the tests that the accelerometer Euler angles are varying wildly, so perhaps it’s better, at the same time as (say) doubling up tau to 0.5, I also increase the dlpf to 5Hz.  Only testing will tell.

P.S. The big downward spike at the end was Phoebe doing a forward roly-poly at the end of the flight as her front legs clipped the ground while she was still travelling at several mph.  Funny to watch and zero damage.

Park testing

It’s a lovely day here at the edge of the Cotswold; bright sunshine and absolutely no wind.  So I went to the park.

First flight was perfect – wow I thought, wish I’d brought my camera.

Second flight drifted by 10m and I was glad I hadn’t bothered.

After numerous flights trying to work out what on earth was causing the drift, lots of watching take-off for common features revealed the truth:  take-off from non-horizontal ground means there is some lateral power applied by the blades, leading to the drift.  As the take-off tilt is corrected, the drift stops accelerating, but still continues at constant speed due to momentum.  Hence the overall drift across the flight.

By having pre-flight Euler angle checks, Phoebe knows immediately she’d not on a flat surface, so the problem is all due to how fast she can compensate and get herself level using the mix of the DLPF and complementary filters along with the absolute angle PID.  Now I know I’ve mentioned these loads of times; the difference here is that I now have a greater understanding of how they are interacting.  And with that in mind, I could watch a take-off carefully, looking at the take-off angle, watch how long into the flight it took for Phoebe to correct that angle, and therefore how much drift speed she gained until she levelled out.

And as a result, I have a much better set of default configuration figures for DLPF, complementary filters and absolute angle PID gains – not perfect perhaps, but possibly good enough that if there is wind on another day, I can rely on these default to then start looking at the horizontal drift kill testing using the horizontal velocity PID.  And that is a big step forwards because once that’s sorted, it’s on to phase 2 and a remote control

 

Park Life

I took Phoebe for a play in the village park just now – it was a huge amount of fun for us both!  When the local kids are at school, it’s empty, and there’s a football pitch sized area of just grass.  It’s also just a few meters higher than our garden (we’re at the lowest point in the village), so while our lawn is currently somewhere between a quagmire and a lake, the park just had soft grass to land on, which, it turned out, is a very good thing!

The only downside is our garden has 6 – 10′ stone walls around it shielding it from the worst of any wind; the park is next to an open field, and today a 20mph wind was blowing.  So I sat, back to the wind and let her rip.  Off she flew at probably 20mph and reached the other side of the pitch in no time at all.  Her wind-drift protection wasn’t engaged so that’s actually what I’d expected (which is why I sat where I did), but I was surprised at how effective the wind was!

She did takeoff at a tilt and self corrected as the complementary filter started kicking in.  With dlpf set to 5Hz, I think I should be able to pull in the accelerometer much quicker.  I got it down to 10ms but I suspect I could go lower still.

And best of all, it was fun – there was nobody and nothing other than me that Phoebe could harm, meaning I could just watch the details of the flight and the change of behaviour as I tweak the config – very satisfying!

I’ll definitely be back there next time it’s not raining – Monday’s forecast is looking promising, and the wind is only expected to be just 14mph!

P.S. After reviewing the stats from the final flight, something useful has come from the 20mph flights too:  I’d been tuning the DLPF / complementary filter the wrong way round.  Under the belief that a low DLFP cuts out noise, I’d been reducing the fraction of time that accelerometer Euler angles became dominant.  But as a result, the stats showed I was tuning out the integrated gyro data.  This meant long term stability was fine, but Phoebe lacked the ability to react to the initial tilt she took off with.  So Tuesday’s priority is to see the net effect of the changes I’ve made – to allow more accelerometer data through (DLPF @ 10Hz rather than 5Hz), but then take longer to allow it through the complementary filter (0.2s rather than 0.01s).  Looking forward to seeing the result.

A record!

Today, I’ve been able to do more testing in one day than I’d managed throughout the whole of January – perhaps as many as 30 flights of 10s!  How did this come about?  Partly due to perfect weather, but mostly due to a lack of any radically unpredictable behaviour and hence no unexpected crashes.

And actually, the lack of any manic unpredictable behaviour has made the multiple testing necessary – I’ve run out of blatantly obvious problems, leaving me with only subtle ones – mostly based upon depedencies between the dlpf, tau, accelerometer noise and the PIDs that use them.  Because of the many interactions, only a series of flights can actually show the flight style variations caused by a minor configuration change.

Which means I’m going to have to be more structured in my testing, and the fact the flights are getting better means I can.  Each test needs to be run several times sequentially to check consistency.  Once any common factor between all flights are identified, then a tweak to flight parameters can be made and the tests rerun.

So the plans for Thursday’s testing are:

  • see if I can turn off the inner stability PID Ki gain – I suspect it’s making the response to the absolute angle PID sluggish and that in turn is leading to drift.  I dropped this by 50% with no problem so I’m pretty sure I’ll be safe to take it down slowly to zero carefully
  • If that works, then it’s back to fiddling with dlpf and complementary filters to minimise noise while ensuring meaningful changes don’t get filtered out

P.S. Despite no unexpected crashes, there were breakages which ended my testing.  My new blades are fantastically strong, but one took on the kids swing (because I didn’t stop it), and as with the last time that happened, the swing won.  Oh, and I lost another leg – the flexibility designed into these legs just doesn’t work as well in the cold.  The plastic they are made from becomes a lot more rigid and so snaps on impact – although that’s pretty rare at the moment – normally they sink a couple of inches deep into the quagmire that’s the lawn!