As a Father’s Day present from me to me, I’ve just bought a PX4FLOW from ebay.  Reik Hua’s quad can hover until bored or battery discharge with one of these.  Mine’s on a slow-boat from China so could take up to a month to arrive.

When it does, the LEDDAR will go into safe-keeping – not retired, just pending the arrival of its sibling LiDAR Scance Sweep.

Beyond saving me from terminal boredom syndrome (TBS), the PX4FLOW will give me stuff to do over the summer and motivation.  Primarily it gives me  high precision Z axis height and X and Y axis velocities.

So the list of stuff to do for the summer includes

  • Fuse X,Y velocoties with integrated acceleometer velocities from long term low drift hover
  • add distance / direction PIDs with integrated PX4FLOW velocities for X and Y, and it’s build in sonar height for Z axis – this should stop the lurge.
  • get the compass working just for additional yaw stability to start with, though this can obviously be extended to provide orientation.
  • Play with fixed height, horizontal movement flight plans
  • Have a go at flying in circles – requires angle PID to provide a constant port/starboard angle combined with a fore/aft constant velocity


  • get DHCP / udhcp working on latest Jessie Zoe WAP
  • get Wiimote working for crawler robot – this will probably be my test platform for the Scan Sweep

Fusion flaw

I collected some diagnostics to start investigating the wobbles shown in the previous post’s video:

Fused velocities

Fused velocities

It’s fairly clear the fusion bias is still strongly towards the accelerometer.  In a way I’m glad as it makes my next steps very clear.

The benefits of having a Brazilian…

comment on the blog: courtesy of Gustavo yesterday, I’m now able to empty the FIFO a batch at a time with a single I2C read (12 bytes) rather than 12 reads of 1 byte.  That’s made my code a lot faster, buying even more time to check other sensors etc.

It’s all to do with i2c.readList (i2c.smbus.read_i2c_block_data): I’d used readList when I was accessing the data registers direcly; reading 14 bytes from register 59 actually gave me a single byte each from registers 59 to 72 – the full set of sensor data I needed.  As a result, I’d ruled out using readList at register 116 (FIFO) getting me 12 bytes per read from just that single register; but that’s what Gustavo does and sure enough it worked for me too.

Sadly a quick flight this morning still showed the same negative G problems – have a look at the az values – these should read roughly 16384 at hover; these aren’t all the samples, just the ones showing negative G during the 8s flight:

