It was around Christmas 2012 that I started investigating an RPi drone, and the first post was at the end of January ’13.
5 years later, phase ‘one’ is all but done, barring all but the first as minor, mostly optional extras:
- Track down the GPS tracking instability – best guess is reduced LiPo power as the flight progress in near zero temperature conditions.
- Get Zoe working again – she’s been unused for a while – and perhaps, if possible, add GPS support although this may not be possible because she’s just a single CPU Pi0W
- Fuse the magnometer / gyrometer 3D readings to long term angle stability, particular yaw which has no backup long term sensor beyond the gyro.
- Add a level of yaw control such that ‘H’ always points the way she’s flying – currently she always points in the same direction she took off at. I’ve tried this several times, and it’s always had a problem I couldn’t solve. Third time lucky.
- Upgrade the operating systems to Raspbian Stretch with corresponding requirements for the I2C fix and network WAP / udhcpd / dnsmasq which currently means the OS is stuck with Jessie from the end of February 2017.
- Upgrade camera + lidar 10Hz sampling versus camera 320² pixels versus IMU 500Hz sampling to 20Hz, 480² pixels, 1kHz respectively. However, every previous attempt to update one leads to the scheduling no longer able to process the others – I suspect I’ll need to wait for the Raspberry Pi B 4 or 5 for the increased performance.
Looking into the future…
- Implement (ironically named) SLAM object mapping and avoidance with Sweep, ultimately aimed at maze nativation – just two problems here: no mazes wide enough for ‘H’ clearance, and the AI required to remember and react to explore only unexplored areas in the search for the center.
- Fuse GPS latitude, longitude and altitude / down-facing LiDAR + video / ΣΣ acceleration δt δt fusion for vertical + horizontal distance – this requires further connections between the various processes such that GPS talks to motion process which does the fusion. It enables higher altitude flights where the LiDAR / Video can’t ‘see’ the ground – there are subtleties here swapping between GPS and Video / LiDAR depending whose working best at a given height above the ground based on an some fuzzy logic.
- Use down-facing camera for height and yaw as well as lateral motion – this is more a proof of concept, restrained by the need for much higher resolution videos which current aren’t possible with the RPi B3.
- Find a cold-fusion nuclear battery bank for flight from the Cotswolds, UK to Paris, France landing in Madrid, Spain or Barcelona, Catalonia!
These future aspirations are dreams unlike to become reality either to power supply, CPU performance or WiFi reach. Although a solution to the WiFi range may be solvable now, the other need future technology, at least one of which my not be available within my lifetime :-).
Wishing you all a happy New Year and a great 2018!
Currently, all speeds, both horizontally and vertically are set to 0.3m/s for the sake of safety in enclosed arenas like indoors and the walled back garden. The down side is that in the park with the GPS waypoints perhaps 20m apart, it takes a very long time, often over a minute between waypoints, wearing out the batteries in a few flights.
The limitation other than safety is to ensure the down-facing video can track the difference between frames, which means there needs to be a significant overlap between consecutive frames.
The video runs at 10Hz*. The RPi camera angle of view (AOV) is 48.8°. With the camera 1m off the ground (the standard set throughout all flights)**, 48.8° corresponds to 80cm horizontal distance (2 x 1m * tan (AOV / 2)). Assuming there needs to be a 90% overlap between frames to get accurate video macro-block vectors, every 0.1s, Hermione can move up to 8cm (10%) or 0.80m/s compared to the current 0.3m/s. I’ll be trying this out on the GPS tracking flights in the park tomorrow.
*10Hz seems to be about the highest frequency for the video that the macro-block processing can handle without causing other sensor processing to overflow – specifically the IMU FIFO.
**1 meter height is for the sake of safety, and because the video 320² pixels macro-blocks can resolve distance accurately on grass and gravel. Doubling the height requires quadrupling the video frame size to 640² to get the same resolution required for grass / gravel, and once again, the processing time required will cause IMU FIFO overflowing.
P.S. The weather isn’t as good as I’d hoped to do the GPS tracking flights in the park yet, but I did take Hermione into the back garden this morning to test her increased horizontal velocity changes; she happily ran at 1m/s over the grass, so that will be the new speed used for the much longer distance GPS flights to reduce Hermione’s flight time and hence her and the DJI Mavic’s battery drain.
Distance + Direction = GPS
By human measurements, the distance was about 7m at about 45° (i.e NE). GPS says 8.6m, video camera tracking says 5 which is the flight plan defined length to travel.
It was never going to be perfect due to the difference between magnetic and true north, the resolution of GPS of around 1m, and how video distance tracking will always be a best guess, but it’s more than good enough for my cunning plan to work.
However, the plan’s taking a premature diversion; during this test, I was less careful and she ended up (in vertical descent mode) clipping 5 props against the drive stone wall. Next step (after replacing the props!) is now to deploy my Scanse Sweep code which will trigger an orderly landing if any object is detected less than 1.5m away – Hermione’s radius is 50cm prop tip to tip diagonally so that’s 1m clearance.
One interesting point: the compass readings are mostly in a very dense cluster, with just a few (relatively) pointing in very different directions – that’s as Hermione passed the family car!
By lowering the video frame rate to 10Hz, I’ve been able to increase the video resolution to 720² pixels. In addition I’ve increased the contrast on the video to 100%. Together these now provide enough detail to track lateral motion on the lawn. Drift with hover is non-existent, so next step was to try a flight around a 2m square. That’s where the disagreement showed itself:
Difference of opinion
- Top left is the flight plan up to the point I killed the flight: 2 meters forwards and left by 0.35m
- Top right shows the 90° anticlockwise yaw so she points the way she’s going
- Bottom left is the track picked up by the PiCamera macro-blocks
- Bottom right is the track derived by double integrating the accelerometer.
Both agree on the forward motion of about 2 meters, but the disagreement arises at the point she turns left. The right of the pair is correct based on my independent third-party view of the flight; although she was pointing left, she flew right from my point of view i.e. backwards from her point of view. I’ve clearly got the maths back-to-front in the lateral motion tracking. I’m pretty sure of the offending line of code, and the fix is trivial, but I’m really struggling to convince myself why what’s there is wrong.
Luckily, during the flights, there were a number of high-torque landings which ultimately broke the bracket for one of Hermione’s legs. Until the replacement arrives from Poland, I have plenty of time to kill convincing myself why the existing code is wrong.
are lacking yet this spring, and having mown the lawn yesterday, features are hard to find for the video lateral tracking. So I think this is a pretty good 37s hover. In fact, I think it’s as good as it can be until the daisies start sprouting:
This is with a frame size of 640² pixels. There’s an check in the code which reports whether the code keeps up with the video frame rate. At 640² it does; I tried 800² and 720² but the code failed to keep up with the video frame rate of 20fps.
As a result, I’ve uploaded the changes to GitHub. There’s work-in-progress code there for calibrating the compass “calibrateCompass()”, although that’s turning out to be a right PITA. I’ll explain more another time.
As a side note, my Mavic uses two forward facing camera to stereoscopically track horizontal movement, combined with GPS and a corresponding ground facing pair of cameras and the IMU accelerometer integration, yet if you watch the frisbee / baseball bat to the left, even the Mavic drifts.
If you look at yesterday’s video full screen, from top left to right, you can see a muddy patch and two chicken poos, the second poo of which is close to Hermione’s front left prop on take-off. I was back out in the dark last night, tracking them down. Here’s why:
Although the graph of camera lateral tracking and the Mavic video are almost facsimiles in direction, the scale is out; the graph shows the distance from take-off to landing to be about 1.7m whereas a tape measure from chicken poo #2 to the cotoneaster shrubbery landing point measures about 4.2m. Given how accurate the direction is, I don’t think there’s any improvement needed for the macro-block processing – simply a scale factor change of ≈ 2.5. I wish I knew more about the video compression method for generating macro-blocks to understand what this 2.5 represents – I don’t like the idea of adding an arbitrary scale of 2.5.
One further point from yesterday’s video, you can see she yaws clockwise by a few degrees on takeoff – Hermione’s always done this, and I think the problem is with her yaw rate PID needing more P and less I gain. Something else for me to try next.
I had tried Zoe first as she’s more indestructible. However, her Pi0W can only cope with 400 x 400 pixels video, whereas Hermione’s Pi B 2+ can cope with 680 x 680 pixel videos (and perhaps higher with the 5 phase motion processing) which seem to work well with the chicken trashed lawn.
not perfect, but dramatically better. The flight plan was:
- take off to 1m
- move left over 6s at 0.25m/s while simultaneously rotating ACW 90° to point in the direction of travel
I’d spent yesterday’s wet weather rewriting the macro-block processing code, breaking it up into 5 phases:
- Upload the macro block vectors into a list
- For each macro-block vector in the list, undo yaw that had happened between this frame and the previous one
- Fill up a dictionary indexed with the un-yawed macro-block vectors
- Scan the directory, identifying clusters of vectors and assigned scores, building a list of highest scoring vector clusters
- Average the few, highest scoring clusters, redo the yaw of the result from step 2, and return the resultant vector
Although this is quite a lot more processing, splitting it into five phases compared to yesterday’s code’s two means that between each phase, the IMU FIFO can be checked, and processed if it’s filling up thus avoiding a FIFO overflow.
Two remaining more subtle problems remain:
- She should have stayed in frame
- She didn’t quite rotate the 90° to head left.
Nonetheless, I once more have a beaming success smile.
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.
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.
OK so here’s a graph of macro-block vectors, where red dots are those whose SAD is higher than the average and green is equal or lower than average. The colour of each dot is more intense for the number of macro-blocks with that vector value..
Here’s a couple separating the greens (currently used) from the reds (currently discarded).
It’s pretty clear that both low and high SAD vectors have a common concentration around “the right value” i.e. SAD’s irrelevant for finding the most concentrated area of vectors. If there’s a way to find that area in code, throw away all vectors outside that area’s boundary and then average out those that are left, the result should be much more accurate.
Adam Heinrich from the Czeck Republic has pointed me in the direction of the RANSAC (RANdom SAmple Consensus) algorithm; a simple, iterative way to identify the collection of macro-block vectors that are self-consistent.
I’ll leave it to Wikipedia to explain how it works.
The reason it’s so good in my context is that it can broken up into batches which can be processed when the code isn’t busy doing something else: i.e. when the FIFO is less than half full and we’re waiting for the next set of blocks from the video.
Once Christmas and New Year chaos are out of the way, I hope to dedicate more of my brain to how this works, and implementing it.
Thanks, Adam, for pointing me in the right direction.