Cloudy conditions

The sun wasn’t shining brightly, so no high contrast on the lawn; the lawn had been mowed, removing the contrasting grass clumps too. Yet she still did a great attempt at a 1m square. I think this is about as good as she can get – it’s time for me to move on to adding compass, GPS and object avoidance.  The code as been updated on GitHub.

GPS, Compass and Object Avoidance

It’s time to down tools on

  • Ö – she just can’t run fast enough to test on grass
  • yaw control – I just can’t work out why something goes very wrong three seconds after the yaw control kicks in

and move on to the next steps.

Compass

The compass data is already available, but needs calibrating to allow for magnetic materials in the area; I’ve even got the calibration code written, but it fails; it uses the gyro to track that the compass has turned > 360° to find the maximum and minimum readings, and hence the offset due to the local readings.  The angles from the gyro were utter rubbish, and I have no idea why – I just need to try harder here.

GPS

I’ve put together a hand-held GPS tracker, and took it for a walk outside my house*.  I also took a photo of our house with my Mavic and overlaid the results (it’s worth clicking on the image to see it full size):

GPS tracking

GPS tracking

It’s not perfect, but the shape tracked by the GPS is reasonable enough once the GPS has settled; note the both green splodges are at the same point, but only the end one is in the right position due the first few samples from the GPS – I’ll have to remember this when incorporating this with Hermione’s flight plan.  Ignoring the initial few samples, I think mostly the errors were less than a meter once away from the house.  The GPS code for Hermione will be closely based on what I used for my handheld version:

from __future__ import division
import gps
import os
import math

###################################################################################################
# Set up the tty to used in /etc/default/gpsd - if the GPS is via USB, then this is /dev/ttyUSB0. #
# The python script listens on port 2947 (gpsd) of localhost.  This can be reconfigured in        # 
# /etc/default/gpsd also.                                                                         #
###################################################################################################

session = gps.gps()
session.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)

num_sats = 0
latitude = 0.0
longitude = 0.0
time = ""
epx = 0.0
epy = 0.0
epv = 0.0
ept = 0.0
eps = 0.0
climb = 0.0
altitude = 0.0
speed = 0.0
direction = 0.0

lat = 0.0
lon = 0.0
alt = 0.0
new_lat = False
new_lon = False
new_alt = False

base_station_set = False

dx = 0.0
dy = 0.0
dz = 0.0

R = 6371000 # radius of the earth in meters

fp_name = "gpstrack.csv"
header = "time, latitude, longitude, satellites, climb, altitude, speed, direction, dx, dy, dz, epx, epy"

os.system("clear")

print header

with open(fp_name, "wb") as fp:
    fp.write(header + "\n")

    #---------------------------------------------------------------------------------
    # With a based level longitude and latitude in degrees, we can be the current X and Y coordinates
    # relative to the takeoff position thus:
    # psi = latitude => p below
    # lambda = longitude => l below
    # Using equirectangular approximation:
    #
    # x = (l2 - l1) * cos ((p1 + p2) / 2)
    # y = (p2 - p1)
    # d = R * (x*x + y*y) ^ 0.5
    #
    # More at http://www.movable-type.co.uk/scripts/latlong.html
    #---------------------------------------------------------------------------------

    while True:
        try:
            report = session.next()
