Back on the wagon

I’ve flown my Mavic probably for 20 minutes over the course of 5 short flights simply to get familiar with the controls while dodging the rain showers of the last couple of days.  I’m back inside again trying to track down why Hermione has started throwing her I²C wobbly again.

Motion processing is working well, keeping processing close to the minimum 100Hz regardless of other sensor inputs – here 156 samples were processed in 1.724s.

Processing rate

Processing rate

Garmin’s height is running stably at the intended 20Hz and it’s well withing the accuracy possible for distances less than 1m

Garmin LiDAR v3

Garmin LiDAR v3

Here’s the problem though: the IMU is fine for 862 samples averaged into the 155 motion processing blocks, showing just gravity as Hermione sits on the ground, but suddenly the IMU values spike for no reason for the 156 sample average.  Note that this happens only when the Garmin is plugged in.  There are in fact two spikes: the first is shown, the second causes an I/O exception and the diagnostics are dumped:

IMU stats

IMU stats

I’ve tried power supplies up to 3.4A, both battery and mains powered; I’ve resoldered various critical PCB joins; I’ve added the 680uF capacitor as the Garmin spec suggests despite Zoe being fine without it, and I’ve used a newly flashed SD card, all to no avail.

I have two things left to try:

  • currently the Garmin is read every motion processing loop, despite being updated at 20Hz; the spec says there’s an interrupt, but as yet, I’ve not got it to work.  Must try harder!
  • Failing that, I’ll have to replace the MPU-9250 with another, and see if the current one is faulty.

Beyond these two, I’m out for ideas.

The GPS + compass plan

My intent with GPS and compass it that Hermione flies from an arbitrary take-off location to a predetermined target GPS location, oriented in the direction she’s flying.

Breaking that down into a little more detail.

  • Turn Hermione on and calibrate the compass, and wait for enough GPS satellites to be acquired.
  • Carry her to the destination landing point and capture the GPS coordinated, saving them to file.
  • Move to a random place in the open flying area and kick off the flight.
  • Before take-off, acquire the GPS coordinates of the starting point, and from that and the target coordinates, get the 3D flight direction vector
  • On takeoff, climb to 1m, and while hovering, yaw to point in the direction of the destination target vector using the compass as the only tool to give a N(X), W(Y), Up(Z) orientation vector – some account needs to be taken for magnetic north (compass) vs. true north (GPS)
  • Once done, fly towards the target, always pointing in the way she’s flying (i.e. yaw target is linked to velocity sensor input), current GPS position changing during the flight always realigning the direction target vector to the destination position.
  • On arrival at the target GPS location, she hovers for a second (i.e. braking) and decends.

There’s a lot of detail hidden in the summary above, not least the fact that GPS provides yet another feed for 3D distance and velocity vectors to be fused with the accelerometer / PiCamera / LiDAR, so I’m going to have to go through it step by step

The first is to fly a square again, but with her oriented to the next direction at the hover, and once moving to the next corner, have yaw follow the direction of movement.  Next comes compass calibration, and flight plan based upon magnetic north west and up.

However, someone’s invoked Murphy’s / Sod’s law on me again: Hermione is seeing the I2C errors again despite no hardware or software changes in this area.  Zoe is behaving better, and I’m trying to double the motion tracking by doubling the video frame rate / sampling rate for the Garmin LiDAR-Lite; the rate change is working for both, but the LiDAR readings see to be duff, reading 60cm when the flight height is less than 10cm.  Grr 🙁

One last sanity check

Before moving on to compass and GPS usage, there’s one last step I want to ensure works: lateral movement.

The flight plan is defined thus:

  • take-off in the center of a square flight plan to about 1m height
  • move left by 50cm
  • move forward by 50cm – this place her in to top left corner of the square
  • move right by 1m
  • move back by 1m
  • move left by 1m
  • move forwards by 50cm
  • move right by 50cm
  • land back at the take-off point.

The result’s not perfect despite running the ground facing camera at 640 x 640 pixels; to be honest, with lawn underneath her, I still think she did pretty well.  She’s still a little lurchy, but I think some pitch / roll rotation PID tuning over the IKEA mat should resolve this quickly.  Once again, you judge whether she achieved this 34 second flight well enough?

She cannae take any more, Captain!

Zoe is now maxed out;  any increased video frame size above the current 400 x 400 pixels at 10 fps leads to a IMU FIFO overflow i.e. the video processing simply takes too long.  She’s also run out of physical space on her frame for more sensors.

On the other hand, Hermione has loads of physical space, but is plagued with I2C problems on her A+.

To add GPS for flight plans, and Scanse Sweep for object avoidance, I need more cores.  For the moment that means a B2 or B3.  Currently I’m leaning towards a B2 as I don’t need the 64 bit kernel of the B3, nor the built in WiFi or Bluetooth – I’d rather continue to use a faster WiFi USB dongle.  With the extra cores, I can move the video processing out of the motion processing into a different process, and have it feed the latest values to the motion processing when available.  Hopefully that will mean support for higher video frame size and rate and perhaps also increase the IMU sampling rate back to the 1kHz – I’ve had to reduce it to 500Hz currently.  Having the extra cores and 4 USB ports means GPS and Scanse Sweep should be much easier to add – I doubt the A3 (if it ever appears) will support those extra USB ports.

So it’s a B2.  For the sake of up to date build instructions, I’ll be installing her from scratch and blogging the instructions once complete.


On a cold winter’s morning…

Both flights use identical code.  There are two tweaks compared to the previous videos:

  1. I’ve reduced the gyro rate PID P gain from 25 to 20 which has hugely increased the stability
  2. Zoe is using my refined algorithm for picking out the peaks in the macro-block frames – I think this is working better but there’s one further refinement I can make which should make it better yet.

