Curiouser and curiouser

After more chipping away, I’ve got it down to this:

  • If I define _GLL_ACQ_COMMAND in class scope, the I2C command had no effect.
  • If I define _GLL_ACQ_COMMAND in instance scope, the I2C command is effective.

I use per-class scope for all my other I2C sensor classes, I wonder why it’s different here?  The code below does not work as is.  Remove the # from line 38 and all works perfectly.  Utterly bizarre.

from __future__ import division
import smbus
import time

class Lidar_Lite:

    __GLL_ACQ_COMMAND       = 0x00
    __GLL_STATUS            = 0x01
    __GLL_SIG_COUNT_VAL     = 0x02
    __GLL_ACQ_CONFIG_REG    = 0x04
    __GLL_VELOCITY          = 0x09
    __GLL_PEAK_CORR         = 0x0C
    __GLL_NOISE_PEAK        = 0x0D
    __GLL_SIGNAL_STRENGTH   = 0x0E
    __GLL_FULL_DELAY_HIGH   = 0x8F
    __GLL_FULL_DELAY_LOW    = 0x10
    __GLL_OUTER_LOOP_COUNT  = 0x11
    __GLL_REF_COUNT_VAL     = 0x12
    __GLL_LAST_DELAY_HIGH   = 0x14
    __GLL_LAST_DELAY_LOW    = 0x15
    __GLL_UNIT_ID_HIGH      = 0x16
    __GLL_UNIT_ID_LOW       = 0x17
    __GLL_I2C_ID_HIGHT      = 0x18
    __GLL_I2C_ID_LOW        = 0x19
    __GLL_I2C_SEC_ADDR      = 0x1A
    __GLL_THRESHOLD_BYPASS  = 0x1C
    __GLL_I2C_CONFIG        = 0x1E
    __GLL_ACQ_COMMAND       = 0x40
    __GLL_MEASURE_DELAY     = 0x45
    __GLL_PEAK_BCK          = 0x4C
    __GLL_CORR_DATA         = 0x52
    __GLL_CORR_DATA_SIGN    = 0x53
    __GLL_ACQ_SETTINGS      = 0x5D
    __GLL_POWER_CONTROL     = 0x65

    def __init__(self):
        self.address = 0x62
#        self.__GLL_ACQ_COMMAND = 0x00

        self.bus = smbus.SMBus(1)
        self.bus.write_byte_data(self.address, self.__GLL_OUTER_LOOP_COUNT, 0xFF)
        self.bus.write_byte_data(self.address, self.__GLL_ACQ_COMMAND, 0x04)
     
    def getDistance(self):
        dist1 = self.bus.read_byte_data(self.address, self.__GLL_FULL_DELAY_HIGH)
        dist2 = self.bus.read_byte_data(self.address, self.__GLL_FULL_DELAY_LOW)
        return ((dist1 << 8) + dist2) / 100

    def getVelocity(self):
        vel = self.bus.read_byte_data(self.address, self.__GLL_VELOCITY)
        return (self.signedInt(vel)) / 100

    def signedInt(self, value):
        return value if value < 128 else (value - 256)


ll = Lidar_Lite()

start_time = time.time()
ii = 0
while ii < 100:
    ii += 1
    velocity = ll.getVelocity()
    distance = ll.getDistance()
    print "Velocity %f; Distance %f" % (velocity, distance)
    time.sleep(0.1)
print "%d reads took %f seconds" % (ii, time.time() - start_time)

P.S. For the moment, I’m just going to accept I must use the per-instance scope rather than wasting more time working out this python oddity. This version has now incorporated into my Quadcopter code.

Spot the difference.

I’m in the process of migrating the sample code for using the LiDAR-Lite to the format I normally use.  It’s not been as simple as I thought.  I’ve narrowed the problem down to this.  Look for the SPOT THE DIFFERENCE comments.

from __future__ import division
import smbus
import time

