Oh what now?

I managed to squeeze in another flight today, and got diagnostics. The flight was only 3 seconds before I had to kill it – she then flipped over and landed upside down on the roof before sliding down (phew) and landing on the grass.  No damage done to the house or Phoebe.

During the first second or so, the flight plan was zero movement in all three axes while the props got up to speed. For the next two seconds, Phoebe rose, but she rose to over 3m in those two seconds instead of the target of one meter.  She also drifted gently to the right and possibly backwards (but that’s harder to see from behind).

This graph shows accelerometer readings over the course of the flight, with gravity (a fixed reading taken during the flight warm-up period and rotated to the same orientation) subtracted:

Acceleration Drift

Acceleration Drift

I’ve added linear trend lines to the graph which, at least over these 3 seconds, do fit with a linear drift in the accelerometer readings.  Note that the drift in readings seems independent of whether she’s stationary on the ground for the first second, or climbing for the remaining flight time.

So what could be the cause?  For once, it isn’t my code – at this point no motion processing has happened – this is raw data read from the sensors.  It also can’t be calibration as the trend lines would be horizontal with a fixed error.

My first suspect for the drift was temperature, but this graph of sensor temperature over the course of the flight shows that’s unlikely:

Flight temperature

Flight temperature

Could this be dlpf filtering out important stuff or including unnecessary stuff?  It was set to 3 – 44Hz and 4.9ms lag.

I have 2 plans of attack here:

  1. try upping the dlpf to 2 – 94Hz and 3ms lag and down to 4 – 21Hz and 8.5ms lag and see what happens.
  2. try physical filtering by isolating the sensors from the prop noise.

Option 2 is Zoë – the genetically engineered clone of Phoebe’s software and electronics with Chloë’s hardware, with a special dash of silicone dampers to isolate the Raspberry Pi and Beret lower platform from the noise from the props / motors transmitted along the arms to the top platform.  The remaining parts are arriving tomorrow.

I do hope this works because otherwise this project has just hit the biggest metaphoric brick-wall possible – think of the great wall of China, but built of granite.

You put your right prop on..

your right prop off.
on, off, on, off
and smash into a wall.
You do the hokey cokey and you turn around.
That’s what it’s all about!

Why do all my crashes break a right hand, ACW / CCW rotating prop?  I have so many spare lefts it’s getting embarrassing!

Though it wouldn’t help that sometime (I think yesterday) I fitted 10″ instead of my normal 11″ props to Phoebe’s left hand side.  Oopsy – it least that explains the increasing negative (ACW) rotation, eventually leading to the left wall collision.

On the plus side, with the correct props fitted on the left hand side, I got stability matching the take-off angle with the attitude only PIDs (i.e. completely as expected), and zero-drift once the motion PIDs were included too (again, completely as expected).  In other words, absolutely perfect…

…except for the fact she kept climbing; nevertheless still worth videoing.  Two reasons why I didn’t video the flights – they’re called Jacob and Milly; I’m the parent in charge until mid-afternoon so my time for test flights today is very limited!

So back to the Z-axis velocity PIDs, it seems.  I wonder whether the dlpf of 3 (44Hz, 4.9ms lag) could be the cause – either too high or too low – I can speculate on the cause of either way, so I’ll try both dlpf 4 (20Hz, 8.3ms lag) and dlpf 2 (90Hz and 3.0ms lag).

Laters…

Confused

Something very weird is going on here: any kind of horizontal acceleration appears to be not detected.  I’ve tried various tweaks in areas that might be the cause:

  • I’ve increased the dlpf from 5Hz to 20Hz in case good data was getting filtered out
  • I’ve reduced the sampling frequency from 1kHz to 500Hz in case there was corruption in the data due to updates happening faster than could be read by the code.
  • I’ve bypassed my calibration code so only raw values from the sensors are used.

And yet the results are identical each time:  somehow, the positive accelerometer spikes which should appear when Phoebe starts to move forward / sideways, and the corresponding ones when that movement stops simply aren’t happening.  Only the Z-axis is showing anything close to credible.

And now, after this extended config change testing, I’m at a complete loss as to why.  Time for some very out of box thinking – whatever that’s supposed to mean!

Breeding filters

Currently accelerometer readings are taken 450 times a second and are ‘integrated’ over time, and used every 37 times a second by dividing by the total time spent taking the last batch of integrated samples was taken.  That gives a very accurate average of acceleration over that block.  But it is lumpy and I started wondering if it could be improved.

I’d considered a rolling average in the past, but that doesn’t account for the fact data isn’t read at equal periods – the sensor provides updated accelerometer readings at 1kHz, and the code picks up the next one and therefore sometimes skips values.  It’s important to take into account the time between each pair of samples; that’s what the integration does, and that’s what the rolling average can’t, unless…

A rolling average looks a bit like this: the N-th values looks like the previous value plus the change since last time.  In this case, α is a fixed constant < 1.