I’d have liked to show Hermione doing the same, but for some reason she’s getting FIFO overflows.  My best guess is that her A+ overclocked to turbo (1GHz CPU) isn’t as fast as a Zero’s default setting of 1GHz – no idea why.  My first attempt on this has been improved scheduling by splitting the macro-block vectors processing into two phases:

  1. build up the dictionary of the set of macro-blocks
  2. processing the dictionary to identify the peaks.

Zoe does this in one fell swoop; Hermione schedules each independently, checking in between that the FIFO hasn’t filled up to a significant level, and if it has, deal with that first.  This isn’t quite working yet in passive test, even on Zoe, and I can’t find out why!  More anon.

30s with, <10s without

A few test runs.  In summary, with the LiDAR and Camera fused with the IMU, Zoe stays over her play mat at a controlled height for the length of the 30s flight.  Without the fusion, she lasted just a few seconds before she drifted off the mat, lost her height, or headed to me with menace (kill ensued).  I think that’s pretty conclusive code fusion works!

With Fusion:

Without Fusion:

Strutting her stuff

Finally, fusion worth showing.

Yes, height’s a bit variable as she doesn’t accurate height readings below about 20cm.

Yes, it’s a bit jiggery because the scale of the IMU and other sensors aren’t quite in sync.

But fundamentally, it works – nigh on zero drift for 20s.  With just the IMU, I couldn’t get this minimal level of drift for more than a few seconds.

Next steps: take her out for a longer, higher flight to really prove how well this is working.

Irritations and Innovations

No breakthroughs to report but:

  • Zoe is now running indoors safely with or without motion fusion installed
  • Without the fusion, she drifts horizontally and continues to rise during hover phase: this suggests the value for gravity at takeoff has drifted during the flight, perhaps temperature related?  It’s only about 15°C in our house currently which is outside the range she works well in.  First test is to add a blob of blue tack on the IMU so it isn’t cooled by the breeze from the props.
  • With fusion, her height is much better, but she swings laterally around her takeoff point – the Garmin LiDAR lite is doing it’s job well but there’s some tuning required for the lateral motion from the Raspberry Camera.  Also it’s dark in the play room, even with the lighting on, so I’m going to add LED lighting under her motors to give the camera better site.  She’s flying over an IKEA LEKPLATS play mat, but ours seems very faded, so I’ll be getting her a new one.
  • I’ve added a whole bunch of safety trip wires so that, for example, if she’s 50cm above where the flight plan says she should be, the flight dies.  Together these make her much safer for flights indoors.
  • I’ve added enhanced scheduling to prioritise IMU over camera input when the IMU FIFO is reading half-full; this is to prevent FIFO overflows as camera processing sometimes takes a while, and the overflows have been happening a lot recently.
  • I’ve also added another couple of pairs of PIDs – I’m not sure how I got away without them before.  The equivalent controls yaw perfectly, but the pitch and roll angles were missing, skipping straight to the rotation rates instead.
    • distance (target – input) =PID=> corrective velocity target
    • velocity (target – input) =PID=> corrective acceleration target
    • acceleration target => angular target (maths to choose an angle for a desired acceleration)
    • angle (target – input) =PID=> corrective rotation target
    • rotation (target – input) =PID=> PWM output

Together all these changes require a lot of tuning, tinkering and testing; I hope to report back with a video when there’s something worth sharing.

Live Cold Fusion Test

I took Zoe outside (temperature 0°C – freezing point) to fly this morning with fusion enabled to see what the effect was – fusion was disabled in the flights; they was purely for compare and contrast of the two independent sensor sources.

Fusion was a complementary filter with the -3dB crossover set to 1s.

In all graphs,

  • blue comes from the Garmin LiDAR (Z axis) or Camera (X and Y axes)
  • orange comes from the accelerometer with necessary subtraction of gravity and integration.
  • grey is the fused value – orange works short term with longer term fusion with blue.


In general, the shapes match closely, but there’s some oddities I need to understand better:

  • horizontal velocity from the Camera is very spiky – the average is right, and the fusion is hiding the spikes well – I’m assuming the problem is my code coping with the change of tilt of the camera compared to the ground.
  •   the vertical height is wrong – at 3 seconds, Zoe should be at 90cm and leveling out.

I need to continue dissecting these stats – more anon.

Diagnoses and Cures

Hermione’s “reach for the stars” was due to I²C errors; I suspected powewr brown-outs.  Her regulator for the LiPo provided only 1.5A, so I tried her passively with mains PSU of 5V at 1A and 2.5A – the error was the same – shifted outputs from the IMU FIFO without any FIFO overflow.  That suggested a interaction with the I²C with the Garmin instead.  I rebuilt the cable with two UTPs (unshielded twisted pairs): SCL with Vss / GND and SDA with Vdd / 5V as per the PX4FLOW spec for long I²C wiring.  I was stunned – it just worked, regardless of whether the 1A or 2.5A power supply was used, I no longer got any I²C corruption.  Next step clearly is to test her live outdoors and check she no longer reaches for the stars.

I also had the bottle to let Zoe loose in the play room.  She still hardly got off the ground on the first flight, so it’s not temperature drift.  However, her second run was perfect, which reminded me that her first run was always cr@p for some reason.  Here’s the stats for both.  The stats are logging both accelerometer and Garmin / Camera distances.  There’s such a tight correlation between the very difference sensors that I’m very tempted to turn the fusion on.  Just a tad more bottle needed.  The key one for each is the bottom left: how high was she according to the two sensor sources.

Flight one

Flight one

Flight 2

Flight 2

I thinkthat’s my courage bottle empty for the day.  When it’s charged up tomorrow, I’ll take the sisters outside to test the above next steps.