#            print report
#            os.system("clear")
            if report['class'] == 'TPV':
                if hasattr(report, 'time'):  # Time
                    time = report.time

                if hasattr(report, 'ept'):   # Estimated timestamp error - seconds
                    ept = report.ept

                if hasattr(report, 'lon'):   # Longitude in degrees
                    longitude = report.lon
                    new_lon = True

                if hasattr(report, 'epx'):   # Estimated longitude error - meters
                    epx = report.epx

                if hasattr(report, 'lat'):   # Latitude in degrees
                    latitude = report.lat
                    new_lat = True

                if hasattr(report, 'epy'):   # Estimated latitude error - meters
                    epy = report.epy

                if hasattr(report, 'alt'):   # Altitude - meters
                    altitude = report.alt
                    new_alt = True

                if hasattr(report, 'epv'):   # Estimated altitude error - meters
                    epv = report.epv

                if hasattr(report, 'track'): # Direction - degrees from true north
                    direction = report.track

                if hasattr(report, 'epd'):   # Estimated direction error - degrees
                    epd = report.epd

                if hasattr(report, 'climb'): # Climb velocity - meters per second
                    climb = report.climb

                if hasattr(report, 'epc'):   # Estimated climb error - meters per seconds
                    epc = report.epc

                if hasattr(report, 'speed'): # Speed over ground - meters per second
                    speed = report.speed

                if hasattr(report, 'eps'):   # Estimated speed error - meters per second
                    eps = report.eps


            if report['class'] == 'SKY':
                if hasattr(report, 'satellites'):
                    num_sats = 0
                    for satellite in report.satellites:
                        if hasattr(satellite, 'used') and satellite.used:
                            num_sats += 1

            #-----------------------------------------------------------------------------
            # Calculate the X,Y coordinates in meters
            #-----------------------------------------------------------------------------
            if new_lon and new_lat and new_alt and num_sats > 6:

                new_lon = False
                new_lat = False
                new_alt = False

                lat = latitude * math.pi / 180
                lon = longitude * math.pi / 180
                alt = altitude


                if not base_station_set:
                    base_station_set = True

                    base_lat = lat
                    base_lon = lon
                    base_alt = alt

                dx = (lon - base_lon) * math.cos((lat + base_lat) / 2) * R
                dy = (lat - base_lat) * R
                dz = (alt - base_alt)

            else:
                continue


            output = "%s, %f, %f, %d, %f, %f, %f, %f, %f, %f, %f, %f, %f" % (time,
                                                                             latitude,
                                                                             longitude,
                                                                             num_sats,
                                                                             climb,
                                                                             altitude,
                                                                             speed,
                                                                             direction,
                                                                             dx,
                                                                             dy,
                                                                             dz,
                                                                             epx,
                                                                             epy)




            print output
            fp.write(output + "\n")
        except KeyError:
            pass
        except KeyboardInterrupt:
            break
        except StopIteration:
            session = None
            print "GPSD has terminated"
            break

The main difference will be that while this code writes to file, the final version will write to a shared memory pipe / FIFO much like the camera video macro-blocks are now.  The GPS will run in a separate process, posting new results as ASCII lines into the FIFO; Hermione’s picks up these new results with the select() she already uses.  The advantage of the 2 processes is both that they can be run on difference cores of Hermione’s CPU, and that the 9600 baudrate GPS UART data rate won’t affect the running speed of the main motion processing to get the data from the pipe.

Lateral object avoidance

My Scanse Sweep is very imminently arriving, and based on her specs, I plan to attach her to Hermione’s underside – she’ll have 4 blind spots due to her legs, but otherwise a clear view to detect objects up to 40m away.  Her data comes in over a UART like the GPS, and like the GPS, the data is ASCII text.  That makes it easy to parse.  The LOA does churn out data at 115,200 bps, so it too will be in a separate process.  Only proximity alerts will be passed to Hermione on yet another pipe, again listened to on Hermione’s select(); the LOA code will just log the rest providing a scan of the boundaries where it is.

Still stuck

Hermione is still causing trouble with yaw control flights despite lots of refinements.  Here’s the latest.

Hermione's troubles

Hermione’s troubles

@3s she’s climbed to about a meter high and then hovered for a second.  All the X, Y, and Z flight plan targets and sensor inputs are nicely aligned.

The ‘fun’ starts at 4 seconds.  The flight plan, written from my point of view says move left by 1m over 4 seconds.  From Hermione’s point of view, with the yaw code in use, this translates to rotate anti-clockwise by 90° while moving forwards by 1m over 4 seconds.  The yaw graph from the sensors shows the ACW rotation is happening correctly.  The amber line in the Y graph shows the left / right distance target from H’s POV is correctly zero.  Similarly, the amber line in the X graph correctly shows she should move forwards by 1m over 4s.  All’s good as far as far as the targets are concerned from her and my POV.

But there’s some severe discrepancy from the sensors inputs POV.  From my POV, she rotated ACW 90° as expected, but then she moved forwards away from me, instead of left.  The blue line on the Y graph (the LiDAR and ground-facing video inputs) confirms this; it shows she moves right by about 0.8m from her POV.  But the rusty terracotta line in the Y graph (the double integrated accelerometer – gravity readings) shows exactly the opposite.  The grey fusion of the amber and terracotta cancel each other out thus following the target perfectly but for completely the wrong reasons.

There are similar discrepancies in the X graph, where the LiDAR + Video blue line is the best match to what I saw: virtually no forward movement from H’s POV except for some slight forward movement after 8s when she should be hovering.

So the net of this?  The LiDAR / Video processing is working perfectly.  The double integrated IMU accelerometer results are wrong, and I need to work out why?  The results shown are taken directly from the accelerometer, and double integrated in excel (much like what the code does too), and I’m pretty convinced I’ve got this right.  Yet more digging to be done.

