diff --git a/.gitignore b/.gitignore index 1dbc687..82dc2cd 100644 --- a/.gitignore +++ b/.gitignore @@ -60,3 +60,4 @@ target/ #Ipython Notebook .ipynb_checkpoints +.idea diff --git a/Adafruit_LSM303/LSM303.py b/Adafruit_LSM303/LSM303.py index d5d08de..98d2502 100644 --- a/Adafruit_LSM303/LSM303.py +++ b/Adafruit_LSM303/LSM303.py @@ -21,26 +21,25 @@ # SOFTWARE. import struct - # Minimal constants carried over from Arduino library: LSM303_ADDRESS_ACCEL = (0x32 >> 1) # 0011001x -LSM303_ADDRESS_MAG = (0x3C >> 1) # 0011110x - # Default Type -LSM303_REGISTER_ACCEL_CTRL_REG1_A = 0x20 # 00000111 rw -LSM303_REGISTER_ACCEL_CTRL_REG4_A = 0x23 # 00000000 rw -LSM303_REGISTER_ACCEL_OUT_X_L_A = 0x28 -LSM303_REGISTER_MAG_CRB_REG_M = 0x01 -LSM303_REGISTER_MAG_MR_REG_M = 0x02 -LSM303_REGISTER_MAG_OUT_X_H_M = 0x03 +LSM303_ADDRESS_MAG = (0x3C >> 1) # 0011110x +# Default Type +LSM303_REGISTER_ACCEL_CTRL_REG1_A = 0x20 # 00000111 rw +LSM303_REGISTER_ACCEL_CTRL_REG4_A = 0x23 # 00000000 rw +LSM303_REGISTER_ACCEL_OUT_X_L_A = 0x28 +LSM303_REGISTER_MAG_CRB_REG_M = 0x01 +LSM303_REGISTER_MAG_MR_REG_M = 0x02 +LSM303_REGISTER_MAG_OUT_X_H_M = 0x03 # Gain settings for set_mag_gain() -LSM303_MAGGAIN_1_3 = 0x20 # +/- 1.3 -LSM303_MAGGAIN_1_9 = 0x40 # +/- 1.9 -LSM303_MAGGAIN_2_5 = 0x60 # +/- 2.5 -LSM303_MAGGAIN_4_0 = 0x80 # +/- 4.0 -LSM303_MAGGAIN_4_7 = 0xA0 # +/- 4.7 -LSM303_MAGGAIN_5_6 = 0xC0 # +/- 5.6 -LSM303_MAGGAIN_8_1 = 0xE0 # +/- 8.1 +LSM303_MAGGAIN_1_3 = 0x20 # +/- 1.3 +LSM303_MAGGAIN_1_9 = 0x40 # +/- 1.9 +LSM303_MAGGAIN_2_5 = 0x60 # +/- 2.5 +LSM303_MAGGAIN_4_0 = 0x80 # +/- 4.0 +LSM303_MAGGAIN_4_7 = 0xA0 # +/- 4.7 +LSM303_MAGGAIN_5_6 = 0xC0 # +/- 5.6 +LSM303_MAGGAIN_8_1 = 0xE0 # +/- 8.1 class LSM303(object): @@ -75,17 +74,31 @@ def read(self): be returned with: ((accel X, accel Y, accel Z), (mag X, mag Y, mag Z)) """ + return self.read_accelerometer(), self.read_magnetometer() + + def read_accelerometer(self): + + """ + :rtype: (accel X, accel Z, accel Y ) (meters per second squared) + """ # Read the accelerometer as signed 16-bit little endian values. accel_raw = self._accel.readList(LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80, 6) accel = struct.unpack('> 4, accel[1] >> 4, accel[2] >> 4) - # Read the magnetometer. + return accel + + def read_magnetometer(self): + + """ + :rtype: (mag X, mag Y, mag Z) (micro-Teslas) + """ mag_raw = self._mag.readList(LSM303_REGISTER_MAG_OUT_X_H_M, 6) mag = struct.unpack('>hhh', mag_raw) - return (accel, mag) - def set_mag_gain(gain=LSM303_MAGGAIN_1_3): + return mag + + def set_mag_gain(self, gain=LSM303_MAGGAIN_1_3): """Set the magnetometer gain. Gain should be one of the following constants: - LSM303_MAGGAIN_1_3 = +/- 1.3 (default) diff --git a/Adafruit_LSM303/instruments.py b/Adafruit_LSM303/instruments.py new file mode 100644 index 0000000..5b1c93b --- /dev/null +++ b/Adafruit_LSM303/instruments.py @@ -0,0 +1,39 @@ +from math import atan2, degrees + + +class Instrument: + def __init__(self, lsm303_sensor): + """ + :type lsm303_sensor : Adafruit_LSM303.LSM303.LSM303 + """ + self.lsm303_sensor = lsm303_sensor + + def vector_2_degrees(self, x, y): + radians = atan2(y, x) + degrees_calc = degrees(radians) + if degrees_calc < 0: + degrees_calc = 360 + degrees_calc + return degrees_calc + + +class Compass(Instrument): + def get_heading(self): + magnet_axis_data = self.lsm303_sensor.read_magnetometer() + return self.vector_2_degrees(magnet_axis_data[0], magnet_axis_data[2]) + + +class Inclinometer(Instrument): + + def get_inclination(self): + return self.get_inclination_respect_x(), self.get_inclination_respect_y() + + def get_inclination_respect_x(self): + accel_axis_data = self.get_data() + return self.vector_2_degrees(accel_axis_data[0], accel_axis_data[2]) + + def get_inclination_respect_y(self): + accel_axis_data = self.get_data() + return self.vector_2_degrees(accel_axis_data[1], accel_axis_data[2]) + + def get_data(self): + return self.lsm303_sensor.read_accelerometer() diff --git a/examples/compass.py b/examples/compass.py new file mode 100644 index 0000000..179910d --- /dev/null +++ b/examples/compass.py @@ -0,0 +1,23 @@ + +import time +import sys + +import Adafruit_LSM303 +from Adafruit_LSM303.instruments import Compass + +lsm303 = Adafruit_LSM303.LSM303() + +# Setting magnetic gain. you can check it at http://www.magnetic-declination.com/ +#lsm303.set_mag_gain(Adafruit_LSM303.LSM303_MAGGAIN_4_7) + +compass = Compass(lsm303) + +print('Printing heading ..') + +while True: + + heading = compass.get_heading() + data = '\rHeading (Degrees): {0} '.format(heading) + sys.stdout.write(data) + sys.stdout.flush() + time.sleep(0.1) diff --git a/examples/inclinometer.py b/examples/inclinometer.py new file mode 100644 index 0000000..7972375 --- /dev/null +++ b/examples/inclinometer.py @@ -0,0 +1,26 @@ +import time +import sys + +import Adafruit_LSM303 +from Adafruit_LSM303.instruments import Inclinometer + +lsm303 = Adafruit_LSM303.LSM303() + +# Setting magnetic gain. you can check it at http://www.magnetic-declination.com/ +# lsm303.set_mag_gain(Adafruit_LSM303.LSM303_MAGGAIN_4_7) + +inclinometer = Inclinometer(lsm303) + +print('Printing Inclination (Degrees) based on x-y-z respective angle ...') + +""" + Information printed is adapted to positive and negative degrees per axis. From 0 to 179 and from 0 to -179 + This means that if you are on pitch down movement you will get a negative number until + you reach the noise of the sensor shooting again the sky. +""" +while True: + inclination = inclinometer.get_inclination() + data = '\rX : {0} Y: {1}'.format(inclination[0], inclination[1]) + sys.stdout.write(data) + sys.stdout.flush() + time.sleep(0.1) diff --git a/examples/simple.py b/examples/simple.py new file mode 100644 index 0000000..91f80ef --- /dev/null +++ b/examples/simple.py @@ -0,0 +1,36 @@ +# Simple demo of of the LSM303 accelerometer & magnetometer library. +# Will print the accelerometer & magnetometer X, Y, Z axis values every half +# second. +# Author: Tony DiCola +# License: Public Domain + +import time +import sys + +import Adafruit_LSM303 + +lsm303 = Adafruit_LSM303.LSM303() + +# Setting magnetic gain. you can check it at http://www.magnetic-declination.com/ +# lsm303.set_mag_gain(Adafruit_LSM303.LSM303_MAGGAIN_4_7) + +# Alternatively you can specify the I2C bus with a bus parameter: +# lsm303 = Adafruit_LSM303.LSM303(busum=2) + +print('Printing accelerometer & magnetometer X, Y, Z axis values, press Ctrl-C to quit...') +while True: + accel = lsm303.read_accelerometer() + mag = lsm303.read_magnetometer() + + # You also can get a accel + mag tuple with this function + # total_data = lsm303.read() + + accel_x, accel_y, accel_z = accel + mag_x, mag_z, mag_y = mag + + data = '\rAccel X={0}, Accel Y={1}, Accel Z={2}, Mag X={3}, Mag Y={4}, Mag Z={5}'.format(accel_x, accel_y, accel_z, + mag_x, mag_y, mag_z) + sys.stdout.write(data) + sys.stdout.flush() + + time.sleep(0.1) diff --git a/examples/simpletest.py b/examples/simpletest.py deleted file mode 100644 index e27a73c..0000000 --- a/examples/simpletest.py +++ /dev/null @@ -1,28 +0,0 @@ -# Simple demo of of the LSM303 accelerometer & magnetometer library. -# Will print the accelerometer & magnetometer X, Y, Z axis values every half -# second. -# Author: Tony DiCola -# License: Public Domain -import time - -# Import the LSM303 module. -import Adafruit_LSM303 - - -# Create a LSM303 instance. -lsm303 = Adafruit_LSM303.LSM303() - -# Alternatively you can specify the I2C bus with a bus parameter: -#lsm303 = Adafruit_LSM303.LSM303(busum=2) - -print('Printing accelerometer & magnetometer X, Y, Z axis values, press Ctrl-C to quit...') -while True: - # Read the X, Y, Z axis acceleration values and print them. - accel, mag = lsm303.read() - # Grab the X, Y, Z components from the reading and print them out. - accel_x, accel_y, accel_z = accel - mag_x, mag_z, mag_y = mag - print('Accel X={0}, Accel Y={1}, Accel Z={2}, Mag X={3}, Mag Y={4}, Mag Z={5}'.format( - accel_x, accel_y, accel_z, mag_x, mag_y, mag_z)) - # Wait half a second and repeat. - time.sleep(0.5) diff --git a/test/__init__.py b/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/test/test_instrument.py b/test/test_instrument.py new file mode 100644 index 0000000..ab41119 --- /dev/null +++ b/test/test_instrument.py @@ -0,0 +1,28 @@ +from unittest import TestCase + +from ddt import ddt, data, unpack +from mock import MagicMock + +from Adafruit_LSM303.instruments import Instrument, Inclinometer + + +@ddt +class TestInstrument(TestCase): + def setUp(self): + lsm303 = MagicMock() + self.instrument = Instrument(lsm303) + + @data( + (0, 0, 0), + (1, 0, 0), + (1, 1, 45), + (0, 1, 90), + (-1, 0, 180), + (-1, -1, 225), + (0, -1, 270), + (1, -0.0000000000000001, 360), + ) + @unpack + def test_vector_2_degrees(self, x, y, expected_degrees): + degrees = self.instrument.vector_2_degrees(x, y) + self.assertEqual(expected_degrees, degrees)