From 0844fd66fbf82c3c6e52fc4db25f9543d11e28b8 Mon Sep 17 00:00:00 2001
From: Jan Schmidt <jan@centricular.com>
Date: Fri, 19 Jan 2018 23:18:53 +1100
Subject: [PATCH] mpu9250: Basic accelerometer support

Initialise the accelerometer, and once per second
output the maximum / minimum reading. Needs hooking
up to MQTT and other stuff
---
 software/boot.py                             |  7 +-
 software/configuration/mpu9250.py            |  6 ++
 software/lib/mpu9250.py                      | 68 ++++++++++++++++++--
 software/scripts/copy_micropython_scripts.sh |  1 +
 4 files changed, 76 insertions(+), 6 deletions(-)
 create mode 100644 software/configuration/mpu9250.py

diff --git a/software/boot.py b/software/boot.py
index 5af9892..496574e 100644
--- a/software/boot.py
+++ b/software/boot.py
@@ -1,6 +1,6 @@
 # This file is executed on every boot (including wake-boot from deepsleep)
 #
-# boot.py: version: 2018-01-14 14:00
+# boot.py: version: 2018-01-20 00:45
 #
 # Usage
 # ~~~~~
@@ -28,6 +28,10 @@
 while not aiko.wifi.connect(configuration.wifi.ssids): time.sleep(0.5)
 aiko.led.set(aiko.led.colors["blue"], 0, True)
 
+import configuration.mpu9250
+import mpu9250
+mpu9250.initialise(configuration.mpu9250.settings, lolibot.i2c_bus)
+
 # import aiko.services
 # import configuration.services
 # aiko.services.initialise(configuration.services.settings)
@@ -45,4 +49,5 @@
 
 while True:
   aiko.mqtt.ping_check()        # TODO: Create a general timer handler
+  mpu9250.accel_check()
   aiko.mqtt.client.check_msg()  # Then make this a blocking call
diff --git a/software/configuration/mpu9250.py b/software/configuration/mpu9250.py
new file mode 100644
index 0000000..654cb52
--- /dev/null
+++ b/software/configuration/mpu9250.py
@@ -0,0 +1,6 @@
+# configuration/mpu9250.py: version: 2018-01-19 22:00
+
+settings = {
+  "i2c_addr": 0x68,
+  "accel_max_g": 2,
+}
diff --git a/software/lib/mpu9250.py b/software/lib/mpu9250.py
index 78ab972..936171e 100644
--- a/software/lib/mpu9250.py
+++ b/software/lib/mpu9250.py
@@ -1,5 +1,16 @@
+# mpu9250.py: version: 2018-01-20 00:45
 import struct
 import math
+import time
+
+mpu = None
+
+time_last_check = 0
+maxZ = -50.0
+minZ = 50.0
+
+class MPU9250Exception(Exception):
+    pass
 
 class MPU9250:
     """support for the MPU-9250"""
@@ -9,9 +20,18 @@ class MPU9250:
     FS_8G = 16
     FS_16G = 24
 
-    def __init__(self, bus, accel_max_g=2, addr=104):
+    def __init__(self, bus, accel_max_g=2, addr=0x68):
         self.bus = bus
         self.addr = addr
+
+        try:
+           b = self.bus.readfrom_mem(self.addr, 0x75, 1)
+        except OSError:
+           raise MPU9250Exception("mpu9250 not detected.")
+
+        if b[0] != 0x71:
+            raise MPU9250Exception("Device that is not mpu9250 detected. WhoAmI reg contained 0x{:x}".format(b[0]))
+
         if accel_max_g <= 2:
             accel_fs_sel = 0
             self.accel_scale = 16384
@@ -27,11 +47,49 @@ def __init__(self, bus, accel_max_g=2, addr=104):
         else:
             raise ValueError("accel_max_g must be <= 16")
 
-        self.bus.writeto_mem(self.addr, 107, bytes([0]))
-        self.bus.writeto_mem(self.addr, 28, bytes([accel_fs_sel]))
+        self.bus.writeto_mem(self.addr, 0x6b, bytes([0]))
+        self.bus.writeto_mem(self.addr, 0x1c, bytes([accel_fs_sel]))
+
+    def update(self):
+        # (ax, ay, az, _, gx, gy, gz) = self.readings
+        self.readings = struct.unpack(">7h", self.bus.readfrom_mem(self.addr,0x3b,14))
 
-    def read(self):
-        (ax, ay, az, _, gx, gy, gz) = struct.unpack(">7h", self.bus.readfrom_mem(104,0x3b,14))
+    def readZ(self):
+        # Returns a tuple of Z acceleration and Gyro reading
+        self.update()
+
+        az = self.readings[2]
+        gz = self.readings[6]
         #return math.sqrt(ax*ax + ay*ay + az*az) / self.accel_scale, math.sqrt(gx*gx + gy*gy + gz*gz)
         return az / self.accel_scale, gz
 
+def initialise(settings, i2c_bus):
+    global mpu
+
+    try:
+        mpu = MPU9250(i2c_bus, settings["accel_max_g"], settings["i2c_addr"])
+    except MPU9250Exception:
+        print("MPU9250 module not detected.")
+    print("MPU9250 initialised")
+
+def readZ():
+    if mpu:
+        return mpu.readZ()
+    return 0
+
+# FIXME: Replace with general timer implementation
+def accel_check():
+    global time_last_check, maxZ, minZ
+    time_now = time.ticks_ms()
+
+    # Track the max/min accelerometer reading between updates
+    # and output them once per second
+    curZ = readZ()
+    maxZ = max(curZ[0], maxZ)
+    minZ = min(curZ[0], minZ)
+
+    if time_now >= time_last_check + 1000:
+        time_last_check = time_now
+        print ("Accel Z {} min {} max {}".format(readZ(), minZ, maxZ))
+        minZ = 50.0
+        maxZ = -50.0
diff --git a/software/scripts/copy_micropython_scripts.sh b/software/scripts/copy_micropython_scripts.sh
index a9423aa..9d56032 100755
--- a/software/scripts/copy_micropython_scripts.sh
+++ b/software/scripts/copy_micropython_scripts.sh
@@ -23,6 +23,7 @@ done
 echo '### Copy configuration/*.py ###'
 ampy put configuration/led.py      configuration/led.py
 ampy put configuration/lolibot.py  configuration/lolibot.py
+ampy put configuration/mpu9250.py  configuration/mpu9250.py
 ampy put configuration/mqtt.py     configuration/mqtt.py
 ampy put configuration/services.py configuration/services.py
 ampy put configuration/wifi.py     configuration/wifi.py