[WARNING] (MainThread) __init__ 1397, Zoe is flying.
[WARNING] (MainThread) __init__ 312, SRD:, 1
[WARNING] (MainThread) __init__ 411, IMU core temp: 19.888756
[WARNING] (MainThread) fly 1522, fly = False, flight plan = , calibrate_0g = 0, hover_target = 150, shoot_video = False, vvp_gain = 400.000000, vvi_gain = 200.000000, vvd_gain= 0.000000, hvp_gain = 1.500000, hvi_gain = 0.100000, hvd_gain = 0.000000, prp_gain = 110.000000, pri_gain = 11.000000, prd_gain = 0.000000, rrp_gain = 90.000000, rri_gain = 9.000000, rrd_gain = 0.000000, yrp_gain = 80.000000, yri_gain = 8.000000, yrd_gain = 0.000000, test_case = 1, rtf_period = 1.500000, tau = 5.000000, diagnostics = False
[WARNING] (MainThread) load0gCalibration 551, 0g Offsets:, 0.000000, 0.000000, 0.000000
[WARNING] (MainThread) fly 1522, fly = True, flight plan = fp.csv, calibrate_0g = 0, hover_target = 380, shoot_video = False, vvp_gain = 400.000000, vvi_gain = 200.000000, vvd_gain= 0.000000, hvp_gain = 1.500000, hvi_gain = 0.100000, hvd_gain = 0.000000, prp_gain = 110.000000, pri_gain = 11.000000, prd_gain = 0.000000, rrp_gain = 90.000000, rri_gain = 9.000000, rrd_gain = 0.000000, yrp_gain = 80.000000, yri_gain = 8.000000, yrd_gain = 0.000000, test_case = 0, rtf_period = 1.500000, tau = 5.000000, diagnostics = False
[WARNING] (MainThread) load0gCalibration 551, 0g Offsets:, 0.000000, 0.000000, 0.000000
[WARNING] (MainThread) fly 1661, pitch -1.718589, roll -2.401715
[WARNING] (MainThread) fly 1662, egx 0.000000, egy 0.000000, egz 0.976289
[WARNING] (MainThread) fly 1739, 0 data errors; 0 i2c errors; 0 2g hits
[CRITICAL] (MainThread) readFIFO 469, ax: -20304; ay: 8588; az: -556; gx: 1501; gy: 1042; gz: 1181!
[CRITICAL] (MainThread) readFIFO 469, ax: 6156; ay: -784; az: -552; gx: -379; gy: 454; gz: -75!
[CRITICAL] (MainThread) readFIFO 469, ax: 1580; ay: -2772; az: -564; gx: -2126; gy: 304; gz: -339!
[CRITICAL] (MainThread) readFIFO 469, ax: -8784; ay: -1812; az: -608; gx: -2906; gy: 239; gz: -864!
[CRITICAL] (MainThread) readFIFO 469, ax: 5016; ay: 1420; az: -636; gx: -1649; gy: 2227; gz: 1277!
[CRITICAL] (MainThread) readFIFO 469, ax: 9652; ay: -1816; az: -732; gx: -1665; gy: 1730; gz: 751!
[CRITICAL] (MainThread) readFIFO 469, ax: -84; ay: 2328; az: -2392; gx: -2358; gy: 1965; gz: 926!
[CRITICAL] (MainThread) readFIFO 469, ax: -5884; ay: -1184; az: -108; gx: -75; gy: 1438; gz: 351!
[CRITICAL] (MainThread) readFIFO 469, ax: -2180; ay: 7628; az: -2316; gx: 493; gy: 2743; gz: 384!
[CRITICAL] (MainThread) readFIFO 469, ax: -152; ay: 5336; az: -2208; gx: 407; gy: -860; gz: -703!
[CRITICAL] (MainThread) readFIFO 469, ax: 21212; ay: -2948; az: -392; gx: -175; gy: -1606; gz: -538!
[CRITICAL] (MainThread) readFIFO 469, ax: -7704; ay: -5088; az: -1184; gx: -899; gy: 2470; gz: 10!
[CRITICAL] (MainThread) readFIFO 469, ax: 4904; ay: -2688; az: -676; gx: -1409; gy: 2067; gz: 649!
[CRITICAL] (MainThread) readFIFO 469, ax: 11732; ay: 3232; az: -976; gx: -1023; gy: 1170; gz: 1019!
[CRITICAL] (MainThread) readFIFO 469, ax: 4920; ay: 2740; az: -3212; gx: -552; gy: 958; gz: 667!
[CRITICAL] (MainThread) readFIFO 469, ax: -10364; ay: 5020; az: -2744; gx: -553; gy: 839; gz: 310!
[CRITICAL] (MainThread) readFIFO 469, ax: 4104; ay: -1516; az: -2756; gx: -693; gy: 46; gz: 654!
[CRITICAL] (MainThread) readFIFO 469, ax: -10048; ay: 5772; az: -20; gx: -686; gy: 283; gz: 1207!
[CRITICAL] (MainThread) readFIFO 469, ax: -6844; ay: 5884; az: -1868; gx: -424; gy: 974; gz: 703!
[CRITICAL] (MainThread) readFIFO 469, ax: 6428; ay: 1472; az: -368; gx: -394; gy: 496; gz: 582!
[CRITICAL] (MainThread) readFIFO 469, ax: -3064; ay: 9312; az: -1852; gx: -575; gy: 2588; gz: 801!
[CRITICAL] (MainThread) readFIFO 469, ax: -840; ay: 6392; az: -1944; gx: -366; gy: 1737; gz: 149!
[CRITICAL] (MainThread) readFIFO 469, ax: -2468; ay: 10792; az: -1580; gx: -737; gy: -727; gz: -65!
[CRITICAL] (MainThread) readFIFO 469, ax: -4448; ay: 2652; az: -3624; gx: -974; gy: 639; gz: -111!
[CRITICAL] (MainThread) readFIFO 469, ax: 4604; ay: 7728; az: -4000; gx: -560; gy: -61; gz: -529!
[CRITICAL] (MainThread) readFIFO 469, ax: -23048; ay: 9136; az: -4992; gx: 1366; gy: -524; gz: -5!
[CRITICAL] (MainThread) readFIFO 469, ax: -3860; ay: -1384; az: -488; gx: 1241; gy: -1463; gz: 293!
[CRITICAL] (MainThread) readFIFO 469, ax: -12488; ay: 6820; az: -516; gx: 2382; gy: -3340; gz: -664!
[CRITICAL] (MainThread) readFIFO 469, ax: -2024; ay: -7380; az: -40; gx: -1119; gy: -831; gz: 76!
[CRITICAL] (MainThread) readFIFO 469, ax: -3676; ay: 1472; az: -1756; gx: -687; gy: 6099; gz: 2686!
[WARNING] (MainThread) fly 2068, IMU core temp: 18.121548
[WARNING] (MainThread) fly 2069, motion_loops 934
[WARNING] (MainThread) fly 2070, sampling_loops 9522
[WARNING] (MainThread) fly 2072, 30 data errors; 0 i2c errors; 0 2g hits

