[SOLVED] Python library for 6-axis-accelerometer-and-compass


#1

I’ve recently purchased the Grove sensor 6-axis-accelerometer-compass V2.0 LSM303D and have attempted to connect it to the GrovePi+ on a Raspberry Pi.

However, I have been unable to find a Python Library to communicate with this device. I was able to find an .ino Arduino code for it https://github.com/Seeed-Studio/6Axis_Accelerometer_And_Compass_v2/blob/master/Accelerometer_Compass_LSM303D.cpp ; however I can’t use this directly with the device (at least, not as intended).

Is there a GrovePi library for this device anywhere, or will I need to reverse engineer the .ino into python code?


#2

Hey,
We don;t have the library for the LSM303D sensor yet but you should be able to get it working on the grovePi. First connect it on the I2C port of the grovePi and then run sudo i2cdetect -y 1 to find the address. Note that down, then use Adafruit library here: https://github.com/adafruit/Adafruit-Raspberry-Pi-Python-Code/tree/master/Adafruit_LSM303 or https://github.com/mwilliams03/BerryIMU to make it work.

So let us know if you face any problems.

-Karan


#3

Hey Karen,
Thanks for your post. I gave writing a Python version of the library a go in order to get it to work.

It almost works, except I am having a few problems with the output of the device.

Here’s the code I’ve put together. Apologies for any rookie errors, this is effectively my foray into Python…

#!/usr/bin.env python
#
# Python translation of Arduino code provided by Frankie.Chu
# for GrovePi+ module.
#
#
# Hardware:     Grove - 3-Axis magnetometer/3-axis accelometer (LMS303D)
#
'''
##
'''

import smbus
import time
import math
import RPi.GPIO as GPIO
import struct

rev = GPIO.RPI_REVISION
if rev == 2 or rev == 3:
    bus = smbus.SMBus(1)
else:
    bus = smbus.SMBus(0)

### LSM303 Address ###
LSM303D_ADDR    = 0x1E # Assuming SA0 grounded

### LSM303 Register definitions ###
TEMP_OUT_L		= 0x05
TEMP_OUT_H		= 0x06
STATUS_REG_M	= 0x07
OUT_X_L_M 		= 0x08
OUT_X_H_M 		= 0x09
OUT_Y_L_M 		= 0x0A
OUT_Y_H_M 		= 0x0B
OUT_Z_L_M 		= 0x0C
OUT_Z_H_M 		= 0x0D
WHO_AM_I		= 0x0F
INT_CTRL_M		= 0x12
INT_SRC_M		= 0x13
INT_THS_L_M		= 0x14
INT_THS_H_M		= 0x15
OFFSET_X_L_M	= 0x16
OFFSET_X_H_M	= 0x17
OFFSET_Y_L_M	= 0x18
OFFSET_Y_H_M	= 0x19
OFFSET_Z_L_M	= 0x1A
OFFSET_Z_H_M	= 0x1B
REFERENCE_X 	= 0x1C
REFERENCE_Y 	= 0x1D
REFERENCE_Z 	= 0x1E
CTRL_REG0 		= 0x1F
CTRL_REG1 		= 0x20
CTRL_REG2		= 0x21
CTRL_REG3 		= 0x22
CTRL_REG4 		= 0x23
CTRL_REG5 		= 0x24
CTRL_REG6 		= 0x25
CTRL_REG7 		= 0x26
STATUS_REG_A	= 0x27
OUT_X_L_A 		= 0x28
OUT_X_H_A 		= 0x29
OUT_Y_L_A 		= 0x2A
OUT_Y_H_A 		= 0x2B
OUT_Z_L_A 		= 0x2C
OUT_Z_H_A 		= 0x2D
FIFO_CTRL		= 0x2E
FIFO_SRC		= 0x2F
IG_CFG1			= 0x30
IG_SRC1			= 0x31
IG_THS1			= 0x32
IG_DUR1			= 0x33
IG_CFG2			= 0x34
IG_SRC2			= 0x35
IG_THS2			= 0x36
IG_DUR2			= 0x37
CLICK_CFG		= 0x38
CLICK_SRC		= 0x39
CLICK_THS		= 0x3A
TIME_LIMIT		= 0x3B
TIME_LATENCY	= 0x3C
TIME_WINDOW		= 0x3D
ACT_THS			= 0x3E
ACT_DUR			= 0x3F

### Mag scales ###
MAG_SCALE_2     = 0x00 # full-scale is +/- 2 Gauss
MAG_SCALE_4     = 0x20 # +/- 4 Guass
MAG_SCALE_8     = 0x40 # +/- 8 Guass
MAG_SCALE_12    = 0x60 # +/- 12 Guass

ACCEL_SCALE     = 2 # +/- 2g

X = 0
Y = 1
Z = 2

def twos_comp(val, bits):
    # Calculate the 2s complement of int:val #
    if(val&(1<<(bits-1)) != 0):
        val = val - (1<<bits)
    return val
#   return val if val < 32768 else val - 65536