In other news…

  • Ö has ground facing lights much like Zoe had.  Currently they are always on, but ultimately I intend to use them in various ways such as flashing during calibration etc – this requires a new PCB however to plug a MOSFET gate into a GPIO pin.
  • piNet has changed direction somewhat: I’m testing within the bounds of my garden whether I can define a target destination with GPS, and have enough accuracy for the subsequent flight from elsewhere to get to that target accurately.  This is step one in taking the GPS coordinates of the centre of a maze, and then starting a flight from the edge to get back there.

That’s all for now, folks.  Thanks for sticking with me during these quiet times.


P.S. I’ve got better things to do that worry about why everything goes astray @ 7s, 3s after the yaw to move left started; it’s officially on hold as I’ve other stuff lurking in the background that’s about the flower.

Ö

I chose to name my piDrones Phoebe, Chloe and Zoe as they can all be spelt with an umlaut – don’t ask me why, I have no idea.  I ran out of umlaut names (despite research) so I opted for Hermione as the latest, greatest model as it sounds similar although she can’t ever bear an umlaut as she lacks the critical ‘oe’.

Anyway, Phoebe, Zoe and the always short-lived* Chloe have all been merged into the best of each; the result is ‘Ö’ pronounced like the french for ‘yes’.  She has Phoebe’s ESCs, motors and props, Chloe’s amazing frame, and Zoe’s Pi0W and PCB.

Ö’s build is virtually indestructible as she weighs just 1kg fully loaded.  Because she’s so light, crash torques are tiny compared to the strength of the frame; the only perceivable damage is to broken props and these are cheap from e-bay and I already have a vast stock of them.  In comparison, Hermione weighs 4kg; this, and the fact she’s so large means crash torque is huge in comparison, damage always occurs for anything but a perfect landing, and replacement frame parts and props is expensive.  Ultimately I still want to have Hermione as queen piDrone because of her X8 format, and use of a B3 4-cores allowing further sensors**, but while I’m still diagnosing the current problems, I think little miss indestructible is better suited financially to the task-in-hand.

Sadly, there’s one problem; Ö’s Pi0W isn’t fast enough to cope with video resolution higher than about 400² pixels, ruling out lawn / gravel etc.  This is what she can do successfully:

On the plus side, I think that’s just enough to sort out my understanding of Hermione’s yaw flaws.


*Chloe got retired (again) as the 1.5A regulator on the PCB was insufficient to drive the A+, IMU, Camera and LiDAR-Lite. The same I2C errors I have before returned. Swapping Chloe’s A+ to Ö’s Pi0W resolved this.

** i.e. GPS, compass and Scanse Sweep

Hermione draws a square, kinda.

She would have drawn a better square had I got the flight plan right; it the event, the plan said to…

  • climb to 90cm over 3s
  • hover for a second
  • move forward by 1m over 4s
  • hover for a second
  • move left by 1m over 4s
  • hover for a second
  • move back by 2m over 8s
  • hover for a second
  • move right by 2m over 8s
  • hover for a second
  • land over 4s…

making a total of 36 seconds in all.

These last two sections meant she should land about a meter back and right from where she took off.  How well did she follow the flawed flight plan?

To me, this is working amazingly well, especially as the camera lateral tracking doesn’t have any significant markers, just grass blades.  I was lucky there was bright sunshine.

What I’d really like to have shown was her actually ‘turning’ each corner, always facing the direction she’s flying; this is completely unnecessary but would look good – the only point of doing it is if there’s a camera taking pics and streaming video live back to the RC as per my Mavic.  But currently my yaw code is still lacking something and I don’t know what yet.

And in other news…

the hedgehogs are back in our garden:

Welcome back!

Welcome back!

This is a combination of the PiNoIR camera (v.1) along with this IR motion detector (the sensor is expensive because it’s digital), and some near-IR LEDs.  Below is a picture of the older model – the newer one runs on an A+ (for size and power reasons) and has been updated to Jessie, and now the LEDs are switchable by GPIO via a mosfet to provide lighting for the camera only when needed.  The upgrade to Jessie means she boots without WiFi at night, and the next morning, I can add the WiFi dongle to connect and check the results.

HogCam

HogCam

Code

#!/usr/bin/env python
# NiteLite - a python daemon process started at system boot, and stopped on shutdown
#          - the default LED pattern is twinkling but if motion is detected, one of 4
#            different patterns are chosen and these are used for 10s after motion detection
#
# Please see our GitHub repository for more information: https://github.com/pistuffing/nitelite/piglow
#

