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?

Phoebe, PyPy and Performance

I’ve just managed to get my Quadcopter code to run under PyPy rather than CPython – that means the code is compiled in advance for each run (Just In Time or JIT) rather than interpreted line by line.  Sadly, this took the performance down to 58% rather than the 95% I’d achieved with CPython 🙁

However, the PyPy code in the standard Raspian distribution is very out of date (version 2.2.1 compared to the current 2.6) so there’s more investigation to be done.

In passing, I also updated the Raspian distribution (sudo apt-get dist-upgrade) installed on Phoebe, and amazingly, that has taken me to about 98%!

Time to go see if that’s made a real difference…

CPython, Cython or PyPy?

I’m pretty sure that my interpreted CPython code is as efficient as possible, so if I want to capture all samples (for accurate integration) rather than the 95% I currently get, I need to make the motion processing code take less than 1ms and the most obvious currently-available solution (until the RPi A2 is launched) is to move from the interpreted CPython to the compiled Cython or PyPy.

I considered this a long time ago when trying to speed up the code, but in the end, I didn’t need to make the move as various tweaks to the code* improved the performance by a factor of 5 or 6.

But now is the time to make that leap of faith.  I’ll update you on progress.


*Primary performance enhancements:

  • customized GPIO library to optimize rising edge performance for the data ready hardware interrupt
  • run sampling at 1kHz but only run motion processing for each batch of 10 (averaged) samples.
  • minimized calls to time.time() to just time stamping each batch of 10 samples – another irony that calling time.time() is the most time consuming call in this code.

 

Phoebe’s full test code

This is the code used for testing. I’ve removed any code that relates to ongoing development / research / testing; this contains only the code used for Phoebe’s flights up to now.

Be very careful with this code; it is tuned exactly to Phoebe; any difference in motors, blades, weight etc. will affect the correct PID gains you should use; however, these should at least provide an order of magnitude starting point for your own tuning / testing; this should allow you to reduce the number of catastrophic crashes I had along the way!

For a ‘test’, I run the code twice – this may not be necessary, but stems from a lack of trust in sensor initialization / calibration during the first stages of testing months ago. The first run simply calibrates the sensors and saves the output. The second does the same again, but then sets the blade spin rate to 59% of the full speed – this is a pure judgement value which leads to the very gentle take-off shown in the video; increments of 0.5% have significant effect so be careful.


sudo python ./drone.py -c
sudo python ./drone.py -c -t 590 -v

Just running drone.py will give you the comment line parameters; use of any other than those shown above comes with absolutely no guarantee of safety – on your own head be it (potentially literally)!

Each test is 6s long – 2s take-off, 2s hover, 2s landing; you can see this at the top of the “while keep_looping:” code. Take-off spin rate is defined by the command line; hover and landing rates are arbitrary choices hard-coded, and less than the command line value. These still need fine tuning to avoid the descending hover and bouncy landings shown in the videos, so stay well clear of your drone when testing!

Finally, and once more, have fun, but don’t blame me if it doesn’t work for you; officially you’re on your own, but I’ll try to help if I can, but there are no guarantees I’ll be able to.

Performance enhancements

So I’m going to ignore the take-off “Elephant in the room / Skeleton in the cupboard” for the moment before I go mad, and instead, start the drone at roughly hover speed, hold it in the air, let go, and tune the PIDs for a sustained hover, plus gentle landing.  I’ll let you know how it goes in a separate post in the future.

In the meantime, I’ve been looking at performance improvements, not to the extent of exchanging interpreter CPython with JIT compiled PyPy or pre-compiled Cython, but just to fine tune the code.

As of yesterday 1 loop of the code read sensors, ran them through PIDs, and sent results to motors taking about 0.02s or 50 loops a second.  That’s actually fast enough, but quicker looping allows for extra code to be added while maintaining this 50 loops a second or faster reaction to problems.

My main target is reading the data from the MPU6050; part one was to increase it’s sampling rate from 100Hz to 200Hz, and fixing a minor bug (not sure if this contributed to the performance or not but hey). As a result, it’s gone up to 65 loops per second.  Great.

But I’d chosen 100Hz sample rate deliberately to ensure that MPU6050 output stayed stable long enough for the Python code to read a complete consistent set of sensor results within the 100Hz / 0.01s timeframe of the MPU6050 sensor sample frequency.  A “sample” comprises reading 12 registers – high and low bytes for each for the accelerometer’s and gyro’s x, y, and z axes.  That’s 12 calls that Python interprets, passes to the smbus driver, and then interprets the result from the smbus driver back to the Python code.  This just felt like an obvious place to try to optimize.  If those 12 Python calls to read a register each could instead be changed to 1 Python call for the smbus driver to read the 12 registers, there should be a significant improvement by reducing the number of interpreted Python calls from 12 to 1!

And there was – based upon the 65 loops per second code from the 200Hz sampling rate increase, I added i2c python support for read_i2c_data_block() – this gets the smbus device to read a series of sequential registers – in this case from 0x3B to 0x48, and only then return the results to the python code in an array.  And it worked.  Reading those 14 registers as one Python call took the loop count up to 87 loops per second or 0.0114ms per loop – compared the the starting point of 0.2ms per loop, that’s a pretty amazing 75% speed increase opening a decent hole for extra code, or better responses or both!

Here’s the updated code for the i2c and MPU6050:

