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.

Accelerometer digital low pass filter

Yet more indoor testing, indirectly GERMS related, more aimed at investigating drift against the accelerometer dlpf setting.

Speculatively, the net is that

  • alpf of 3 or less (>= 41Hz) means there is no vertical drift, but there is horizontal drift because real X- and Y-axis noise is not being filtered out and the motion processing thinks there’s X- / Y-axis acceleration when there isn’t.*
  • alpf of 4 or higher (<= 20Hz) gives no horizontal drift, but there is vertical drift because real Z-axis acceleration is being filtered out as noise meaning the motion processing thinks there’s less acceleration than there is.*

Again speculative, I think X- and Y- axis noise was due to unbalanced props leading to asymmetric noise which when integrated leads to non-zero drift velocity: if one of the props has slight damage, this will generate the assymeteric noise very well.

My props do have nicks and muck on them, so I cleaned them up, and tried alpf 3 again, but the drift still remained – to some extent expected as the cleaned props still had nicks in them.  So out came a set of unflown props.  Balanced as best I can, I tried again at alpf 3. Some vertical drift had appeared unexpectedly, so I tried alpf 2, and it was perfect.  So that’s what I’ll be sticking with.

The only down side of this is that the noise getting through at alpf 2 means my germs idea in its basic form is a no go.  It’s shelved for the moment as it’s no longer necessary for my short flights, and instead, I’ll see how far I can extend the complementary filter to see how long a zero drift flight can be achieved.


*To be more precise, it the integration of the acceleration (i.e. the velocites) that are wrong. The Z-axis velocity is greater than zero in reality but zero according to the motion processing due to the acceleration that had been filtered out. In contrast, the X- and Y- velocities are zero according to motion processing, but drifting in reality as the dlpf is letting through asymmetric noise.

 

Better GERMS stats

GERMS @ 20Hz ALPF

GERMS @ 20Hz ALPF

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.

 

One step beyond…

lies Madness.  With such good results from reducing the dlpf filters and the samples per motion, there was once more step: set the gyro dlpf to 0; but that has implications.  Setting the gyro dlpf to 0 sets the filter frequency to 250Hz – a good thing, and the delay to 0.97ms – an even better thing.  But it also changes the sampling rate of the ADC to 8kHz from the standard 1kHz according to the spec – still fair enough – just a few extra lines of code required…

So what should have resulted was still data samples at 250Hz, but with a lag < 1ms – even higher precision forecasting of Euler angles based on gyro results.

But what I saw was the same 14s ‘flight’ according to the number of sensor reads at 250Hz actually took 19.95s according to time.time().  That’s just plain odd – it suggests the sampling rate was actually ≈175Hz.  This corresponds to a sample rate divider of  about 48 instead of the 32 it should be (8000/166.66 vs. 8000/250).

I tried to reverse engineer what the divider should be used to get 250Hz sampling, but regardless of the value (I tried 19, 31 and 47), I always got 20s elapsed time flights.  Most odd – I think I’ll just shelve gyro dlpf 0 for now.

 

A look back at time

With the latest code, sampling the sensors at 250Hz, and doing motion processing at 50Hz, I’m capturing every sample: in an approximately 10.6 second flight measured by time.time() wrapped around the flight code, I’m seeing 530 motion processing runs and 2650 samples to  0.1% accuracy.

And yet she drifts a couple of meters in that 10 seconds.  For a while I thought that was it: an A2 couldn’t capture any more samples, and I had no choice but to break out more sensors, which would also be dependent on the A2.

But then, one of those thoughts occurred to me – yes, once again when I was dozing on the sofa to refresh my brain: the rotation rates from the gyro are a lot more critical now.  They are used to approximate the current angle of tilt based upon the previous one.  That then is used to rotate accelerometer readings to the earth frame, pass them through the gravity extraction filter, and then rotate the extracted gravity back to the quad frame to calculate better Euler angles.

And here’s the crux: previously I’d been using the MPU9250 digital low pass filters (dlpf) to filter out ‘noise’, but there was a price: the filtering causes a delay as well as a reduction in noise.  And that delay means the prediction of the current angle would always be wrong.