class Lidar_Lite:

    __GLL_ACQ_COMMAND       = 0x00
    __GLL_STATUS            = 0x01
    __GLL_SIG_COUNT_VAL     = 0x02
    __GLL_ACQ_CONFIG_REG    = 0x04
    __GLL_VELOCITY          = 0x09
    __GLL_PEAK_CORR         = 0x0C
    __GLL_NOISE_PEAK        = 0x0D
    __GLL_SIGNAL_STRENGTH   = 0x0E
    __GLL_FULL_DELAY_HIGH   = 0x8F
    __GLL_FULL_DELAY_LOW    = 0x10
    __GLL_OUTER_LOOP_COUNT  = 0x11
    __GLL_REF_COUNT_VAL     = 0x12
    __GLL_LAST_DELAY_HIGH   = 0x14
    __GLL_LAST_DELAY_LOW    = 0x15
    __GLL_UNIT_ID_HIGH      = 0x16
    __GLL_UNIT_ID_LOW       = 0x17
    __GLL_I2C_ID_HIGHT      = 0x18
    __GLL_I2C_ID_LOW        = 0x19
    __GLL_I2C_SEC_ADDR      = 0x1A
    __GLL_THRESHOLD_BYPASS  = 0x1C
    __GLL_I2C_CONFIG        = 0x1E
    __GLL_ACQ_COMMAND       = 0x40
    __GLL_MEASURE_DELAY     = 0x45
    __GLL_PEAK_BCK          = 0x4C
    __GLL_CORR_DATA         = 0x52
    __GLL_CORR_DATA_SIGN    = 0x53
    __GLL_ACQ_SETTINGS      = 0x5D
    __GLL_POWER_CONTROL     = 0x65


    def __init__(self):
        self.address = 0x62
        self.distWriteReg = 0x00
        self.distWriteVal = 0x04

        try:
            self.bus = smbus.SMBus(1)
        except:
            return

        self.bus.write_byte_data(self.address, self.__GLL_OUTER_LOOP_COUNT, 0xFF)


        ############################ SPOT THE DIFFERENCE ###########################
#        self.bus.write_byte_data(self.address, self.distWriteReg, self.distWriteVal)
        self.bus.write_byte_data(self.address, self.__GLL_ACQ_COMMAND, 0x04)
        ############################ SPOT THE DIFFERENCE ###########################
     
    def getDistance(self):
        dist1 = self.bus.read_byte_data(self.address, self.__GLL_FULL_DELAY_HIGH)
        dist2 = self.bus.read_byte_data(self.address, self.__GLL_FULL_DELAY_LOW)
        return ((dist1 << 8) + dist2) / 100

    def getVelocity(self):
        vel = self.bus.read_byte_data(self.address, self.__GLL_VELOCITY)
        return (self.signedInt(vel)) / 100

    def signedInt(self, value):
        return value if value < 128 else (value - 256)


ll = Lidar_Lite()

start_time = time.time()
ii = 0
while ii < 100:
    ii += 1
    velocity = ll.getVelocity()
    distance = ll.getDistance()
    print "Velocity %f; Distance %f" % (velocity, distance)
    time.sleep(0.1)
print "%d reads took %f seconds" % (ii, time.time() - start_time)

Using the commented out line, and all works.
Using the uncommented line, and it fails.

The register sets whether data is updated automatically rather than when a command is set to get a new reading.

There’s no python error; neither raise Python syntax errors.

Anyone got any ideas as to what the problem could be?

20ms

I’ve just tested this sample I pulled from GitHub; it works.  This isn’t a good thing.  This success sucks!  The only difference between what it’s doing and what I am is all the 20ms sleeps between all reads and writes.  Together that means each read of velocity or distance takes > 60ms – hence the lack of need for sleep in the while loop at the bottom.

But at least I can use it to work out either whether it’s possible to integrate the 20ms into my quadcopter code while I read the IMU in the meantime, or better, get rid of the 20ms breaks altogether.

However, I don’t have a good feeling about achieving either of the above based on what I’ve seen so far.  Only time will tell.

import smbus
import time