That’s 30 errors detected out of 9522 samples, so not terrible, but not perfect.  I was flying her at her new high speed sampling rate of 1kHz and with the accelerometer low pass filter set to 0 (460Hz).  Next step is to turn off the protective code that skips these negative G values and have another go with alpf 2 (92Hz) to see what happens. Certainly the other errors of -32768 in the gyro have now gone, so it’s worth a try.

¡uʍop ǝpᴉsd∩

Short version: if you’re as obsessive as I am, then the Invensense IMU’s may be flawed.

Longer versions: I followed up my duff data errors theory by adding a tiny bit of code that checked whether the Z-axis accelerometer reading was negative; that’s only possible if the quad is upside-down and powering itself towards the ground:

[WARNING] (MainThread) __init__ 1382, Zoe is flying.
[WARNING] (MainThread) __init__ 312, SRD:, 2
[WARNING] (MainThread) __init__ 411, IMU core temp: 23.261427
[WARNING] (MainThread) fly 1507, fly = True, flight plan = fp.csv, calibrate_0g = 0, hover_target = 380, shoot_video = False, vvp_gain = 400.000000, vvi_gain = 200.000000, vvd_gain= 0.000000, hvp_gain = 1.500000, hvi_gain = 0.100000, hvd_gain = 0.000000, prp_gain = 110.000000, pri_gain = 11.000000, prd_gain = 0.000000, rrp_gain = 90.000000, rri_gain = 9.000000, rrd_gain = 0.000000, yrp_gain = 80.000000, yri_gain = 8.000000, yrd_gain = 0.000000, test_case = 0, rtf_period = 1.500000, tau = 5.000000, diagnostics = False
[WARNING] (MainThread) load0gCalibration 536, 0g Offsets:, 0.000000, 0.000000, 0.000000
[WARNING] (MainThread) fly 1646, pitch -1.679818, roll -2.179765
[WARNING] (MainThread) fly 1647, egx 0.000000, egy 0.000000, egz 0.977003
[WARNING] (MainThread) fly 1724, 0 data errors; 0 i2c errors; 0 2g hits
[CRITICAL] (MainThread) fly 1804, ABORT: Duff Data: -228!
[WARNING] (MainThread) fly 2053, IMU core temp: 22.899000
[WARNING] (MainThread) fly 2054, motion_loops 218
[WARNING] (MainThread) fly 2055, sampling_loops 1127
[WARNING] (MainThread) fly 2057, 1 data errors; 0 i2c errors; 0 2g hits

That’s -228 * 2g / 32768 = 0.014g!  No wonder she drifts if that keeps happening.

Yet she wasn’t upside down; the only explanation was that the top byte of the accelerometer Z axis reading had the top bit set meaning it’s a negative value.  Most likely is actually the top bit was 0xFF – i.e. duff data of 0xFF80 was read from the FIFO as the Z axis acceleration.

On the basis the sensor might be damaged, I swapped to a brand-new one, never used and never abused or dropped.  The second sensor also produced the same result.

There were no signs of I2C errors, and I was using the FIFO, so my first finger of blame points at the MPU-9250 pushing duff data into the FIFO.  My only doubt is that this happened consistently for each of the 6+ flights I did, and every time within a second or two of takeoff.

I have some more tests to do before I can point my finger with confidence: I’ll post again when I have more information.

Better GERMS stats