The gyros are not ‘noisy’ and they are factory calibrated.  So I decided to turn the gyro dlpf to it’s minimum 188Hz with 1.9ms delay; and while I was at it, I thought it worth doing something similar with a accelerometer DLPF.

And sure enough, back to zero drift for the flight.

For anyone using the code I posted to GitHub yesterday, search for

cli_alpf = 3
cli_glpf = 2

and change them to

cli_alpf = 1
cli_glpf = 1

and see what happens!

Pardon? Too noisy, can’t hear you!

The difference between Chloe’s flight in the video, and the ones I tested the day before is the digital low pass filter, or to be more specific the accelerometer’s dlpf since the MPU-9250 provides independent filters for gyro and accelerometer data.

This isn’t the anti-aliasing filter which is applied prior to digital sampling of the analogue  accelerometer at 1kHz – the dlpf is aimed solely at getting the data you want, and not the noise you don’t.  I don’t know what the anti-aliasing filter is set to, but it must be less that 500Hz in the same way music is sampled at 44.1kHz even though what’s nominally audible is below 20kHz – there’s a high-order low-pass anti-aliasing filter just above 20kHz,

Anyway, for the previous flights, Chloe’s accelerometer filter was set to 184Hz on the basis that the higher the filter frequency, the lesser the delay in processing the filter (5.8ms according to the spec).  I dropped it to 92Hz (7.8ms delay), and the improvement was dramatic.

And that suggests the primary cause of my drift now is possibly noise: higher frequency vibrations from the motors (for example) are irrelevant to the flight speeds, but could cause errors in velocity integration because the pass band of the DLPF is set too high.

There’s clearly a balance however which is that if the DLPF is set too low (41, 20, 10 and 5Hz are the other options), while the noise will be filtered out, the risk of filtering out the good stuff increases as does the lag (11.8, 19.8, 35.7 and 66.9ms respectively) i.e. how out of date the filtered data is!  Perhaps that’s where Kalman steps in as it offers a level of prediction (so I’ve read) which could overcome this lag.  But for now, Kalman can continue sitting on the back-burner while I play with the dlpf values to find that magic number between noisy and necessary data samples.

Maiden voyage

HoG lost her flight virginity today, and she lost it with enthusiasm – a little too much to be honest.  Three second flight plan – one second takeoff to 0.5m, one second hover and one second descent.  All maiden flights are a huge gamble: in HoGs case, she had

  • new arms
  • new props
  • new motors
  • new frame
  • new calibration method
  • new butterworth filter parameters.

Given that, I’d say her performance was surprisingly good!

She took off vertically from sloping ground.  That alone is nearly an unqualified success.  For it to have been a complete success though, she would have stopped at 0.5m off the ground and hovered.  Instead, she whizzed up to 3m  and then I hit the kill switch even before she had the change to try to hover.

A few lessons learnt even from such a short flight though:

  • zero g calibration seems to work, but it needs doing for X, Y and Z axis
  • having dlpf set to 180Hz rather than the normal 20Hz probably wasn’t a smart move regardless of how good the Butterworth might be
  • aluminium arms bend and don’t straighten when they hit the ground at over 7.5ms-1!

New arms are on the way and will arrive tomorrow, allowing me to do the zero g calibration of the Z axis also!

But what’s Zero-G calibration, and how do you do it without going into space?

Historically, I’ve been jumping through hoops trying to get sensor calibration stable, controlling the temperature to 40°C while rotating her in the calibration cube to measure ±g in all three axes to get gains and offsets.  Yet despite all that effort, the sensors, and hence Zoë, still drifted, even if only modestly over time, still enough that she couldn’t fly in the back garden for more than a few seconds without hitting a wall.

The move to the MPU-9250 for HoG from Zoë’s MPU-6050 IMU initially seemed a retrograde step – it didn’t seem to be able to measure absolute temperature, only the difference from when the chip was powered up.  And that meant the 40°C calibration could no longer work.  Lots and lots of reading the spec’s yielded nothing initially,

