GawiQ
Posts: 14
Joined: Sat Mar 24, 2018 3:31 pm

Fast, simple code to read and save accelerometer data

Sat Mar 24, 2018 4:58 pm

Hi
In need of a programme to read and save data from two accelerometers (adafruit LIS3DH) I managed to implement something based on Matt Dysons Github (https://github.com/mattdy/python-lis3dh).
I used two almost identical programmes, each one to read data from different sensor. They differ only in I2C bus (one is 0, the other 1).
Then I made the third programme to read and save data from the previous two.

The problem is - it is all too slow, awfully (unnecesserily too I think) complicated and I don't know what and how to do it properly :/ For now it goes at around 70-80Hz and it would be best if it could do a 2000Hz (don't know if it's possible) but any speedup will help.
Also does anyone know any simple method to check the timing/frequency of a loop?

Here are the codes:

Code: Select all

from Adafruit_GPIO import I2C
import RPi.GPIO as GPIO  # needed for Hardware interrupt


class LIS3DH0:

    # I2C
    i2c = None
    I2C_ADDRESS_1 = 0x18
    I2C_ADDRESS_2 = 0x19
    # default
    I2C_DEFAULT = I2C_ADDRESS_1

    # bus
    BUS_NUMBER = 0  

    # Ranges
    RANGE_2G  = 0b00  # default
    RANGE_4G  = 0b01
    RANGE_8G  = 0b10
    RANGE_16G = 0b11
    # default
    RANGE_DEFAULT = RANGE_2G

    # Refresh rates
    DATARATE_400HZ          = 0b0111  # 400Hz  # default
    DATARATE_200HZ          = 0b0110  # 200Hz
    DATARATE_100HZ          = 0b0101  # 100Hz
    DATARATE_50HZ           = 0b0100  # 50Hz
    DATARATE_25HZ           = 0b0011  # 25Hz
    DATARATE_10HZ           = 0b0010  # 10Hz
    DATARATE_1HZ            = 0b0001  # 1Hz
    DATARATE_POWERDOWN      = 0       # Power down
    DATARATE_LOWPOWER_1K6HZ = 0b1000  # Low power mode (1.6KHz)
    DATARATE_LOWPOWER_5KHZ  = 0b1001  # Low power mode (5KHz) / Normal power mode (1.25KHz)
    # default
    DATARATE_DEFAULT = DATARATE_400HZ

    # Registers
    REG_STATUS1       = 0x07
    REG_OUTADC1_L     = 0x08
    REG_OUTADC1_H     = 0x09
    REG_OUTADC2_L     = 0x0A
    REG_OUTADC2_H     = 0x0B
    REG_OUTADC3_L     = 0x0C
    REG_OUTADC3_H     = 0x0D
    REG_INTCOUNT      = 0x0E
    REG_WHOAMI        = 0x0F  # Device identification register
    REG_TEMPCFG       = 0x1F
    REG_CTRL1         = 0x20  # Used for data rate selection, and enabling/disabling individual axis
    REG_CTRL2         = 0x21
    REG_CTRL3         = 0x22
    REG_CTRL4         = 0x23  # Used for BDU, scale selection, resolution selection and self-testing
    REG_CTRL5         = 0x24
    REG_CTRL6         = 0x25
    REG_REFERENCE     = 0x26
    REG_STATUS2       = 0x27
    REG_OUT_X_L       = 0x28
    REG_OUT_X_H       = 0x29
    REG_OUT_Y_L       = 0x2A
    REG_OUT_Y_H       = 0x2B
    REG_OUT_Z_L       = 0x2C
    REG_OUT_Z_H       = 0x2D
    REG_FIFOCTRL      = 0x2E
    REG_FIFOSRC       = 0x2F
    REG_INT1CFG       = 0x30
    REG_INT1SRC       = 0x31
    REG_INT1THS       = 0x32
    REG_INT1DUR       = 0x33
    REG_CLICKCFG      = 0x38
    REG_CLICKSRC      = 0x39
    REG_CLICKTHS      = 0x3A
    REG_TIMELIMIT     = 0x3B
    REG_TIMELATENCY   = 0x3C
    REG_TIMEWINDOW    = 0x3D

    # Values
    DEVICE_ID  = 0x33
    INT_IO     = 0x04  # GPIO pin for interrupt
    CLK_NONE   = 0x00
    CLK_SINGLE = 0x01
    CLK_DOUBLE = 0x02

    AXIS_X = 0x00
    AXIS_Y = 0x01
    AXIS_Z = 0x02

    # changed busnumber to 1 (from -1)
    # alternative i2c address=0x19
    def __init__(self, address=I2C_DEFAULT, bus=BUS_NUMBER,
                 g_range=RANGE_DEFAULT, datarate=DATARATE_DEFAULT,
                 debug=False):
        self.isDebug = debug
        self.debug("Initialising LIS3DH0")

        # self.i2c = Adafruit_I2C(address, busnum=bus)
        self.i2c = I2C.Device(address, busnum=bus)
        self.address = address

        try:
            val = self.i2c.readU8(self.REG_WHOAMI)
            if val != self.DEVICE_ID:
                # raise Exception(("Device ID incorrect - expected 0x%X, " +
                #                  "got 0x%X at address 0x%X") % (
                #                          self.DEVICE_ID, val, self.address))
                raise Exception(("Device ID incorrect - expected 0x{:x}, " +
                                 "got 0x{:x} at address 0x{:x}").format(
                                     self.DEVICE_ID, val, self.address))

            self.debug(("Successfully connected to LIS3DH0 " +
                        "at address 0x{:x}").format(self.address))
        except Exception as e:
            print("Error establishing connection with LIS3DH0")
            print(e)

        # Enable all axis
        self.setAxisStatus(self.AXIS_X, True)
        self.setAxisStatus(self.AXIS_Y, True)
        self.setAxisStatus(self.AXIS_Z, True)

        # Set refresh rate (default: 400Hz)
        self.setDataRate(datarate)

        self.setHighResolution()
        self.setBDU()

        self.setRange(g_range)

    # Get reading from X axis
    def getX(self):
        return self.getAxis(self.AXIS_X)

    # Get reading from Y axis
    def getY(self):
        return self.getAxis(self.AXIS_Y)

    # Get reading from Z axis
    def getZ(self):
        return self.getAxis(self.AXIS_Z)

    # Get a reading from the desired axis
    def getAxis(self, axis):
        # Determine which register we need to read from (2 per axis)
        base = self.REG_OUT_X_L + (2 * axis)

        # Read the first register (lower bits)
        low = self.i2c.readU8(base)
        # Read the next register (higher bits)
        high = self.i2c.readU8(base + 1)
        # Combine the two components
        res = low | (high << 8)
        # Calculate the twos compliment of the result
        res = self.twosComp(res)

        # Fetch the range we're set to, so we can
        # accurately calculate the result
        g_range = self.getRange()
        divisor = 1
        if g_range == self.RANGE_2G:    divisor = 16380
        elif g_range == self.RANGE_4G:  divisor = 8190
        elif g_range == self.RANGE_8G:  divisor = 4096
        elif g_range == self.RANGE_16G: divisor = 1365.33

        return float(res) / divisor

    # Get the range that the sensor is currently set to
    def getRange(self):
        val = self.i2c.readU8(self.REG_CTRL4)  # Get value from register
        val = (val >> 4)  # Remove lowest 4 bits
        val &= 0b0011  # Mask off two highest bits

        if val == self.RANGE_2G:   return self.RANGE_2G
        elif val == self.RANGE_4G: return self.RANGE_4G
        elif val == self.RANGE_8G: return self.RANGE_8G
        else:                      return self.RANGE_16G

    # Set the range of the sensor (2G, 4G, 8G, 16G)
    def setRange(self, g_range):
        if g_range < 0 or g_range > 3:
            raise Exception("Tried to set invalid range")

        val = self.i2c.readU8(self.REG_CTRL4)  # Get value from register
        val &= ~(0b110000)  # Mask off lowest 4 bits
        val |= (g_range << 4)  # Write in our new range
        self.writeRegister(self.REG_CTRL4, val)  # Write back to register

    # Enable or disable an individual axis
    # Read status from CTRL_REG1, then write back with
    # appropriate status bit changed
    def setAxisStatus(self, axis, enable):
        if axis < 0 or axis > 2:
            raise Exception("Tried to modify invalid axis")

        current = self.i2c.readU8(self.REG_CTRL1)
        status = 1 if enable else 0
        final = self.setBit(current, axis, status)
        self.writeRegister(self.REG_CTRL1, final)

    def setInterrupt(self, mycallback):
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(self.INT_IO, GPIO.IN)
        GPIO.add_event_detect(self.INT_IO, GPIO.RISING, callback=mycallback)

    def setClick(self, clickmode, clickthresh=80,
                 timelimit=10, timelatency=20, timewindow=100,
                 mycallback=None):
        if (clickmode == self.CLK_NONE):
            val = self.i2c.readU8(self.REG_CTRL3)  # Get value from register
            val &= ~(0x80)  # unset bit 8 to disable interrupt
            self.writeRegister(self.REG_CTRL3, val)  # Write back to register
            self.writeRegister(self.REG_CLICKCFG, 0)  # disable all interrupts
            return
        self.writeRegister(self.REG_CTRL3, 0x80)  # turn on int1 click
        self.writeRegister(self.REG_CTRL5, 0x08)  # latch interrupt on int1

        if (clickmode == self.CLK_SINGLE):
            # turn on all axes & singletap
            self.writeRegister(self.REG_CLICKCFG, 0x15)
        if (clickmode == self.CLK_DOUBLE):
            # turn on all axes & doubletap
            self.writeRegister(self.REG_CLICKCFG, 0x2A)

        # set timing parameters
        self.writeRegister(self.REG_CLICKTHS, clickthresh)
        self.writeRegister(self.REG_TIMELIMIT, timelimit)
        self.writeRegister(self.REG_TIMELATENCY, timelatency)
        self.writeRegister(self.REG_TIMEWINDOW, timewindow)

        if mycallback is not None:
            self.setInterrupt(mycallback)

    def getClick(self):
        reg = self.i2c.readU8(self.REG_CLICKSRC)  # read click register
        self.i2c.readU8(self.REG_INT1SRC)         # reset interrupt flag
        return reg

    # Set the rate (cycles per second) at which data is gathered
    def setDataRate(self, dataRate):
        val = self.i2c.readU8(self.REG_CTRL1)  # Get current value
        val &= 0b1111  # Mask off lowest 4 bits
        val |= (dataRate << 4)  # Write in our new data rate to highest 4 bits
        self.writeRegister(self.REG_CTRL1, val)  # Write back to register

    # Set whether we want to use high resolution or not
    def setHighResolution(self, highRes=True):
        val = self.i2c.readU8(self.REG_CTRL4)  # Get current value
        status = 1 if highRes else 0

        # High resolution is bit 4 of REG_CTRL4
        final = self.setBit(val, 3, status)
        self.writeRegister(self.REG_CTRL4, final)

    # Set whether we want to use block data update or not
    # False = output registers not updated until MSB and LSB reading
    def setBDU(self, bdu=True):
        val = self.i2c.readU8(self.REG_CTRL4)  # Get current value
        status = 1 if bdu else 0

        # Block data update is bit 8 of REG_CTRL4
        final = self.setBit(val, 7, status)
        self.writeRegister(self.REG_CTRL4, final)

    # Write the given value to the given register
    def writeRegister(self, register, value):
        self.debug("WRT {} to register 0x{:x}".format(
            bin(value), register))
        self.i2c.write8(register, value)

    # Set the bit at index 'bit' to 'value' on 'input' and return
    def setBit(self, input, bit, value):
        mask = 1 << bit
        input &= ~mask
        if value:
            input |= mask
        return input

    # Return a 16-bit signed number (two's compliment)
    # Thanks to http://stackoverflow.com/questions/16124059/trying-to-
    #   read-a-twos-complement-16bit-into-a-signed-decimal
    def twosComp(self, x):
        if (0x8000 & x):
            x = - (0x010000 - x)
        return x

    # Print an output of all registers
    def dumpRegisters(self):
        for x in range(0x0, 0x3D):
            read = self.i2c.readU8(x)
            print("{:x}: {}".format(x, bin(read)))

    def debug(self, message):
        if not self.isDebug:
            return
        print(message)

Code: Select all

from LIS3DH_bus1 import LIS3DH1
from LIS3DH_bus0 import LIS3DH0
from time import sleep



if __name__ =='__main__':
    sensor1 = LIS3DH1(debug=True)
    sensor1.setRange(LIS3DH1.RANGE_2G)
    sensor0 = LIS3DH0(debug=True)
    sensor0.setRange(LIS3DH0.RANGE_2G)
    
    
    file = open("wyniki.txt","a")
    import time
    timestr = time.strftime("%Y%m%d-%H%M%S")
    file.write("\r")
    file.write("\r")
    file.write(timestr)
    file.write("\r")  
  
    
    t_end = time.time() + 2 
    while time.time() < t_end:
    #while True:
        
        t = time.time()
        
        x = sensor1.getX()
        y = sensor1.getZ()
        z = sensor1.getY()*-1
        
        x0 = sensor0.getY()*-1
        y0 = sensor0.getX()
        z0 = sensor0.getZ()

        print("\rX: %.6f\tY: %.6f\tZ: %.6f | \tX0: %.6f\tY0: %.6f\tZ0: %.6f\tt: %.6f" % (x, y, z, x0, y0, z0, t))
       
        file = open("wyniki.txt","a")
        file.write("\r%.6f\t%.6f\t%.6f\t%.6f\t%.6f\t%.6f\t%.6f" % (x, y, z, x0, y0, z0, t))
        sleep(1/10) 


Please help the noob
Cheers

saltydog
Posts: 39
Joined: Mon Dec 24, 2012 10:40 am

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 10:59 am

The last line in 'main':
sleep(1/10)
Your only taking readings at 10 samples per second.
If your using python2.7, then that is 0. which might explain why your getting readings at 70 per second.

Is this code any easier to read (less features):
https://github.com/DcubeTechVentures/LI ... IS3DHTR.py

This is a different sensor but might help explain what’s going on:
http://blog.bitify.co.uk/2013/11/readin ... berry.html

Chris

GawiQ
Posts: 14
Joined: Sat Mar 24, 2018 3:31 pm

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 2:21 pm

Yeah, I know. Sleep(1/10) just left after many different tries of testing. I run it on python 3 and did trials with sleep(0.00001) even, still with the same results.
Codes you linked look much cleaner and understandable for me, thanks :)