class Lidar_Lite():
  def __init__(self):
    self.address = 0x62
    self.distWriteReg = 0x00
    self.distWriteVal = 0x04
    self.distReadReg1 = 0x8f
    self.distReadReg2 = 0x10
    self.velWriteReg = 0x04
    self.velWriteVal = 0x08
    self.velReadReg = 0x09

  def connect(self, bus):
    try:
      self.bus = smbus.SMBus(bus)
      time.sleep(0.5)
      return 0
    except:
      return -1

  def writeAndWait(self, register, value):
    self.bus.write_byte_data(self.address, register, value);
    time.sleep(0.02)

  def readAndWait(self, register):
    res = self.bus.read_byte_data(self.address, register)
    time.sleep(0.02)
    return res

  def getDistance(self):
    self.writeAndWait(self.distWriteReg, self.distWriteVal)
    dist1 = self.readAndWait(self.distReadReg1)
    dist2 = self.readAndWait(self.distReadReg2)
    return (dist1 << 8) + dist2 def getVelocity(self): self.writeAndWait(self.distWriteReg, self.distWriteVal) self.writeAndWait(self.velWriteReg, self.velWriteVal) vel = self.readAndWait(self.velReadReg) return self.signedInt(vel) def signedInt(self, value): if value > 127:
      return (256-value) * (-1)
    else:
      return value

ll = Lidar_Lite()

ll.connect(1)

while True:
    velocity = ll.getVelocity()
    distance = ll.getDistance()
    print "Velocity %d; Distance %d" % (velocity, distance)

Latest code on GitHub

Up to now, I’ve deferred adding code to GitHub until a particular development phase is over.  However, so much has changed in the past few months, I ought to share.  Features:

  • X8 optional configuration
  • more precise and efficient scheduling using select() allowing for extra sensors…
  • LEDDAR – fully tested
  • PX4FLOW – tested to the extent the quality of the PX4FLOW allows
  • ViDAR – testing in progress
  • Garmin LIDAR-Lite V3 – arrival imminent
  • Compass – tested but unused except for logging
  • Fusion – tested, but each sensors source requires different fusion paramters

The compass function is unused except for logging.  The ViDAR and Fusion features require at least a height sensor and further calibration.  Therefore, I strongly recommend setting

self.camera_installed = False

unless you want to see how well it isn’t working yet.

Your can enable logging for the ViDAR stats without including them in the Fusion by setting the above to True and also setting these two variables to False:

 #-------------------------------------------------------------------------------
 # Set the flags for horizontal distance and velocity fusion
 #-------------------------------------------------------------------------------
 hdf = True
 hvf = True

This code comes with absolutely no warranty whatsoever – even less than it normally does.  Caveat utilitor.

Compass and ViDAR

First, ViDAR: I’ve got fed up of calling it video macro-block something or the other, so now it’s Video Detection and Ranging.  I think it’s now working as well as it can without a height detector; it’s a long way from perfect, but until I can rely on stable height, I can’t identify further bugs.  I’m almost certainly going to wait for the Garmin LiDAR-Lite (GLL) to provide the height information.

There’s one bug I’ve fixed which explains why the PX4FLOW only uses a gyro – the angles involved are increments not absolutes.  The picture below tries to show that it’s not the absolute tilt of the quadcopter that needs to be accounted for, but the increment:

ViDAR angles

ViDAR angles

I’ve also got the compass working – I’ve just pulled out the critical bits.  Most of the samples I found use the MPU9250 master I2C to attach to the compass; compass data is then picked from MPU9250 registers.  But I found this version which exposed the compass registers for direct access which is much cleaner and clearer.