an = an-1 + α * (anow - an-1)

or

an = (1 - α) * an-1 + α * anow

And this looks awfully similar to both the Kalman and Complementary filters I mentioned the other day.

So what would happen if I used a complementary filter style τ gain which is time sensitive rather than the rolling average α gain?

Dunno without testing it, so I did.  And it wasn’t bad – she kind swooped across her take-off point several times, but she was losing height throughout – that problems been there for a while due to discrepancies between calibrated gravity and the measure of gravity in flight – I’m still trying to work out how to deal with that.

So a good start, put possibly needing some PID tuning, and also, if possible some reduction of dlpf from the current 5Hz to 20 or 40Hz as hopefully the new filter is doing its job anyway.

 

 

What next prior to CamJam?

2 problems to solve prior to the CamJam

  1. occasional spikes in accelerometer readings get integrated into horizontal and vertical velocity, and are the most likely cause of sudden mid-flight drift.
  2. I’d like to get the dlpf higher if I can to make sure I’m not filtering out good stuff.

I think the accelerometer spikes are due to the MPU6050 data registers being updated while they’re still being read.  This may be because she’s hard looping in Python trying to catch the next sensor update as it happens.  Instead, I’m looking to is the GPIO.wait_for_event() call to catch the data ready hardware interrupt.  I tried this previously but it actually slowed things down.  But I’m now rewriting that area of the GPIO Python library to try to speed things up.  We shall see.

To get the dlpf higher, I need to get the primary noise source, the props and motors, rotating at a higher rate.  She was doing so in the past on 10″ props, but the power surges required by these smaller props were causing power brown outs, causing Phoebe to reboot mid-flight.  So she move to 11″ props, the brown outs stopped, but the noise dropped down to more like 40Hz rather than the previous 60Hz.

But now her control and power power supplies are completely isolated, there can’t be brown outs, so it’s worth retrying the 10″ at higher dlpf (20 or 40Hz) to see if this brings in more useful data that had been previously lost.

Time is of the essence.

I’ve seen a few on-line conversations adamantly stating that quadcopter control needs a real-time OS (RTOS) or microcontroller to ensure accurate timing but with zero solid evidence to back it up.  I’ve always been adamant this is just viral hearsay, and I’m being the heretic by coding in Python on Linux.

Of course the PWM signal to the ESCs needs absolute accuracy, but luckily the Raspberry Pi does provide hardware PWM.

Anyway, yesterday, I got to do some testing on the newly restructured code, and the results were underwhelming.  Not terrible but not the radical change I was expecting.  The only possible cause I could see in the stats  was noise swamping accelerometer data.

I’m now not sure how exactly that led to a train of thought about fine-tuning time in my code, but it did.

The net result now is that there is a single time-stamp in the code taken immediately after I read a new batch of sensor data.  From that point onwards, time effectively stops for the processing of that data by the rest of the code.  All integration, averaging, and PID integration and differential processing uses that single timestamp.  That makes for best accuracy in all three(ish) areas.  It also has speeded the code up by another 5% (all 7 PIDs had their own time tracking which has now been removed).  This means I can also reduce noise by including more sensors samples in the integration and averaging code before feeding it to the PIDs at the same rate as before.  So beneficial in lots of ways.

Timing innaccuracy probably wasn’t the cause of the accelerometer noise, but at the same time, improving it has to be “a good thing”TM.  My best guess as to the cause of the noise is a dodgy motor which if spun manually, sounds like it has muck in its bearings.  Also the new high power props spin slower bringing more prop noise in under the MPU6050 dlpf.

I’m now hoping the timing rework, combined with lower dlpf and a new motor will cut the noise down significantly allowing me to see the results of the code rework.

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

 

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.

That was just embarrassing

The test flight from the other day with zero drift working well on hover does appear to have been a one off.  Today, there was drift, and there was to some extent corrective action in place too, but it was too small and too late – I suspect PID tuning as the ‘too late’ action suggest the horizontal velocity PID I-gain is dominant.  I think for the moment, I’ll stick with D-gain disabled as the results were still better than previously.

So today I moved on to trying my larger, more powerful blades (11 x 3.7 rather than 10 x 3.3).  This should reduce the spin rate for a given power or put it another way, more oomph available for corrective action.  What a mistake that was; 2 flights, 2 blades (£25) smashed to pieces, and a significant reduction in the (already limited) drift control.

The only plus side is that it did suggest a cause and fix:  with the bigger blades, spin rate was much reduced, perhaps allowing more noise past the DLPF and in to swamp the genuine sensor readings; so on Wednesday (next good forecasted weather), I’ll head down to the park (no walls, no iron railings for dismantling props) with Phoebe reloaded with her 10 x 3.3 props, and a lower (10Hz) DLPF to see the net result.

I simply can’t risk another crash – the 10 x 3.3 blades are rare as hen’s teeth compared to the 11 x 3.7 – perhaps that’s why?

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.