But in passing I’d spotted some new registers for storing accelerometer offsets to allow them to be included in the IMU motion processing.  That suggested there was a way to get valid offsets.  Additionally, again in passing, I’d spotted a couple of Zero-G specifications: critically that the Zero-G level change against temperature was only ±1.5mg / ºC.  That means an offset measured in a Zero-G environment hardly drifts against temperature.   And a Zero-G environment doesn’t mean going up to space – it simply means reading the X and Y axis values when the Z-axis is aligned with gravity.  So with HoG sat on the floor, X and Y offsets are read, and then holding her against a wall gives the Z offset.  So calibration and updating the code takes only 5 minutes and requires no special equipment.

Delight and despair at the same time: delight that I now had a way forwards with the MPU-9250 (and it would work with the MPU-6050 also), but despair at the time and money I’d spent trying to sort out calibration against temperature.

Code updated on GitHub

I’ve just posted the latest code to GitHub.  With my old-stock much cheaper carbon fiber 11 x 5 props fitted, I was able to do several test flights without a care in the world about smashing into things, and as a result, I know I have code worth posting.

Ironically these cheaper blades are also stronger as they don’t have the cork sandwich like the T-motor ones.  So despite several arguments with walls, swings, swingball posts, and a slide, I’m still using the same set of 4 I started with.

Next steps then are to tinker with dlpf, gravity filter and motion PID settings to kill the fixed velocity drift (which arises during takeoff), and the absolute height attained on take-off.  And I can do so with confidence know I have a stock of cheap blades, and if they do run out, a replacement set is a quarter of the price of the old ones.

Zoë, meet the metaphorical brick wall.

Zoë’s been out several times today; all pretty much identical: she took-off, climbed for a second as programmed, halted at hover but slowly accelerated upwards from the hover to an ever increasing vertical climb; throughout there was nigh on zero horizontal drift.  Then I killed the flight before she went too high.  And that’s despite the complementary filter I’ve added to try to extract slow gravity drift from the peaks of real acceleration.  The complementary filter is just too crude a method – it lags too much for the frequency of filtering I need.

Ideally what I’m looking for is a way to get two accelerometer outputs from the MPU6050: one with a dlpf set to 0.1Hz to get real acceleration filtered out from (gravity + acceleration) and the second one (as now) running 45 or 90Hz to ensure nothing is missed.  Realistically that means I need to develop my own dlpf at 0.1Hz with a high cut-off rate.

I’ve found a couple of links already, here and here for me to start reading.

In the meantime, Zoë’s grounded until I can solve the sensor drift.  And before anyone else comments about barometer / altimeter / ultrasonic range finder, yes, I know those will solve the problems easily but within restrictions I’m not willing to accept for the moment.

As a result, this seems like a good point to update the code on GitHub – the main change is the threading model.

Minor update

I’ve added the gravity complementary filter and a few other bits to hopefully work together with it:

  • dlpf is now set to 2 – 94Hz with 3ms lag (from 3 – 44Hz and 4.9ms lag)
  • motion processing frequency of 71Hz (from 43Hz)

The aim of the first change is to ensure we get to see the acceleration spikes, so the filter can take them out of gravity.  We can’t let the dlpf take them out as the motion processing needs to see them to calculate net acceleration with gravity removed.

The aim of the second change is to increase the resolution of the motion processing cycles so that fewer cycles are impacted by acceleration spikes.

Both of the above changes are only possible due to the recent increase of speed in the code from ∼450Hz to ∼900Hz.

Net result though wasn’t significant in the first test flight – same climb well above what the flight plan defined.  Not a problem, I realized it would need tuning.

What was a problem is the battery bank I use to power Phoebe.  It appears they don’t like impacts.  I’ve gone through 4 now – it seems as though the USB B socket gets detached; the bank charges fine, but there’s no power from the USB B socket.  Ironically, they are beautifully made, and I can’t find a way to open them up to fix the problem.

I could do with a super low profile USB B plug, but I’ve never seen such a thing.

Instead another tenner to Amazon for yet another battery bank.