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.
Both flights use identical code. There are two tweaks compared to the previous videos:
- I’ve reduced the gyro rate PID P gain from 25 to 20 which has hugely increased the stability
- 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:
- build up the dictionary of the set of macro-blocks
- 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.
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!
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.
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.
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.
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.
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.
This is a quick test with Zoe in the lounge to confirm a suspicion. I lifted Zoe from the ground by about 1m, held her there, and then returned her to the ground.
edz_input (blue) and evz_input (orange) come direct from the LiDAR. Σ evz_input δt is the integral of the velocity (grey). Blue and grey should be pretty close matches. According to the LiDAR spec, the velocity register data is positive as the reflective surface moves away from the sensor i.e. as Zoe climbs away from the ground. But clearly it’s not. That would explain a significant chaos in current test flights. Thank heavens it’s a trivial fix!
It’s just a completely crass discrepancy between the sensor and it’s documents. Unbelievable.
Nothing exciting to report I’m afraid – still waiting for the rain to stop to take the girls out. So I thought I’d show you what they look like. Both are now kitted out with LiDAR and RaspiCam for vertical and lateral distance sampling. They run the same software – the code enables 8 ESC objects if the hostname is hermione.local or 4 otherwise:
As you may have guess, Hermione’s lid is a Salad Bowl from Italy – I spotted one we had and it fitted perfectly, so I got myself an orange one – £5 + £15 shipping to avoid the Italian postal service stealing it in transit. The bowl is concave on the underside (in its role as a salad bowl). Luckily, it’s 10cm diameter matching some black acrylic disks I have (one of which is on Hermione’s underside to which attaches the Garmin LiDAR Lite and the0 RaspiCam. Ultimately, the Scanse Sweep will be attached to this top black disc.
On that train of thought, I need to get a move on as the Scanse Sweep could well arrive before Christmas according to the latest update note.
Zoe now has her new PCB, a Garmin LiDAR-Lite and camera. Initial tests aren’t showing any of the I2C, FIFO or power black-outs. The first test with the motor-power disengaged is to check the combination of video vectors, height and yaw.
So standing in the center of the lounge, I held Zoe at arms length and rotated her around in a few circles where she was always facing the direction she was going; the video output processing averages out yaw thus only produces linear movement; the yaw is reinstated from the gyro:
The lighting in the lounge wasn’t bright, and the rug she pirouetted around was low contrast but heavily textured. She ‘landed’ at the same spot she took off from. Overall I call this a success – 0.75 meters drift error over a 36 second flight is amazing!
Next step: power up the motors and take her for a flight outside, probably tomorrow when the torrential rain and 42mph winds subside. The question here is whether the errors come back once she’s powered by the LiPo via a 5V 1,5A switching regulator.