Another day, another couple of indoor test flights, experimenting with the IMU accelerometer low pass filter, to see if that would help extract the GERMS info.  Previously, it was set to 2 (92Hz bandwidth); today I tried 4 (20Hz)

The good side is it clearly did – the acceleration and deceleration now stands out clearly at the 1.5, 3.5 and 5s points corresponds to the transitions to ascent, hover, and descent.

Also there was no horizontal drift – during yesterday’s flight, there was drift of half a meter or so.  Clearly yesterday some of the noise on the accelerometer X and Y axes had contributed to the X and Y axis velocities leading to horizontal drift.

The down side is that the lower bandwidth also filtered out some of the Z-axis real acceleration, so during nominal “hover”, Phoebe actually continued to climb slightly- that’s why there’s no spike at 7.5s like yesterday’s graph; Phoebe was still in the air when the flight ended so the post-flight ground impact didn’t register in the stats.

The gyro does offer a dlpf setting for each axis in the MPU-9250; it’s a shame there’s only a single setting of the accelerometer covering all 3 axes.

At least it seems like my GERMS filter idea could work in principle, even if in practise the MPU-9250 can’t be configured to play nicely together with the GERMS filter.

I’ll try 3 (41Hz) accelerometer DLPF tomorrow to see whether this is the perfect comprise.


Blind, deaf, disorientated, lost and scared of heights…

and yet, amazing.

So in that last video of a ~10s flight, Phoebe drifted about 1m when she shouldn’t have.  To me, that’s amazing; to you, that may be underwhelming.  So here’s why you should be amazed.  Basic school math(s):

distance = ½ x acceleration x time² ⇒ plugging in the numbers above says she was accelerating at 0.02m/s² (2cm/s²) instead of zero to travel 1 meter in 10 seconds.

Gravity is roughly 10m/s².  The accelerometer is set to read up to ±4g.  So that’s a range of 8g or roughly 80m/s²

So 0.02/80 * 100 = 0.025% error or a ±8 error value in the sensor range of ±32768.

Now the critical part – the sensors are only rated at 2% (±655) accuracy, and yet I’m getting 0.025% or 80 times that degree of accuracy.

And that’s why I don’t think I can get things very much better, whatever I do.

There is a slight chance that when the A2 is released (sounds like that’s shifted to next year now), I may be able to run the sampling rate at 1kHz without missing samples (I’ve had to drop to 500Hz to ensure I don’t miss samples at the moment).

Other than that though, she needs more sensors:

  • camera for motion tracking (!blind)
  • ultrasonic sensors for range finding (!deaf)
  • compass (!disorientated)
  • GPS (!lost)
  • altimeter (!scared of heights).

but they’ll also need an A2 for the extra processing.  So this is most definitely it for now.

I couldn’t let it lie!

I still feel there must be a way to get a fixed 0g offset algorithm, based on the fact off-the-shelf quads won’t have to undergo this level of faff by the user; there must be a simple setup for the manufacturer.

Here’s what I got in my latest attempt:

XY 0g offsets vs. temp change

XY 0g offsets vs. temp change

Look’s good, doesn’t it?  Nice trend lines which the samples offsets are really tightly aligned with.

The graph X-axis shows the change in temperature since the IMU started.  The 333.87 is about 1ºC, so this is what the test plots offset changes as the chip temperature increases away from the boot temperature.

The graph Y-axis shows the 0g offsets: the accelerometers scale is configured as ±4g so the vetical axis covering -1000 ⇒ +800 is the equivalent of -0.12g ⇒ +0.10g.

The lines show the 0g offsets as the difference in temperature increases from the initial boot termperature: the green ones are the accelerometer X-axis offsets; the blue ones are the accelerometer Y-axis offsets; the paler -464 lines come from a garage estimated +10ºC test environment; the darker +1600 lines come from an indoor estimated +20ºC.

OK, with that explained, down to so positive interesting bits:

  • the 0g offsets are all very close to 0 when the test starts i.e. when the temperature difference from ambient is near to 0.
  • all of the samples fit a linear trend line beautifully.
  • the 2 y-axis trends are very similar despite the tests being booted at very different temperatures

But there are very odd bits too:

  • in contrast to the y-axis trends, the x-axes trends are virtually the opposite of each over when booted at the very different temperatures
  • all of the offets are hugely different from the ones I’ve measured by the same method previously, perhaps by a factor of 10!