class accelcomp:
    mag = [0,0,0]
    accel = [0,0,0]
    tiltcomp = [0,0,0]
    heading=0
    headingDegrees=0
    tiltHeading=0
    tiltHeadingDegrees=0

    def __init__(self):
        whoami = bus.read_byte_data(LSM303D_ADDR, WHO_AM_I)

        if(whoami == 0x49):
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG1, 0x57) # 0x57 = ODR=50hz, all accel axes on ## maybe 0x27 is Low Res?
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG2, (3<<6)|(0<<3)) # set full scale +/- 2g
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG3, 0x00) # no interrupt
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG4, 0x00) # no interrupt
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG5, (4<<2)) # 0x10 = mag 50Hz output rate
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG6, MAG_SCALE_2) # Magnetic Scale +/1 1.3 Guass
            bus.write_byte_data(LSM303D_ADDR, CTRL_REG7, 0x00) # 0x00 continuous conversion mode
            
    def getMag(self):
        self.mag[X] = twos_comp(bus.read_byte_data(LSM303D_ADDR, OUT_X_H_M) << 8 | 
                          bus.read_byte_data(LSM303D_ADDR, OUT_X_L_M), 16)
        self.mag[Y] = twos_comp(bus.read_byte_data(LSM303D_ADDR, OUT_Y_H_M) << 8 | 
                          bus.read_byte_data(LSM303D_ADDR, OUT_Y_L_M), 16)
        self.mag[Z] = twos_comp(bus.read_byte_data(LSM303D_ADDR, OUT_Z_H_M) << 8 | 
                          bus.read_byte_data(LSM303D_ADDR, OUT_Z_L_M), 16)

    def getAccel(self):
        accel = [0,0,0]
        accel[X] = twos_comp(bus.read_byte_data(LSM303D_ADDR, OUT_X_H_A) << 8 | 
                           bus.read_byte_data(LSM303D_ADDR, OUT_X_L_A), 16)
        accel[Y] = twos_comp(bus.read_byte_data(LSM303D_ADDR, OUT_Y_H_A) << 8 | 
                           bus.read_byte_data(LSM303D_ADDR, OUT_Y_L_A), 16)
        accel[Z] = twos_comp(bus.read_byte_data(LSM303D_ADDR, OUT_Z_H_A) << 8 | 
                           bus.read_byte_data(LSM303D_ADDR, OUT_Z_L_A), 16)

        for i in range(X, Z+1):
            self.accel[i] = accel[i] / math.pow(2, 15) * ACCEL_SCALE

    def getHeading(self):
        self.heading = math.atan2(self.mag[X], self.mag[Y])

        if self.heading < 0:
            self.heading += 2*math.pi
        if self.heading > 2*math.pi:
            self.heading -= 2*math.pi

        self.headingDegrees = round(math.degrees(self.heading),2)

    def getTiltHeading(self):
        truncate = [0,0,0]
        for i in range(X, Z+1):
            truncate[i] = math.copysign(min(math.fabs(self.accel[i]), 1.0), self.accel[i])
        try:
            pitch = math.asin(-1*truncate[X])
            roll = math.asin(truncate[Y]/math.cos(pitch)) if abs(math.cos(pitch)) >= abs(truncate[Y]) else 0
            # set roll to zero if pitch approaches -1 or 1
        
            self.tiltcomp[X] = self.mag[X] * math.cos(pitch) + self.mag[Z] * math.sin(pitch)
            self.tiltcomp[Y] = self.mag[X] * math.sin(roll) * math.sin(pitch) + \
                               self.mag[Y] * math.cos(roll) - self.mag[Z] * math.sin(roll) * math.cos(pitch)
            self.tiltcomp[Z] = self.mag[X] * math.cos(roll) * math.sin(pitch) + \
                               self.mag[Y] * math.sin(roll) + \
                               self.mag[Z] * math.cos(roll) * math.cos(pitch)

            self.tiltHeading = math.atan2(self.tiltcomp[Y], self.tiltcomp[X])

            if self.tiltHeading < 0:
                self.tiltHeading += 2*math.pi
            if self.tiltHeading > 2*math.pi:
                self.heading -= 2*math.pi

            self.tiltHeadingDegrees = round(math.degrees(self.tiltHeading),2)

        except Exception:
            print "AccelX {}, AccelY {}".format(self.accel[X], self.accel[Y])
            print "TruncX {}, TruncY {}".format(truncate[X], truncate[Y])
            print "Pitch {}, cos(pitch) {}, Bool(cos(pitch)) {}".format(pitch, math.cos(pitch), bool(math.cos(pitch)))

    def isMagReady(self):
        temp =  bus.read_byte_data(LSM303D_ADDR, STATUS_REG_M) & 0x03

        return temp

    def update(self):

        self.getAccel()
        self.getMag()
        self.getHeading()
        self.getTiltHeading()