GawiQ
Posts: 14
Joined: Sat Mar 24, 2018 3:31 pm

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 4:34 pm

One thing - could you explain how to select other output data rates and range in the first code you linked? I don't understand the way it is selected there :/
Here is the sensors data sheet: http://www.st.com/content/ccc/resource/ ... 274221.pdf

GawiQ
Posts: 14
Joined: Sat Mar 24, 2018 3:31 pm

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 5:55 pm

Using the code from the first link i made something like this:

Code: Select all

import smbus
import time

# Get I2C bus
bus1= smbus.SMBus(1)
bus0 = smbus.SMBus(0)
# LIS3DHTR address, 0x18(24)
# Select control register1, 0x20(32)
#              0x27(39)	Power ON mode, Data rate selection = 10 Hz
#                    X, Y, Z-Axis enabled
bus1.write_byte_data(0x18, 0x20, 0x27)
bus0.write_byte_data(0x18, 0x20, 0x27)
# LIS3DHTR address, 0x18(24)
# Select control register4, 0x23(35)
#        0x00(00)	Continuous update, Full-scale selection = +/-2G
bus1.write_byte_data(0x18, 0x23, 0x10)
bus0.write_byte_data(0x18, 0x23, 0x10)

t_end = time.time() + 1 
while time.time() < t_end:

