Countdown to CamJam: 2 days left

I’m off to London tomorrow morning, and up to Cambridge after work, so I’m packing her up ready for her journey.

She’s probably doing the best she can given the inaccuracy of the sensors, but I’m still not quite confident she’s good enough for an indoor demo, based upon a few tuning test flights this morning.

So fingers crossed for a safe journey and a good day on Saturday, and I’ll update you on the results.

Wish us luck!

P.S. Just cheated and run her through the calibration cube again with the aim of getting the best possible set of sensor readings for the show.

24 hours later.

Remember this graph from the other day?  It’s the target pitch angle that was so swamped with noise it was useless – and therefore any attempt at drift compensation was too.

Target pitch angle

Target pitch angle

After some serious code tweaking, here’s today’s results:

Velocity vs Angle Targets

Velocity vs Angle Targets

So where did all the noise go?

If you remember, the conversion from horizontal speed PID output to absolute angle PID target goes like this:

 pa_target = -math.atan(evx_out / eaz)

Both the top and bottom sides of the division were loaded with noise. So it had to be disposed of.

Topside was easy: the horizontal velocity PID output had an element of D-gain, which contributed little but noise.  Setting the gain to zero all but removed that.

Underside required several different fixes:

  • remove the D-gain from the vertical speed PID much as above
  • reduce the dlpf to 10Hz (not sure if this actually mattered)
  • average out horizontal and vertical acceleration.

It’s the last of these that had the dominant effect.  Because I’ve recently changed to updating the PWM less frequently than the code spins, there are several readings from each accelerometer that can be averaged over the period between PWM updates.

Together, the top and bottom side changes has resulted in a pitch / roll angle targets that are perfectly aligned with horizontal velocity errors: when evx is positive (forward movement), pa_target is positive (nose up angle) to compensate.

That doesn’t mean drift compensation is working quite yet – but it at least now seems that only PID tuning stands between drift and no-drift.

I’m so pleased with this transformation I’ve updated the code on GitHub.

Cause of my two biggest problems diagnosed?

  1. lack of drift control by my drift control code
  2. mismatched scales between horizontal and vertical axes

Let’s deal with drift control first:

 pa_target = -math.atan2(evx_out, 1.0 + eaz - eaz_offset)

This essentially says the required pitch angle is the inverse tangent of the desired horizontal acceleration (the corrective output of the horizontal velocity PID) divided by the actual vertical acceleration.

The math(s) is right, but  both top and bottom of the fraction are plagued with noise; the top due to PID d-gain, and the bottom because it’s a direct feed from the accelerometer.  The two noise sources aren’t in phase with each other, so this is what you get as the target pitch angle:

Target pitch angle

Target pitch angle

Removing the noise from the horizontal velocity PID output (evx_out) is just a case of setting the D gain to zero.  Handling the vertical accelerometer noise is a diferent matter – I think only reducing the dlpf will help here.

