Normality has returned. I took Hermione out to test what video resolution she could process. It turns out 640 x 640 pixels (40 x 40 macro-blocks) was yesterday’s video frame size. 800 x 800 pixels (50 x 50 macro-blocks) this morning was a lot less stable, and I think the 960 x 960 pixels (60 x 60 macro-blocks) explains why.
At 960 x 960, Hermione leapt up into the sky; the height breach protection killed the props at 1.5m but by then she was accelerating so hard she probably climbed to 3m before dropping back down like a brick onto the hard stone drive upside-down. Luckily only 2 props got trashed as that’s an exact match for the spares I had left.
Rocketing into the sky appears to be Hermione’s symptom of a IMU FIFO overflow. For some reason, Hermione doesn’t catch the FIFO overflow interrupt so she just carries on, but now with gravity reading much less than zero because the FIFO has shifted so in fact she’s reading gyro readings, and so had to accelerate hard to compromise. The shift happens because the FIFO is 512 bytes and I’m filling it with 12 byte batches; 512 % 12 != 0.
How does this explain the 800 x 800 wobbles? My best guess is that these 2500 macro-blocks (800² / 16²) are processed just fast enough to avoid the FIFO overflow shown by 900², but does have a big impact on the scheduling such than instead of the desired 100Hz updates to the motors, it’s a lot nearer the limit of 23Hz imposed by the code. That means less frequent, larger changes.
So that mean I need to find the balance between video frame size and IMU sampling rate filling the FIFO to get the best of both. Luckily with indestructible Zoe imminently back in the running, I can test with her instead.
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.
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.
Hermione’s been out and about using the full X8; her first flight shot her into the air at hyper-speed due to a FIFO-overflow that the code didn’t catch. With the code fixed and a replacement motor installed, I tried again. After a couple of reasonable flights proving the X8 PIDs needed no tuning, they then started to fail for a couple of reasons:
- FIFO overflows
- I2C I/O errors
Both types were caught and killed the flights, but why were they happening?
- The FIFO errors I think are due simply because I’m now pushing the limits of my A+’ performance; adding an exception to ensure the FIFO was processed as a priority when the FIFO fills over 50% has resolved that. However…
- The I2C I/O errors seem (after lots of testing) to be related to the Garmin LiDAR-Lite. If it’s plugged in, regardless of whether I’m sampling it or not, I get the I2C I/O errors. If it’s unplugged, all works perfectly.
I have various things to try out:
- It could be due to the extra power-draw when it’s plugged in. Easy to test as I’ve just bought a huge 4S2P 6600mA LiPo:
GensAce 4S2P 6600mAh LiPo
- It could be confirmation of a rumour I’d heard up that the LiDAR-Lite V2 wasn’t good at sharing the I2C bus with anyone else – I hope the V3 doesn’t suffer from the same.
- It could be due to noise on the bus caused by the 20cm long cables. I picked up some stuff from my PX4FLOW investigations about wiring requirements for long cables.
- Mentioned in the PX4FLOW docs, and also in the Garmin LiDAR Lite V3 instructions is the need for a decoupling capacitor between Vcc and ground.
If / when I can get the Garmin working together with the IMU, then it’s all systems go for sustained zero drift flights.
Based upon the walk graphed in the previous post, the measured distanced in the garden, and a crude guesstimation that I was carrying the test rig about 1m off the ground as I walked the walk, the macro-block output is about 87,500 pixels per meter height per meter distance or
horizontal distance = height * macro-block output / 87500
For the moment, that’s more than accurate enough.
A few things left to do before this code can be used in Hermione
- Currently, the video and macro block processing run in separate threads, connected by a shared memory FIFO; ultimately this needs splitting into into two processes which could then run on separate CPUs should a multi-core A3 appear.
- There’s some work to integrate the select.select() waiting on the FIFO into the main quadcopter scheduling loop – it replaces the current time.sleep() with listening on the FIFO with a timeout of the time.sleep() value.
- At the gory details level, I need to make sure the video and IMU X and Y axes are aligned when the new PSBs arrive.
Shouldn’t take too long except for the minor detail I’m off to DisneyLand Paris next week 🙁
I’ve finally got the FIFO buffer code working with lots of protection against overflow, and also using a guaranteed way to ensure the code and FIFO are always synchronised. It works perfectly, so I’ve updated the code on GitHub
Then I took Zoe and Phoebe about; with the floppy props Zoe flew OK but not as well as usual and, as usual, neither would fly at all with the CF props. Some stats revealed unsurprisingly that it’s Z-axis noise from the props; Zoe’s floppy props aren’t so floppy at freezing temperatures but when I brought her indoors, she was fine again.
The problem is the motors / props can’t react fast enough to sharp spikes in acceleration, so drift ensues – in this case downwards vertical drift keeping them both pinned to the ground when the sensors felt the spikes. I need to find a way to soften those acceleration spikes such that the net integrated velocity is the same, and the motors can react to it.
There’s a couple of approaches I can take here, and as usual, I’ll be trying both.
The first is to add additional foam buffering between the HoG and the frame to soften the blows just like Zoe’s floppy props do. The second is to tweak the vertical velocity PID gains to be dominated by the I-gain and reduce the P-gain significantly.
Phoebe Is Losing Gravity Readings; Integration’s Mauled Zoe
Sorry it’s been quiet for a few days; I’ve been trying to get to the bottom of the manic leap-into-the-air problems I’ve had with Phoebe and Zoe. It’s not solved yet, but I’m getting a better idea of why it might be happening. The symptoms are the same, but I believe the cause is different.
With Zoe, the manic climb only happened when I put on the carbon props; with the floppy props all was ok. She was running with the accelerometer scale set to ±2g. The flexibility of the floppy props softened vertical acceleration sufficiently to prevent any peaks above 2g during ascent. My speculation is that with the carbon props, she did breach the 2g limit. That means she’d be under reading acceleration leading to the integrated vertical velocity being lower than it actually was, meaning the PIDs turned up the props speeds, and off she shot. However, I’ve not had the chance to test this theory as I’ve been waiting for some replacement parts – props, arm, slimline set of connectors and a new MPU to build a much sleeker HoG for her. I’ll post again when this is done, and hopefully be able to show off the improvement.
Phoebe’s case is different; for her she’s getting FIFO overflows. At the start of the flight, she gets one good read of data, and then every further read is shifted by 4 bytes so the codes believes each FIFO data batch consists of ax, ay, az, gx, gy, gz, but what it’s actually getting is az, gx, gy, gz, ax, ay. I now have lots of error checking to ignore such batches and this has revealed that I only get one good batch of sample data before the code spots the next batch is corrupt and aborts the flight. It seems the code isn’t running fast enough to stop the FIFO overflowing; I’ve been doing various tweaks to the code to ensure the code is synchronized with the sensors, and that time usage is optimized – calls to time.time() have gone, as are a lot of the diagnostics. The one remaining thing is that the FIFO is filled at the sampling rate, not the register update rate; Phoebe’s been running at 500Hz but her sampling is 1kHz. With Zoe, both are set to 1kHz. Next step is to try that and see what happens.
I’ll update you when there’s something worth sharing about both Phoebe and Zoes’ progress.
Zoe at 1kHz sampling, 8s flight from Andy Baker on Vimeo.
If you are interested in what triggered the transformation, have a look at these comments.
The net results are I’m getting perfect data reads at 1kHz sampling and alpf set to 2. And that’s as good as the sensor can possibly provide.
So Zoe is good enough for the Cotswold Jam on Saturday (sold out, sorry), and my employer’s Engineering Conference the following week, and hopefully the Raspberry Pi Birthday Party in early March.
There’s probably some PID tuning to be done that might stop the low-frequency wobbles due to the gyro PID I gain being a bit too enthusiastic. That would then curtail the drift too.
I’ll post more videos if that turns out to be true.
The IMU FIFO has a fixed size, so it can fill up if not read frequently; it’s then configurable whether the IMU stops adding to the FIFO or overwrite what’s there already. Either way, this results in data corruption. I’d heard, and a quick test confirmed that the FIFO size is 512 bytes. The same test also revealed it takes 0.064s to empty a full FIFO byte-by-byte or 0.000125s (1/8000)s to read a single byte or 1.5ms to read the 12 bytes that are a full set of sensor data.
For the FIFO code to work, I need to read the FIFO and update the ESCs often enough in comparison to the sensor sampling rate to ensure the FIFO never overflows. The rate for updating the ESCs is arbitrary; experience say about 100Hz is a good value.
The other factor is how long the motion processing takes; again experience with the hardware interrupt code suggests this is around 2ms.
A flight loop looks like this: sleep, empty FIFO, motion_processing.
The question is how long to sleep in order to get the 100Hz ESC update frequency and not allow the FIFO to overflow?
esc_period = 100Hz = 0.01s
read_period = 0.0015s
motion_period = 0.002s
num_samples = sampling_rate * dt
esc_period ≈ sleep_period + n_samples x read_period + motion_period
0.01 ≈ sleep_period + sampling_rate x dt x 0.0015 + 0.002
sleep_period ≈ 0.01 - 0.002 - sampling_rate x dt x 0.0015
I’ve updated the code, and then collapsed it to a simplified version which is now on GitHub. With some basic PID tuning, flights are back to the equivalent quality Phoebe could do although I need to keep testing for finer tuning and confidence building.
It’s Charles Babbage‘ 224th Birthday today, so how better to celebrate than to take his namesake, the Raspberry Pi Babbage Bear for a flight!
I’m so pleased with the QCIMUFIFO.py (Quadcopter Inertial Motion Unit First In First Out) code that I’ve decided to make it the primary development source, renaming Quadcopter.py to QCDRI.py (Quadcopter Data Ready Interrupt). They are both on GitHub along with a version of qc.py which makes it easier to select which to run.