# LIS3DHTR address, 0x18(24)
# Read data back from 0x28(40), 2 bytes
# X-Axis LSB, X-Axis MSB
    data_b1_0 = bus1.read_byte_data(0x18, 0x28)
    data_b1_1 = bus1.read_byte_data(0x18, 0x29)

# Convert the data
    x1 = data_b1_1 * 256 + data_b1_0
    if x1 > 32767 :
        x1 -= 65536

# LIS3DHTR address, 0x18(24)
# Read data back from 0x2A(42), 2 bytes
# Y-Axis LSB, Y-Axis MSB
    data_b1_0 = bus1.read_byte_data(0x18, 0x2A)
    data_b1_1 = bus1.read_byte_data(0x18, 0x2B)

# Convert the data
    y1 = data_b1_1 * 256 + data_b1_0
    if y1 > 32767 :
        y1 -= 65536

# LIS3DHTR address, 0x18(24)
# Read data back from 0x2C(44), 2 bytes
# Z-Axis LSB, Z-Axis MSB
    data_b1_0 = bus1.read_byte_data(0x18, 0x2C)
    data_b1_1 = bus1.read_byte_data(0x18, 0x2D)

# Convert the data
    z1 = data_b1_1 * 256 + data_b1_0
    if z1 > 32767 :
        z1 -= 65536
    	
    	
    data_b0_0 = bus0.read_byte_data(0x18, 0x28)
    data_b0_1 = bus0.read_byte_data(0x18, 0x29)
    
    x0 = data_b0_1 * 256 + data_b0_0
    if x0 > 32767 :
        x0 -= 65536

    data_b0_0 = bus0.read_byte_data(0x18, 0x2A)
    data_b0_1 = bus0.read_byte_data(0x18, 0x2B)

    y0 = data_b0_1 * 256 + data_b0_0
    if y0 > 32767 :
        y0 -= 65536

    data_b0_0 = bus1.read_byte_data(0x18, 0x2C)
    data_b0_1 = bus1.read_byte_data(0x18, 0x2D)