####################################################################################################
#
#  Gyroscope / Accelerometer class for reading position / movement.  Works with the Invensense IMUs:
#
#  - MPU-6050
#  - MPU-9150
#  - MPU-9250
#
#  The compass / magnetometer of the MPU-9250 is not used
#
####################################################################################################
class MPU6050:
    .
    .
    .
    .
    .
    .
    .
    def initCompass(self):    
        #-------------------------------------------------------------------------------------------
        # Set up the I2C master pass through.
        #-------------------------------------------------------------------------------------------
        int_bypass = self.i2c.readU8(self.__MPU6050_RA_INT_PIN_CFG)
        self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG , int_bypass | 0x02)

        #-------------------------------------------------------------------------------------------
        # Connect directly to the bypassed magnetometer, and configured it for 16 bit continuous data
        #-------------------------------------------------------------------------------------------
        self.i2c_compass = I2C(0x0C)
        self.i2c_compass.write8(self.__AK893_RA_CNTL1, 0x16);
   
    def readCompass(self):
        compass_bytes = self.i2c_compass.readList(self.__AK893_RA_X_LO, 7)

        #-------------------------------------------------------------------------------------------
        # Convert the array of 6 bytes to 3 shorts - 7th byte kicks off another read
        #-------------------------------------------------------------------------------------------
        compass_data = []
        for ii in range(0, 6, 2):
            lobyte = compass_bytes[ii]
            hibyte = compass_bytes[ii + 1]
            if (hibyte > 127):
                hibyte -= 256

            compass_data.append((hibyte << 8) + lobyte)

        [mgx, mgy, mgz] = compass_data
        return mgx, mgy, mgz


mpu6050 = MPU6050()
mpu6050.initCompass()

try:
    while True:
        mx, my, mz = mpu6050.readCompass()
        print "%d, %d, %d" % (mx, my, mz)
        time.sleep(0.5)


except KeyboardInterrupt, e:
    pass
finally:
    pass


I probably won’t include the compass data for yaw and orientation yet, but at least the main hurdle has been overcome.

GPS

Basic Installation

  • sudo apt-get gpsd gpsd_tools python-gps
  • Edit /etc/default/gpsd and add the GPS device into the boot configuration of gpsd
     DEVICES="/dev/ttyACM0"
  • Reboot or restart the GPS daemon gpsd.
  • Try these various tests to make sure it’s working.

Python Script

I’ve written an extension of an Adafruit python script which captures GPS data and writes it to screen and file:

from __future__ import division
import gps
import os
import math

# Listen on port 2947 (gpsd) of localhost
session = gps.gps("localhost", "2947")
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

lat1 = 0.0
lon1 = 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"

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 = longitude => p below
    # lambda = latitude => 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
                if hasattr(report, 'epx'):   # Estimated longitude error - meters 
                    epx = report.epx

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

                if hasattr(report, 'alt'):   # Altitude - meters
                    altitude = report.alt
                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
            #-----------------------------------------------------------------------------
            lat2 = latitude * math.pi / 180
            lon2 = longitude * math.pi / 180

            dx = (lat2 - lat1) * math.cos((lon1 + lon2) / 2) * R
            dy = (lon2 - lon1) * R
            lon1 = lon2
            lat1 = lat2

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




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

Here’s what I get plotting a walk around the garden carrying her – it’s not a great match.

Garden track

Garden track

0.0m is my office window.  Everything beyond is -10m, -20m is the garden;  the walk in the garden was

  • 15m SSW
  • 9m ESE
  • 18m N

That’s an approximate right angle triangle, but the scan doesn’t show that without a lot of imagination.  The scaling is right, but the direction is very noisy.  I’m not sure whether this is a code bug or just pushing the limits of GPS accuracy.  I’ll have a few more goes to see if I can get any better.  GPS samples are at roughly 1Hz.

Code update plans for Chloe

  • ultimately, that output file with be a FIFO read by Chloe’s code.  She’ll use select.select() to monitor the file contents, and sleep, whereas now she just sleeps.
  • I’ll add command line parameters –indoors or –outdoors to select the range of sensors to be used – primarily GPS; PX4FLOW / Scanse Sweep and LEDDAR will work in both environments.
  • If using the GPS, add startup code to ensure at least 4 satellites are feeding the GPS for accurate triangulation – during the walk, I had 4 indoors and 6 outdoors; obviously the more satellites, the greater the accuracy.

Crawler phase 1 complete.

Crawler is finally working:

The problem was the cwiid python library only recognises the 1st edition Wii controllers – those without Wii motionplus.  You can get these originals from ebay.  Here’s the code.

#!/usr/bin/env python

import RPi.GPIO as GPIO # Import the GPIO Library
import cwiid
import time
import os