Regarding the horizontal / vertical scale difference, I’d been tinkering to ensure angles are calculated consistently throughout the code – in particular the Euler angles, conversion matrix, and the equation above.  The aim was to eliminate yaw in all calculations by eliminating any cross-pollination between X and Y axes between Phoebe and the earth  (or putting another way, making sure that if Phoebe yaws, so does the earth, so that viewed from above, the horizontal axes align.  If I had a way to correct yaw, then I’d need to add it back into all the math(s)  but I don’t!

As a result the scaling seemed to improve significantly: all axes now are out by a factor of about 2.5 – but are self consistent.

The flight ascended to ≈2.5m and drifted by ≈4m.

Flight path

Flight path

Equal scale axes

Equal scale axes

 

The hunt for horizontal and vertical scale differences

I’ve done a quick hunt for any physical / electronic scaling differences shown in my previous post.

3 passive test runs show that each sensor reads ≈1g when their axis is pointing downwards; so the sensors aren’t operating on different scales.

Next, most likely cause is integrating sensor outputs and the effect of propeller noise.  The following graph is from the same flight that showed the scale differences.

Noise

Noise

It’s not a great surprise that the Z-axis noise is smaller than X- and Y-axes.  After all, the props spin horizontally.  Perhaps reducing the X- and Y-axis noise levels physically might help?  The sensor board is already separated from the Raspberry Pi by some noise suppression standoffs (i.e. rubber posts), but they are rather stiff.  I don’t have anything to hand to try for the moment so I’ll just have to leave it on my ‘to do’ list.

Regardless propeller noise should be symmetrical.  So integrating it to produce speed values shouldn’t lead to any scaling discrepancies between horizontal and vertical.  Certainly not to the scale they are seen.

Perhaps there’s some aliasing between sampling, filter and noise frequencies meaning the noise is being included in the horizontal speed calculations.  That could lead to a fixed offset but you’d expect it to vary per run.  But this factor of 20 appears to be the same each time.

I could filter out the noise by reducing dlpf from 20Hz to 10 or 5.  A “higher resolution” plot of the same x-axis accelerometer data for 1s shows 31Hz as the main noise component.  There’s no signs of aliasing, and there are several samples per cycle so again, this noise shouldn’t be the problem.

X-axis high-res. noise plot

X-axis high-res. noise plot

The accelerometer resolution is set to ±2g (the highest resolution), with 16bit output meaning each bit increment represents about 0.6mms-2 (0.0006ms-2) which is more than high enough.  There’s clearly no signs of aliasing in the plot.

Which leaves only my code as the cause for this scaling, though I’ve been over it with a fine-toothed comb several times to no avail.  I think for the moment I’m going to have to just let it lie, and instead get myself back to drift control.

 

Killing 10Hz

The 10Hz signal coming from the accelerometer is actually nearer 16.8Hz if you look at the stats less carelessly than I did.

That brings it into the very plausible range of the rate the props were spinning, with a sharp spike in the spectrum analyzer showing the rotation per blade at about 33.6Hz rather than per prop at 16.8Hz.  The 33.6Hz blade signal would have been filtered out by the 20Hz DLPF leaving this much smaller 16.8Hz one.

So I’ve got 3 options:

  • ignore it, assuming it’s effectively a sine wave and integrating will cancel it out.
  • drop the dlpf from 20Hz to 10Hz
  • move the sensor board – currently it’s horizontally adjacent to all the props, and hence the noisiest place possible.

Weather tomorrow is looking good, so hopefully I’d have the chance to try the software options.

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.

Noise from insufficient anti-aliasing?

CDs store their digital data at 44.1kHz, and when recorded, the analogue to digital converter (ADC) runs with a 20kHz high-order low-pass filter to ensure the 44.1kHz digitization does not introduce low frequency noise through aliasing (sampling at a frequency less than the signal being sampled).

Pretty much all of the noise that’s spoiling the accelerometer readings is generated by Phoebe’s motors and propellers.  That’s proven both by the pre-flight tilt angle calculation which works out the angle of the takeoff platform using Euler angles, and the gravity calibration, both of which work very well because no motors are spinning.  The propellers spin at roughly 5000 RPM or 83Hz.

On the MPU-6050, there are also low-pass filters akin to the CD recording.  From the data sheet, it’s not at all clear whether these are applied before the sensor ADCs (like analogue audio digitizing), nor what order the filters are.

In the code currently, the DLPF filter is set to 20Hz; sensor data registers are updated at 250Hz.  Phoebe reads the output at 50Hz.  At a simplistic level, that says I shouldn’t have any aliasing problems.  So where’s this noise coming from?

I guess that depends on the order of the MPU-6050 low pass filter.  It’s not at all clear from the MPU-6050 spec what order the filter is.  If the LPF is only first order, then 40Hz signals come through at -6dB and 80Hz signals come through at -12dB (I think).  Which means Phoebe’s sampling the data at 50Hz could cause low frequency noise.  She may well be better without the sleep, and so sampling the digital output at 170Hz, or perhaps dropping the LPF to 5 or 10Hz?

At the same time, I need to consider lengthening the complementary filter frequency from it’s current 0.25s to 0.5s or more since it currently seems to filter out much of the useful gyro input to the angle calculations.

And that’s the plan for tomorrow’s good weather testing.

Couldn’t see the woods for the trees…

As you may have read previously, all my testing was plagued with noise.  It was corrupting the Euler angles enforcing a very low DLPF of 5Hz which then means important data was lost; the most obvious symptom of which was corrupting the vertical speed – Phoebe was rising to twice her programmed height because the accelerometer data was being clipped.

I’d speculated that the noise could well be generated by Phoebe herself by feeding the PWM signals too frequently creating broad-spectrum jitters in the motors.  As a solution I was considering adding a rolling average for the PWM output.  But a comment from David in that post suggested a much easier approch – just let Phoebe sleep each loop.

By adding sleep, she updates the PWM less frequently, reducing the number of PWM pulse width changes per second, and thereby reducing the noise – nothing as complex as rolling averaging was needed.

So I tried it just now: I set the sleep delay to 14ms which combined with the processing time of 6ms leads to a 50Hz update.  First flight looked much better with the 10Hz DLPF, so I upped it again to 20Hz and it was perhaps the best flight ever: vertical takeoff to 1m as requested, hover over the takeoff point, landing back to the take-off point.  No drift because no wind.  So many problems resolved by a little sleep.

Time to update GitHub with this radical imrpovement! GitHub updated.  I’ve updated the default parameters to match what I was using so all you need to run the same test is

sudo python ./qc.py -f

I’ll produce a video next time I head off down the park – better scenery than the back garden.

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.

A bit more on drift tuning

So I ran a few more tests this morning fiddling with the horizontal speed PIDs to maintain drift tuning.  And actually, I think they worked with just the PID D gain set.  Trouble is, D-gain tends to be spiky normally as it tracks change in error rather than absolute error, so any minor wobbles tend to lead to big changes in output.  Throw into the mix the noise the accelerometer is renowned to produce and you have a recipe for disaster.  And that’s what happened.

On the plus side, she did maintain her overall position around her taking off point; on the minus side, she deviated several meters either side while doing so.

One of those deviations smashed her into the kids’ swings, ripping off her video camera – the cable got cut, but I think the RaspiCam should be OK.  In the same crash the leg / frame were distorted more than ever before – once again, repairable, but I am starting to think I may need to take her to the village park for testing!

Where next?

  • I’m wondering if I can do a running average of the accelerometer output to reduce the noise?
  • There is scope to reduce the DLPF in the MPU6050 from the 20Hz I’m using down to 5Hz – I think that’s my easiest step to try next
  • I do need to consider swapping to a horizontal acceleration rather than speed PID – perhaps now’s the time to do that, as it opens up the use of I gain for smoothing – the down side is that you’d only quickly nudge the joystick to change movement since it’s controlling acceleration rather than speed.
  • Finally, I’ll see if I can do something to add physical filtering to isolate the breadboard from the frame better – I have some silicone foam tape 1cm thick that the breadboard could sit on – this is a bit of a faffy rebuild those, so this is the last step – it turns out fixing Phoebe’s distorted frame meant she was in enough pieces I could add the new padded circuit, so we’ll see what effect that has.