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.

9 thoughts on “Performance enhancements

  1. So could I ise readSensorsRaw() in a script, inside a loop without temporize it and be sure to read data at my sample rate (in this case 200Hz)?
    for example, could I use it as:

    while True:
    read = MPU6050.readSensorsRaw()

    PS:Thanks for the patience, I am sorry but I am trying to understand these concepts.

    • You do not need to temporize the reads. Instead you should wait for the sensor chip to tell you new data is ready. There are several ways to do it.

      The code here loops reading the “Data Ready” register. The latest code on GitHub/PiStuffing/Quadcopter uses a hardware interrupt as it’s much faster.

      I now have data update frequency set to 1kHz (the maximum), and by using the interrupt I always get good data.

      • Thanks. I was reading the qc.py file, I think you are refering to the function: readSensors() with the function
        RPIO.edge_detect_wait(RPIO_DATA_READY_INTERRUPT) inside

        Now, if I put it inside a while loop as:

        while True:
        read = readSensors()

        I do not care about loop cicles and read frequency because it is scheduled by the read of the interrupt inside, right?

        • Yes, exactly! But edge_detect_wait() comes from my own higher performance version of the RPi.GPIO library (again on my GitHub) – but I think the standard one now has the same performance improvements in its wait_for_edge() function.

          • Ok! Thank you very much! Just two question:
            In the code above you use
            while not (self.i2c.readU8(self.__MPU6050_RA_INT_STATUS) == 0x01):
            time.sleep(0.001)
            continue

            Is it the other way to wait for the new data is ready ? In this way I can use a simple:

            while True:
            read = readSensorsRaw()

            for be sure that, independently how much fast loop is going on, the reading will be scheduled by __MPU6050_RA_INT_STATUS that I expect is at sample rate (200Hz), right?

            Second question: why 1 kHz for the time.sleep() for the interrupt status register?

  2. Thanks for this tutorial. Reading all register with read_i2c_data_block() is a powerful idea, but just a question: when you have to read data from gyro do you have to temporize the for/while loop with the time.sleep() at sample rate or it does not need that ?

    • Reading the sensors do need to be timed to make sure the code reads all 14 bytes before they get overwritten. But the best way is to use the hardware interrupt provided by the MPU6050. The code loops as fast as it can, only pausing waiting for the interrupt that says a new set of data from the sensor is ready.

      Doing it that way, plus lots of other tweaks now means the code now loops at 450Hz compared to the 50Hz mentioned above.

  3. Pingback: Tutorial: come leggere i dati dal IMU | solenerotech IT

  4. Pingback: Tutorial: How to read data from IMU | solenerotech EN

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.