# Set the GPIO modes
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

# Set variables for the GPIO motor pins
pinMotorAForwards = 10
pinMotorABackwards = 9
pinMotorBForwards = 8
pinMotorBBackwards = 7

# Set additional variables to support L293D bodge
pinMotorA1 = 4
pinMotorA2 = 27
pinMotorB2 = 22
pinMotorB1 = 23

# Set variables for the line detector GPIO pin
pinLineFollower = 25

# Define GPIO pins to use on the Pi
pinTrigger = 17
pinEcho = 18

# Set variable for the LED pin
pinLED1 = 5
pinLED2 = 6

# How many times to turn the pin on and off each second
Frequency = 20
# How long the pin stays on each cycle, as a percent
DutyCycleA = 100
DutyCycleB = 100
# Setting the duty cycle to 0 means the motors will not turn
Stop = 0

# Set the GPIO Pin mode to be Output
GPIO.setup(pinMotorAForwards, GPIO.OUT)
GPIO.setup(pinMotorABackwards, GPIO.OUT)
GPIO.setup(pinMotorBForwards, GPIO.OUT)
GPIO.setup(pinMotorBBackwards, GPIO.OUT)

GPIO.setup(pinMotorA1, GPIO.OUT)
GPIO.setup(pinMotorA2, GPIO.OUT)
GPIO.setup(pinMotorB1, GPIO.OUT)
GPIO.setup(pinMotorB2, GPIO.OUT)

GPIO.output(pinMotorA1, False)
GPIO.output(pinMotorA2, False)
GPIO.output(pinMotorB1, False)
GPIO.output(pinMotorB2, False)

# Set the pinLineFollower pin as an input so its value can be read
GPIO.setup(pinLineFollower, GPIO.IN)

def StopMotors():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, False)
    GPIO.output(pinLED2, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, False)

def Backwards():
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, False)
    GPIO.output(pinLED2, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def Forwards():
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB)
    GPIO.output(pinLED1, True)
    GPIO.output(pinLED2, True)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)

def Right():
    print("Right")
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB * 0.3)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, True)
    GPIO.output(pinLED2, False)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def BLeft():
    print("Right")
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, True)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def FLeft():
    print("Right")
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB)
    GPIO.output(pinLED1, True)

    GPIO.output(pinMotorA1, True)
    GPIO.output(pinMotorA2, False)
    GPIO.output(pinMotorB1, False)
    GPIO.output(pinMotorB2, True)

def Left():
    print("Left")
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA * 0.3)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB * 0.3)
    GPIO.output(pinLED1, False)
    GPIO.output(pinLED2, True)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)

def BRight():
    print("Left")
    pwmMotorAForwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorABackwards.ChangeDutyCycle(Stop)
    pwmMotorBForwards.ChangeDutyCycle(DutyCycleB * 0.3)
    pwmMotorBBackwards.ChangeDutyCycle(Stop)
    GPIO.output(pinLED1, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)

def FRight():
    print("Left")
    pwmMotorAForwards.ChangeDutyCycle(Stop)
    pwmMotorABackwards.ChangeDutyCycle(DutyCycleA)
    pwmMotorBForwards.ChangeDutyCycle(Stop)
    pwmMotorBBackwards.ChangeDutyCycle(DutyCycleB * 0.3)
    GPIO.output(pinLED1, False)

    GPIO.output(pinMotorA1, False)
    GPIO.output(pinMotorA2, True)
    GPIO.output(pinMotorB1, True)
    GPIO.output(pinMotorB2, False)