import signal
import time
import RPi.GPIO as GPIO
import os
from datetime import datetime
import subprocess

#------------------------------------------------------------
# Set up the PIR movement detection
#------------------------------------------------------------
GPIO_PIR = 18
GPIO_IR_LED = 12
GPIO.setmode(GPIO.BOARD)
GPIO.setup(GPIO_PIR, GPIO.IN, GPIO.PUD_DOWN)
GPIO.setup(GPIO_IR_LED, GPIO.OUT)
GPIO.output(GPIO_IR_LED, GPIO.LOW)

#------------------------------------------------------------
# Final steps of setup
#------------------------------------------------------------
keep_looping = True

def Daemonize():
	os.setpgrp()

#------------------------------------------------------------
# Once booted, give the user a couple of minutes to place the camera
#------------------------------------------------------------
time.sleep(2 * 60.0)

try:
    while keep_looping:
        #----------------------------------------------------
        # Block waiting for motion detection
        #----------------------------------------------------
        GPIO.wait_for_edge(GPIO_PIR, GPIO.RISING)

        #----------------------------------------------------
        # Turn on the IR LED
        #----------------------------------------------------
        GPIO.output(GPIO_IR_LED, GPIO.HIGH)

        #----------------------------------------------------
        # Take a snap
        #----------------------------------------------------
	now = datetime.now()
	now_string = now.strftime("%y%m%d-%H%M%S")
	camera = subprocess.Popen(["raspistill", "-rot", "180", "-o", "/home/pi/Pictures/img_" + now_string + ".jpg", "-n", "-ISO", "800", "-ex", "night", "-ifx", "none"], preexec_fn =  Daemonize)

        #----------------------------------------------------
        # Turn off the IR LED after 5s
        #----------------------------------------------------
        time.sleep(5)
        GPIO.output(GPIO_IR_LED, GPIO.LOW)

        #----------------------------------------------------
        # Wait 30s before checking for motion again
        #----------------------------------------------------
        time.sleep(30.0)
except KeyboardInterrupt, e:
        pass

GPIO.cleanup()

Save it as /home/pi/hogcam.py and make it executable:

chmod 775 /home/pi/hogcam.py

Load on boot

Raspian uses systemd for running on boot.  In /etc/systemd/system, create a new file called hogcam.service:

[Unit]
Description=HogCam

[Service]
ExecStart=/home/pi/hogcam.py

[Install]
WantedBy=multi-user.target

Save it off.  You can then enable, start and stop it thus.  It will also start on boot.

sudo systemctl enable hogcam.service
sudo systemctl start hogcam.service
sudo systemctl stop hogcam.service

Lateral motion tracking with yaw

I’m doing some very careful testing before I set Hermione loose live to fly in a circle.  This morning, I’ve confirmed the video lateral motion block tracking is working well.

For this first unpowered flight, I walked her forwards about 3m and then left by about the same.  Note that she always pointed in the same direction; I walked sideways to get the left movement:

Forward - Left

Forward – Left

For this second unpowered flight, again, I walked her forwards about 3m, but then rotated her by 90° CCW before walking another.  Because of the yaw, from her point of view, she only flew forwards, and the yaw is not exposed on the graph.  This is exactly how it should be:

Forward - Yaw 90° CCW - Forward

Forward – Yaw 90° CCW – Forward

So I’m happy the lateral motion tracking is working perfectly.  Next I need to look at the target.  I can go that with the same stats.

The only problem I had was that the sun needs to be shining bright for the video tracking to ‘fly’ above the lawn; clearly it needs the high contrast in the grass when sunlit.

POV

The problem: the camera point of view is in the quad frame; the garmin point of view is in the earth frame.  They need to both be in the same frame to produce a vector that’s meaningful.  A pretty radical rewrite of this area last night resulted.  A test flight this morning sadly was pretty much the same as yesterday: a very stable hover, but shooting off right when she should have gone left.  More stats:

POV

POV

The top pair of accelerometer vs camera show pretty good alignment, right up to the point of 0.4m to the right.  I believe this is correct, but I wouldn’t put money on it yet!

The middle pair are accelerometer vs LiDAR height over time, which is excellent.

The bottom pair are the flight plans in earth and quad frames (the quad one is simply the earth one rotated from my to her POV) – this is where there’s clearly a problem – they should be the same but they are wrong once the flight rotates.  I can’t see an obvious bug in the code, which makes me suspect there’s an obvious bug in my understanding instead.

A difference of opinion.

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

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.