# Convert the data
    z0 = data_b0_1 * 256 + data_b0_0
    if z0 > 32767 :
        z0 -= 65536
    
    t = time.time()   
    time.sleep(0.00001)
# Output data to screen
    print("\rX1: %.6f\tY1: %.6f\tZ1: %.6f\tX0: %.6f\tY0: %.6f\tZ0: %.6f\tt: %.6f" % (x1, y1, z1, x0, y0, z0, t))
Aside from the sensors output data rate the frequency of the while loop is around 175Hz. Does anyone know how to speed it up more? I've heard about cython or using different libraries (wiringPi maybe)

User avatar
bensimmo
Posts: 4175
Joined: Sun Dec 28, 2014 3:02 pm
Location: East Yorkshire

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 7:11 pm

What's the maximum data rate the sensor can report at?
Remove any print from the loop,

Place the writing to a file in its own thread.
Build up the data in memory and batch write to the file as needed.
That should keep the writes down (so not slowing it down) and when it does it doesn't stop the program.
Maybe?
Increase the speed of the I2C bus?

Check you device can actually do it.

GawiQ
Posts: 14
Joined: Sat Mar 24, 2018 3:31 pm

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 9:01 pm

Thanks for the reply.
Sensors can report at over 5000Hz.
Gonna try saving data to an array and print it out after the loop ends.
Default data rate for I2C is 100kHz if I'm not wrong. Do you think it needs to be sped up?