Net: I critically need to try to understand these oddities if I stand any chance of using these offsets in a meaningful way.  If I can, then this gives me the best possible / nigh on perfect way to allow for offsets shifting as the difference from ambient changes.

The logic behind the protective fix

It first glance, the protective fix I put in looks as though it only deals with the exact error I showed in the graph two posts ago just looking for -1 in az, temp, gx, gy, and gz:

if gz == -1 and gy == -1 and gx == -1 and temp == -1 and az == -1:
    self.num_data_errs += 1

But actually, it’s a lots more subtle than that.

The MPU data registers are as follows:

  • registers 59 – 64 represent 2 bytes each of ax, ay, and az
  • registers 65 and 66 represent 2 bytes of temperature
  • registers 67 – 72 represents 2 bytes each of gx, gy and gz.

The duff data showed that ax and ay had been filled in, but the rest had not at the point of reading the data over I2C.  So I speculate that when the registers are being filled with new values, it starts at register 59 and finishes at 72.

The value for az is critical; if the value is -1 instead of somewhere around 8000 (1g), it permanently affects the velocity values controlling the rest of the flight, leading to permanent fixed speed drift.  If az is incorrectly -1, then whether the values of ax and ay or correct or -1 doesn’t matter; the flight is doomed to drift regardless.

In contrast, gyro values are used only short term, and the influence of a duff value gets overridden by the complementary filter in a fraction of a second.  The error lives for just a few milliseconds until it’s lost forever; the flight just has a few millisecond glitch, but the next motion processing loop will correct that glitch.

So actually, the test needs only check if az == -1.  az of -1 maps to freefall with no air resistance; it simply won’t happen unless you fly the quad upside down!  But there’s a very slight chance you might see freefall values (quadcopter in ISS?), but that does mean the test can still be refined.

In ‘C’, an “if” like the above does not necessarily process all of the tests. As soon as one fails the test (for example if gz is not -1), then code code stops testing, and drops out to the “break” statement.

So the tests can be ordered so that the most important fact is checked first: az.  If az is duff, then the values for ax and ay are irrelevant, so there’s no point in checking whether they are -1 whatsoever.  If az is not -1, then even though temperature and gyro data may still be duff, if doesn’t matter, and it’s worth taking the accelerometer data.  Only if az is -1, and the temperature and gyro values are all -1 does it mean the data cannot be reused, and so must be re-acquired.

The difference is subtle, but this is python code running at 1kHz, and every little bit counts (pun intended).

The revised “if” test at the top could actually look like this:

if az == -1 and temp == -1 and gx == -1 and qy == -1 and gz == -1:
    self.num_data_errs += 1

To be honest, this really doesn’t matter, but I did a few test flights today which were a lot better, but there was a blustery wind which meant the flights weren’t visually pretty so no video, so I had time to kill, and my mind wandered.

Zero g offsets against temperature

Zero-g offsets against temperature

Zero-g offsets against temperature

The graph shows the values reported by the accelerometer (in units of g) at various IMU chip temperatures (about 10° above ambient) with a linear trend line courtesy of Excel. The x-axis looks great – virtually zero drift against temperature; ironically although the z-axis looks awful, the combination of this, plus the 4th order, 0.1Hz Butterworth gravity filter means that height is measured accurately, and is sustained throughout an (albeit short) flight.

But the y-axis offset does drift against temperature, and the spread of samples is quite, well, spread compared to the x-axis. This may well be the primary cause of the horizontal drift shown in the last couple of videos.

I think the next step is to move to the local park where I can check by sight whether the drift is of fixed velocity (which rules the above out), or fixed acceleration (which backs-up this speculation) without worrying about walls.

Ahead of the solar eclipse

Beating the solar-eclipse from Andy Baker on Vimeo.

A few test flights to see whether a bug fix I made had solved my eternal problem with vertical velocity – I’d assumed vertical acceleration would never exceed 2g, so used the ±2g configuration for the accelerometer to get the highest resolution of ±0.6mm/s2.  Some stats from yesterday suggested 2g was being exceeded on the initial take-off, meaning that throughout the flight, the integrated velocity would be wrong.  So I’ve updated the accelerometer range to ±4g (1.2mm/s2). So far that’s not had the radical change I’d hope for, so more testing to try to understand why.