P.S. After some checking of the stats from the last run, I think I’ll drop back to the 100Hz sampling rate for the MPU6050. At 200Hz, there were a couple of duff readings suggesting the reading of the 14 registers could take marginally longer than the 5ms allowed by the 200Hz cycle. On the plus side, these duff values were swallowed up in the faster reaction meaning they had no real net-effect.

What’s troubling me with the RPi.GPIO python library?

AlarmPi needs to monitor 2 inputs currently: the switch and the PIR.  Once the AlarmHub is finished, it needs to track a TCP socket as well so that when one Alarm goes off, the hub can tell all the others to go off too.

There are several ways for the RPi.GPIO library to monitor and report inputs:

  • RPi.GPIO.input(channel) but you’d need to poll frequently and would probably miss a change
  • RPi.GPIO.wait_for_edge() blocks, so you wouldn’t miss the event, but it can only track one channel; the Alarm + Hub needs at least 3
  • RPi.GPIO.edge_detected() doesn’t block, which means that although it still only covers 1 channel again, each could be checked in turn; in addition, the input is latched until read meaning it can’t be missed.  The downside is that you’d need to keep checking say once a second to detect switch and PIR changes
  • RPi.GPIO.add_event_detect() allows a callback to be made when an edge is detected; unfortunately this happens on a separate thread, and does not wake a sleeping main thread.  The only way to work around this is for the callback thread to send os.signal(signal.SIGINT) to wake the sleeping main thread via a signal handler, but that then makes it harder to use ctrl-C to stop the code.

AlarmPi currently uses this last option as the only one that can be made to work efficiently, but the code shows the messy interactions between callbacks, the main thread. and the signal handler. It’s also forced to have a super extended debounce selected (30s) on the switch callback; once the switch is turned on, it needs to beep / light the LED for 30s to allow the user to leave the room before the PIR is enabled. Because the switch callback doesn’t wake the main thread, this 30s processing takes place in the callback itself. To allow this to work, the callback bounce delay must be longer than 30s. If it isn’t, then when the alarm is turned on, any bounce in the switch is queued until the 30s callback has finished, and then it is processed, immediately toggling the switch off again disabling the PIR as though the user had turned it off. With this hacky debounce delay of 30s, this actually means once the hub exists that if you accidentally turn on the alarm, you can’t turn it off until the PIR is active, at which point attempting to turn off the alarm will trigger the PIR, and most likely deploy the CS gas, ring the police etc. Yet without the hacky fix, any switch bounce (likely) will automatically turn the alarm off immediately every time you try to turn it on; catch 22.

When the Hub comes along, the situation gets worse as the main thread would need to sleep until an intruder is detected, and then use a sockets.select() call for receiving data, meaning yet more messy interactions with callbacks and signal handlers.

So I’m looking at modifying the RPi.GPIO library.  Here’s my current plan:

  • Make the RPi.GPIO library more object oriented:
  • GPIO.setup() returns a gpio object representing a single channel / GPIO pin.  Errors are reported via Try: Except handlers
  • Currently, in the C library, each GPIO pin is accessed via a file descriptor (fd) passed to epoll() for input and write() for output – these would now be stored inside the GPIO class object, one per channel
  • A new python class function gpio.socket() returns the fd in a socket object.
  • In turn, this fd can be used by socket.select() (I hope!) just as other TCP sockets are used; the advantage here is that select can watch many fds at a time, sleep when nothing is happening, and wake when one or more sockets have something to report.
  • The current blocking functions RPi.GPIO.input and output would still be supported.
  • The current callbacks would become unnecessary, as would the wait_for_edge, edge_detected, and add_event_detect – the sockets solution provides a solution covering these and more, although support for them should be retained if at all possible for back-compatibility reasons.
  • In passing, I’ll also fix another restriction I hit in the Turtle project, where a GPIO output always is set up with default value 0; instead RPi.GPIO.setup() will carry an extra parameter, defaulted to 0, but allowing the user to specify 1 if needed.

The only problem is I have no idea how Python libraries really work.  I have the ‘C’ code for the RPi.GPIO library, and 22 years experience of writing C code.  I’m just not 100% confident that the GPIO fds can be used by socket.select (although I think it should work since select() uses epoll() under the surface) nor am I experienced in writing the C code to support the new python class required.

Sounds like an interesting challenge. Not sure whether I’m up to it, but I’ll give it a try in the background.

Running to stand still…

Just as my python code was nearly complete, GPIO interrupt support has been added. To me this is fantastic as I loathed the fact my drone remote control and the drone itself we’re having to periodically (every 0.1 seconds) check its inputs to update itself. Now I’ll be able to

  • add the joystick interrupts on the RC, for both the switches and the joystick movements, so the remote control can just sleep pending an interrupt. It’s not actually quite that simple as in fact, the remote control periodically sends a KEEPALIVE message to the drone so the drone knows it’s still in contact with the RC. If it finds it’s not, it switches to auto-landing mode, but it does at least reduce the work when there is no change on the joysticks.
  • add accelerometer interrupts to the drone itself, so if it’s running smoothly, there’s no need for it to poll the accelerometer; it can just sleep on select() waiting for the next RC commands.
  • Together this means lower battery usage on both, and faster reaction to critical events. A double plus!

    The only downside is the rewrite of the code scheduling and resultant testing. I think for the mo, I’ll add this to my todo list and concentrate on getting the drone airborne and stable first.