def do_WiiRemote():
    global speed, DutyCycleA, DutyCycleB

    #connecting to the wiimote. This allows several attempts
    # as first few often fail.
    print 'Press 1+2 on your Wiimote now...'
    wm = None
    while not wm:
        try:
            LED1.ChangeDutyCycle(50)
            wm=cwiid.Wiimote()

        except RuntimeError:
            continue

    LED1.ChangeDutyCycle(0)
    LED1.stop()

    #set wiimote to report button presses and accelerometer state
    wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC

    #turn on led to show connected
    wm.led = 1

    buttons = wm.state['buttons']
    speed = 100
    DutyCycleA = speed
    DutyCycleB = speed
    moving = False

    #--------------------------------------------------------------------------
    # There are official cwiid numbers for the following - I need to

    WII_BTN_2 = 1              # cwiid.BTN_2
    WII_BTN_1 = 2              # cwiid.BTN_1
    WII_BTN_B = 4              # cwiid.BTN_B
    WII_BTN_A = 8              # cwiid.BTN_A
    WII_BTN_MINUS = 16         # cwiid.BTN_MINUS
    WII_BTN_HOME = 128
    WII_BTN_LEFT = 256
    WII_BTN_RIGHT = 512
    WII_BTN_DOWN = 1024
    WII_BTN_UP = 2048
    WII_BTN_PLUS = 4096        # cwiid.BTN_PLUS
                               # cwiid.NUNCHUK_BTN_C
                               # cwiid.NUNCHUK_BTN_Z


    while not(buttons == WII_BTN_HOME + WII_BTN_A + WII_BTN_B):
        buttons = wm.state['buttons']
        print("Buttons: " + str(buttons))
        if (buttons == WII_BTN_A + WII_BTN_B + WII_BTN_2): # Halt
            wm.led = 2
            print ("Halting Raspberry Pi...")
            GPIO.cleanup()
            bashCommand = ("sudo halt")
            print "echo:"+bashCommand
            os.system(bashCommand)
        elif (buttons & cwiid.BTN_1):
            print("BTN_1")
        elif (buttons & cwiid.BTN_2):
            print("BTN_2")
        elif (buttons & cwiid.BTN_A): # Shoot!!! :o)
            print("BTN_A")
            StopMotors()
        elif (buttons & cwiid.BTN_B): # Fire Missile!!! :o)
            print("BTN_B")
        elif (buttons == WII_BTN_UP):
            print("BTN_UP")
            Forwards()
            moving = True
        elif (buttons == WII_BTN_DOWN):
            print("BTN_DOWN")
            Backwards()
            moving = True
        elif (buttons == WII_BTN_LEFT):
            print("BTN_LEFT")
            Left()
            moving = True
        elif (buttons == WII_BTN_RIGHT):
            print("BTN_RIGHT")
            Right()
            moving = True
        elif (buttons == WII_BTN_UP + WII_BTN_RIGHT):
            FRight()
            moving = True
        elif (buttons == WII_BTN_UP + WII_BTN_LEFT):
            FLeft()
            moving = True
        elif (buttons == WII_BTN_DOWN + WII_BTN_RIGHT):
            BRight()
            moving = True
        elif (buttons == WII_BTN_DOWN + WII_BTN_LEFT):
            BLeft()
            moving = True
        elif (buttons & cwiid.BTN_PLUS):
            print("BTN_PLUS")
            if speed <= 90: speed = speed + 10 else: speed = 100 print("Speed: " + str(speed)) DutyCycleA = speed DutyCycleB = speed elif (buttons & cwiid.BTN_MINUS): print("BTN_MINUS") if speed >= 10:
                speed = speed - 10
            else:
                speed = 0
            print("Speed: " + str(speed))
            DutyCycleA = speed
            DutyCycleB = speed
        elif (buttons & cwiid.NUNCHUK_BTN_C): #
            print("NUNCHUK_BTN_C")
        elif (buttons & cwiid.NUNCHUK_BTN_Z): #
            print("NUNCHUK_BTN_Z")
        else:
            print("Nothing...")
            if moving == True:
                moving = False
                StopMotors()

        time.sleep(0.2)

    GPIO.cleanup()



# Set the GPIO to software PWM at 'Frequency' Hertz
pwmMotorAForwards = GPIO.PWM(pinMotorAForwards, Frequency)
pwmMotorABackwards = GPIO.PWM(pinMotorABackwards, Frequency)
pwmMotorBForwards = GPIO.PWM(pinMotorBForwards, Frequency)
pwmMotorBBackwards = GPIO.PWM(pinMotorBBackwards, Frequency)

