Merge branch 'dev'

.gitignore

@@ -7,5 +7,6 @@ __pycache__/
 config.ini
 *.sqlite3
 *.pyc
+*.egg-info/
 
 auv_control_pi/bin/*

Dockerfile

@@ -1,5 +1,5 @@
 FROM python:3.6
-ENV PYTHONUNBUFFERED
+ENV PYTHONUNBUFFERED 1
 RUN mkdir /code
 WORKDIR /code
 ADD . /code/

Makefile

@@ -0,0 +1,6 @@
+
+dev:
+	docker-compose -f docker-compose-dev.yml up
+
+dev-stop:
+	docker-compose -f docker-compose-dev.yml down

auv_control_pi/components/ahrs.py

@@ -0,0 +1,303 @@
+"""
+This is adapted from https://github.com/micropython-IMU/micropython-fusion
+
+Basically things have been renamed to AHRS naming scheme, pep8 improvements
+and adjusted to work with CPython instead of MicroPython.
+
+
+Supports 6 and 9 degrees of freedom sensors. Tested with InvenSense MPU-9150 9DOF sensor.
+Source https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU.git
+also https://github.com/kriswiner/MPU-9250.git
+Ported to Python. Integrator timing adapted for pyboard.
+User should repeatedly call the appropriate 6 or 9 DOF update method and extract heading pitch and roll angles as
+required.
+Calibrate method:
+The sensor should be slowly rotated around each orthogonal axis while this runs.
+arguments:
+getxyz must return current magnetometer (x, y, z) tuple from the sensor
+stopfunc (responding to time or user input) tells it to stop
+waitfunc provides an optional delay between readings to accommodate hardware or to avoid hogging
+the CPU in a threaded environment. It sets magbias to the mean values of x,y,z
+"""
+import os
+import asyncio
+import logging
+from ..wamp import ApplicationSession, rpc
+
+PI = os.getenv('PI', False)
+
+if PI:
+    from navio.lsm9ds1 import LSM9DS1
+
+from ..config import config
+from math import sqrt, atan2, asin, degrees, radians
+from ..utils import micros, elapsed_micros, clamp_angle
+
+logger = logging.getLogger(__name__)
+SIMULATION = os.getenv('SIMULATION', False)
+
+
+class AHRS(ApplicationSession):
+    """Class provides sensor fusion allowing heading, pitch and roll to be extracted.
+
+    This uses the Madgwick algorithm.
+    The update method must be called peiodically.
+    """
+
+    name = 'AHRS'
+
+    def __init__(self, *args, **kwargs):
+        super().__init__(*args, **kwargs)
+        if not SIMULATION:
+            self.imu = LSM9DS1()
+            self.imu.initialize()
+        self.declination = config.declination
+        self.board_offset = config.board_offset
+        # local magnetic bias factors: set from calibration
+        self.magbias = (config.magbias_x, config.magbias_y, config.magbias_z)
+        self.start_time = None              # Time between updates
+        self.q = [1.0, 0.0, 0.0, 0.0]       # vector to hold quaternion
+        gyro_meas_error = radians(270)         # Original code indicates this leads to a 2 sec response time
+        self.beta = sqrt(3.0 / 4.0) * gyro_meas_error  # compute beta (see README)
+        self.update_frequency = 10
+
+    def onConnect(self):
+        logger.info('Connecting to {} as {}'.format(self.config.realm, self.name))
+        self.join(realm=self.config.realm)
+
+    def calibrate(self, getxyz, stopfunc, waitfunc=None):
+        magmax = list(getxyz())             # Initialise max and min lists with current values
+        magmin = magmax[:]
+        while not stopfunc():
+            if waitfunc is not None:
+                waitfunc()
+            magxyz = tuple(getxyz())
+            for x in range(3):
+                magmax[x] = max(magmax[x], magxyz[x])
+                magmin[x] = min(magmin[x], magxyz[x])
+        self.magbias = tuple(map(lambda a, b: (a + b)/2, magmin, magmax))
+
+    @rpc('ahrs.get_heading')
+    def get_heading(self):
+        return self.heading
+
+    @rpc('ahrs.get_declination')
+    def get_declination(self):
+        return config.declination
+
+    @rpc('ahrs.set_declination')
+    def set_declination(self, val):
+        self.declination = float(val)
+        config.declination = self.declination
+        config.save()
+
+    @property
+    def heading(self):
+        if SIMULATION:
+            return 0
+
+        else:
+            offset = self.declination + self.board_offset
+            heading = (
+                180 + degrees(radians(offset) + atan2(2.0 * (self.q[1] * self.q[2] + self.q[0] * self.q[3]),
+                self.q[0] * self.q[0] + self.q[1] * self.q[1] - self.q[2] * self.q[2] - self.q[3] * self.q[3]))
+            )
+            heading = clamp_angle(heading)
+            return heading
+
+    @property
+    def pitch(self):
+        if SIMULATION:
+            return 0
+        return degrees(-asin(2.0 * (self.q[1] * self.q[3] - self.q[0] * self.q[2])))
+
+    @property
+    def roll(self):
+        if SIMULATION:
+            return 0
+        return degrees(atan2(2.0 * (self.q[0] * self.q[1] + self.q[2] * self.q[3]),
+            self.q[0] * self.q[0] - self.q[1] * self.q[1] - self.q[2] * self.q[2] + self.q[3] * self.q[3]))
+
+    def update_nomag(self, accel, gyro):    # 3-tuples (x, y, z) for accel, gyro
+        ax, ay, az = accel                  # Units G (but later normalised)
+        gx, gy, gz = (radians(x) for x in gyro)  # Units deg/s
+        if self.start_time is None:
+            self.start_time = micros()  # First run
+        q1, q2, q3, q4 = (self.q[x] for x in range(4))   # short name local variable for readability
+        # Auxiliary variables to avoid repeated arithmetic
+        _2q1 = 2 * q1
+        _2q2 = 2 * q2
+        _2q3 = 2 * q3
+        _2q4 = 2 * q4
+        _4q1 = 4 * q1
+        _4q2 = 4 * q2
+        _4q3 = 4 * q3
+        _8q2 = 8 * q2
+        _8q3 = 8 * q3
+        q1q1 = q1 * q1
+        q2q2 = q2 * q2
+        q3q3 = q3 * q3
+        q4q4 = q4 * q4
+
+        # Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az)
+        if norm == 0:
+            return  # handle NaN
+        norm = 1 / norm        # use reciprocal for division
+        ax *= norm
+        ay *= norm
+        az *= norm
+
+        # Gradient decent algorithm corrective step
+        s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay
+        s2 = _4q2 * q4q4 - _2q4 * ax + 4 * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az
+        s3 = 4 * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az
+        s4 = 4 * q2q2 * q4 - _2q2 * ax + 4 * q3q3 * q4 - _2q3 * ay
+        norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)    # normalise step magnitude
+        s1 *= norm
+        s2 *= norm
+        s3 *= norm
+        s4 *= norm
+
+        # Compute rate of change of quaternion
+        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
+        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
+        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
+        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
+
+        # Integrate to yield quaternion
+        deltat = elapsed_micros(self.start_time) / 1e6
+        self.start_time = micros()
+        q1 += qDot1 * deltat
+        q2 += qDot2 * deltat
+        q3 += qDot3 * deltat
+        q4 += qDot4 * deltat
+        norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)    # normalise quaternion
+        self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
+
+    def _update_data(self):
+        accel, gyro, mag = self.imu.getMotion9()
+        mx, my, mz = (mag[x] - self.magbias[x] for x in range(3))  # Units irrelevant (normalised)
+        ax, ay, az = accel  # Units irrelevant (normalised)
+        gx, gy, gz = (radians(x) for x in gyro)  # Units deg/s
+        if self.start_time is None:
+            self.start_time = micros()  # First run
+        q1, q2, q3, q4 = (self.q[x] for x in range(4))  # short name local variable for readability
+
+        # Auxiliary variables to avoid repeated arithmetic
+        _2q1 = 2 * q1
+        _2q2 = 2 * q2
+        _2q3 = 2 * q3
+        _2q4 = 2 * q4
+        _2q1q3 = 2 * q1 * q3
+        _2q3q4 = 2 * q3 * q4
+        q1q1 = q1 * q1
+        q1q2 = q1 * q2
+        q1q3 = q1 * q3
+        q1q4 = q1 * q4
+        q2q2 = q2 * q2
+        q2q3 = q2 * q3
+        q2q4 = q2 * q4
+        q3q3 = q3 * q3
+        q3q4 = q3 * q4
+        q4q4 = q4 * q4
+
+        # Normalise accelerometer measurement
+        norm = sqrt(ax * ax + ay * ay + az * az)
+        if norm == 0:
+            return  # handle NaN
+        norm = 1 / norm  # use reciprocal for division
+        ax *= norm
+        ay *= norm
+        az *= norm
+
+        # Normalise magnetometer measurement
+        norm = sqrt(mx * mx + my * my + mz * mz)
+        if norm == 0:
+            return  # handle NaN
+        norm = 1 / norm  # use reciprocal for division
+        mx *= norm
+        my *= norm
+        mz *= norm
+
+        # Reference direction of Earth's magnetic field
+        _2q1mx = 2 * q1 * mx
+        _2q1my = 2 * q1 * my
+        _2q1mz = 2 * q1 * mz
+        _2q2mx = 2 * q2 * mx
+        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4
+        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4
+        _2bx = sqrt(hx * hx + hy * hy)
+        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4
+        _4bx = 2 * _2bx
+        _4bz = 2 * _2bz
+
+        # Gradient descent algorithm corrective step
+        s1 = (-_2q3 * (2 * q2q4 - _2q1q3 - ax) + _2q2 * (2 * q1q2 + _2q3q4 - ay) - _2bz * q3 * (
+                _2bx * (0.5 - q3q3 - q4q4)
+                + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (
+                      _2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+              + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        s2 = (_2q4 * (2 * q2q4 - _2q1q3 - ax) + _2q1 * (2 * q1q2 + _2q3q4 - ay) - 4 * q2 * (
+                1 - 2 * q2q2 - 2 * q3q3 - az)
+              + _2bz * q4 * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (
+                      _2bx * (q2q3 - q1q4)
+                      + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (
+                      _2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        s3 = (-_2q1 * (2 * q2q4 - _2q1q3 - ax) + _2q4 * (2 * q1q2 + _2q3q4 - ay) - 4 * q3 * (
+                1 - 2 * q2q2 - 2 * q3q3 - az)
+              + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5 - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
+              + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+              + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        s4 = (_2q2 * (2 * q2q4 - _2q1q3 - ax) + _2q3 * (2 * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (
+                _2bx * (0.5 - q3q3 - q4q4)
+                + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (
+                      _2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my)
+              + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5 - q2q2 - q3q3) - mz))
+
+        norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)  # normalise step magnitude
+        s1 *= norm
+        s2 *= norm
+        s3 *= norm
+        s4 *= norm
+
+        # Compute rate of change of quaternion
+        qDot1 = 0.5 * (-q2 * gx - q3 * gy - q4 * gz) - self.beta * s1
+        qDot2 = 0.5 * (q1 * gx + q3 * gz - q4 * gy) - self.beta * s2
+        qDot3 = 0.5 * (q1 * gy - q2 * gz + q4 * gx) - self.beta * s3
+        qDot4 = 0.5 * (q1 * gz + q2 * gy - q3 * gx) - self.beta * s4
+
+        # Integrate to yield quaternion
+        deltat = elapsed_micros(self.start_time) / 1e6
+        self.start_time = micros()
+        q1 += qDot1 * deltat
+        q2 += qDot2 * deltat
+        q3 += qDot3 * deltat
+        q4 += qDot4 * deltat
+        norm = 1 / sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)  # normalise quaternion
+        self.q = q1 * norm, q2 * norm, q3 * norm, q4 * norm
+
+    async def update(self):
+        """Must call to get updated data
+
+        3-tuples (x, y, z) for accel, gyro and mag data
+
+        This should be called at a frequency between 10-50 Hz
+        """
+        while True:
+            if not SIMULATION:
+                self._update_data()
+
+            self.publish('ahrs.update', {
+                'heading': self.heading,
+                'roll': self.roll,
+                'pitch': self.pitch,
+            })
+
+            if SIMULATION:
+                await asyncio.sleep(0.1)
+            else:
+                await asyncio.sleep(0.01)