User avatar
OutoftheBOTS
Posts: 711
Joined: Tue Aug 01, 2017 10:06 am

Re: Fast, simple code to read and save accelerometer data

Sun Mar 25, 2018 9:15 pm

Understanding I2C protocol. Please read the protocol for reading fro a I2C bus on page 7 http://www.ti.com/lit/an/slva704/slva704.pdf

As you can see each byte you read has a number of over heads. It takes 38 clock cycles to read 1 byte of data on a I2C bus. I2C is great for having many slaves on the 1 bus as it has addressing of each device but this comes at a price of over heads lowing data transfer speed.

It is usual to use SPI bus for reading IMU sensors at high freq as SPI has less over heads and higher freq meaning much faster data transfer,

User avatar
bensimmo
Posts: 4175
Joined: Sun Dec 28, 2014 3:02 pm
Location: East Yorkshire

Re: Fast, simple code to read and save accelerometer data

Mon Mar 26, 2018 6:45 am

GawiQ wrote:
Sun Mar 25, 2018 9:01 pm
Thanks for the reply.
Sensors can report at over 5000Hz.
Gonna try saving data to an array and print it out after the loop ends.
Default data rate for I2C is 100kHz if I'm not wrong. Do you think it needs to be sped up?
See above about SPI,

But try it see what happens.
(I don't know, i've not looked into it, I just know it can do it)
In /boot/config.txt
Alter
dtparam=i2c_arm=on
to
dtparam=i2c_arm=on,i2c_arm_baudrate=400000
Save and reboot.
It should now run at the basic original spec. (1992 that was)
No idea if it can run at any of the newer, yet still very old speeds.

Return to “Python”