# Start the software PWM with a duty cycle of 0 (i.e. not moving)
pwmMotorAForwards.start(Stop)
pwmMotorABackwards.start(Stop)
pwmMotorBForwards.start(Stop)
pwmMotorBBackwards.start(Stop)

# Set pins as output and input
GPIO.setup(pinTrigger, GPIO.OUT) # Trigger
GPIO.setup(pinEcho, GPIO.IN) # Echo

# Set the LED Pin mode to be Output
GPIO.setup(pinLED1, GPIO.OUT)
GPIO.setup(pinLED2, GPIO.OUT)

LED1Frequency = 2
LED2Frequency = 2

LED1DutyCycle = 0
LED2DutyCycle = 0

LED1 = GPIO.PWM(pinLED1, LED1Frequency)
LED2 = GPIO.PWM(pinLED2, LED2Frequency)

LED1.start(LED1DutyCycle)
LED2.start(LED2DutyCycle)

# Set trigger to False (Low)
GPIO.output(pinTrigger, False)

# Allow module to settle
time.sleep(0.5)


try:
    do_WiiRemote()

# If you press CTRL+C, cleanup and stop
except KeyboardInterrupt:
    # Reset GPIO settings
    GPIO.cleanup()


So all that remained was to get it to start at boot time.  This set of instructions works perfectly for me.

See previous post for BOM etc

Next time I’m bored or frustrated, I’ll have a go at updating cwiid to recognise newer versions of the Wii controller.


Just spotted this made it onto Adafruit’s blog.

RPIO.PWM now works on Pi Zero, B2 and B3

Reik Hua sent me his copy of the mailbox code just as I’d started looking into using the mailbox for RPIO.PWM, thus saving me loads of time and brain-ache!  He uses it for his quadcopter with B2 and B3 Raspberry Pis, and I’ve just tested it on Zoe, the Pi Zero and it worked there perfectly too.  Reik Hua has kindly agreed to me passing this on to Chris Hager who owns the RPIO code on GitHub, thus making it generally available to all and allowing me to discard another personal hack from my repository.

What’s the fuss about?  With the new kernel version in May, the RPIO code stopped working as it was assuming a fixed memory address for the DMA.  The mailbox provides the actual address which had changed with the new kernels.  So not only does the RPIO now work with 0’s, 2’s and 3’s – it’s also now future proofed.  And that’s why I could show Zoe back in the air yesterday!

Buffering

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.

A box it is then!

I’ve tried various ways to acclimatise Zoe’s sensors prior to flight.  The best so far is to set the props spinning at minimum speed, and after 5 seconds, grab a FIFO full of data (42 batches of samples in the 512 byte FIFO and 12 byte batch size), and use these to calculated start-of-flight gravity.  The props then continue to run at this base speed up to the point the flight kicks off.

The net result is a stable flight with no vertical drift during hover, but with horizontal drift of about a meter.  Without this code, horizontal drift is half this but she continues to climb during hover.

I’m not sure how I can improve this, so I’ll leave it alone for now and instead have a look at making a DIY cardboard box to keep Zoe out of the wind.

In passing, I did a quick analysis of the code size: 1021 lines of python code, 756 lines of comments and 301 blank lines giving a total of 2078 lines in Quadcopter.py.  Here’s the script I knocked together quickly FYI:

code = 0
comments = 0
spaces = 0

with open("Quadcopter.py", "rb") as f:
    for line in f.readlines():
        line = line.strip()
        if len(line) == 0:
            spaces += 1
        elif line[0] == '#':
            comments += 1
        else:
            code += 1
 
    print "Code %d, Comments %d, Blank Lines %d, Total %d" % (code, comments, spaces, code + comments + spaces)

I’ve put Zoe’s code up on GitHub as the best yet, although the either / or of vertical / horizontal drift is seriously starting to pᴉss me off.

Note that since I’ve moved the IMU FIFO into the Quadcopter.py code, QCIMUFIFO.py is not longer on GitHub; Quadcopter.py is the latest best working version and QCDRI.py is the best version that uses the data ready interrupt in case you are seeing the I2C errors like I used to.