if __name__ == '__main__':

    from time import sleep

    lsm = accelcomp()

    print '     Accelerometer     ||        Compass        ||              Heading'
    print '   x   |   y   |   z   ||   x   |   y   |   z   ||       calc      | tilt corrected  |'
    while True:
        lsm.getAccel()
        while not bool(lsm.isMagReady()):
            print "Not Ready!"
            sleep(.01)
        lsm.getMag()
        lsm.getHeading()
        lsm.getTiltHeading()
        print '{0:.3f} |{1:.3f} |{2:.3f} ||{3:6} |{4:6} |{5:6} ||  {6:6} deg     |  {7:6} deg     |'.format(
                    lsm.accel[X], lsm.accel[Y], lsm.accel[Z], lsm.mag[X], lsm.mag[Y], lsm.mag[Z], lsm.headingDegrees, lsm.tiltHeadingDegrees)
        sleep(.5)

Running my library as main for testing gives me data like the following:

 Accelerometer     ||        Compass        ||              Heading

x | y | z || x | y | z || calc | tilt corrected |
0.022 |-0.012 |1.056 || 876 | -1119 | 5788 || 141.94 deg | 305.58 deg |
0.027 |-0.011 |1.052 || 919 | -1128 | 5826 || 140.83 deg | 305.62 deg |
0.026 |-0.010 |1.053 || 903 | -1108 | 5777 || 140.82 deg | 305.61 deg |
0.024 |-0.009 |1.054 || 914 | -1140 | 5790 || 141.28 deg | 305.57 deg |
0.024 |-0.010 |1.056 || 895 | -1122 | 5801 || 141.42 deg | 305.42 deg |
0.024 |-0.010 |1.055 || 905 | -1088 | 5747 || 140.25 deg | 306.69 deg |
0.022 |-0.008 |1.057 || 900 | -1106 | 5748 || 140.86 deg | 306.15 deg |

It may be difficult to see the problems from the output directly:
1/ The Accelerometer at rest shows values that look almost correct; however the resting Z-direction gives a reading greater than 1g. This is true for the X and Y axes when rotated to be parallel to the gravity vector field. This causes math errors with asin. I’ve truncated these values in the tilt corrected code to avoid these errors.
2/ The compass gains an error over a full rotation: Facing Magnetic North (as determined by my phone), the value outputted is roughly correct. As the sensor is rotated clockwise, the error increases almost linearly to about 15 degree error at 200 degrees, at which point it jumps up to about 30 degrees off at 250 degrees, then rapidly comes back to within 3 degrees at North again.

I’ve attached a spreadsheet of the trials performed and the resulting graph.

I can’t see any problem with the code; the maths are equivalent to the c++ code I was working from. I’ve examined the LSM303D datasheet closely, but cannot find any reason for these discrepancies. Could this be an issue with the device itself, or is there something in my code I’ve missed?


#4

Hey,
Congrats on getting the sensor to work. Really glad to see that it’s working.

I think the accelerometer will show around 1G in one axis because that denotes the acceleration due to gravity. The other errors might be more or less because of errors in register config or how you read the data. Can you compare the functions from here to make sure that you are not missing anything: https://github.com/adafruit/Adafruit-Raspberry-Pi-Python-Code/blob/master/Adafruit_LSM303/Adafruit_LSM303.py

-Karan


#5

Hey

I see this post is from November 2015. Does any library exist now or will there ever be one? Your wiki says the LM303SD is Rasberry PI compatible but I cant find a library anywhere for it.


#6

Hi @stehag,

I think this is the library you’re looking for.
Please click on the link / banner.

https://github.com/DexterInd/GrovePi/tree/master/Software/Python/grove_6axis_acc_compass

If there’s any assistance needed, please contact us.

If not, please tell us, so we can close the thread - I can see the OP asked for a library which didn’t exist back then, but it now does.

Thank you!


#7

Thank you for your answer.

I see that I didn’t read the the topic careful enough. I found the library for the 6 axsis accelerometer and compass. But it turns out I have this:[ https://www.seeedstudio.com/Grove-6-Axis-Accelerometer%26amp%3BGyroscope-p-2606.html]

So I am actually missing the library for the 6 axsis accelerometer and GYROSCOPE I cant find this library either. I tried changing the address to 0x6A an running the provided example for the 6 axis accelerometer and compass, but the program is stuck at acc=acc_mag.getRealAccel()


#8

Hi @stehag,

I understand your issue.

Here’s a library for the LSM6DS3 6DOF sensor:


Experiment with this library and see if this solves your issue.
You’ll need to have another python script in the same directory in which you import the library like in the following example:

from LSM6DS3 import *
from time import sleep

mySensor = LSM6DS3()

while True:
    print(mySensor.readRawAccelX(), \
        mySensor.readRawAccelY(), \
        mySensor.readRawAccelZ(), \
        mySensor.readRawGyroX(), \
        mySensor.readRawGyroY(), \
        mySensor.readRawGyroZ())
    sleep(0.08)

Please let us know if there’s anything unclear, or if you get stuck somewhere.

Thank you!


#10

@stehag sorry to hear about this. I think you might want to get in touch with Adafruit or mwbritton about why it’s not working or how to troubleshoot it.


#11