stmhal, STM32F4DISC: Small changes to ST accel driver.

1 files changed, 27 insertions, 17 deletions

diff --git a/stmhal/boards/STM32F4DISC/staccel.py b/stmhal/boards/STM32F4DISC/staccel.py
index ce2646406a..eb36f0343c 100755..100644
--- a/stmhal/boards/STM32F4DISC/staccel.py
+++ b/stmhal/boards/STM32F4DISC/staccel.py
@@ -24,36 +24,38 @@ LIS302DL_WHO_AM_I_ADDR = const(0x0f)
 LIS302DL_WHO_AM_I_VAL = const(0x3b)
 LIS302DL_CTRL_REG1_ADDR = const(0x20)
 LIS302DL_OUT_X = const(0x29)
+LIS302DL_OUT_Y = const(0x2b)
+LIS302DL_OUT_Z = const(0x2d)
 # Configuration for 100Hz sampling rate, +-2g range
-LIS302DL_CONF = 0b01000111
+LIS302DL_CONF = const(0b01000111)
 
-def signed8(x):
+def convert_raw_to_g(x):
     if x & 0x80:
-        return x - 256
-    else:
-        return x
+        x = x - 256
+    return x * 18 / 1000
 
 class STAccel:
     def __init__(self):
         self.cs_pin = Pin('PE3', Pin.OUT_PP, Pin.PULL_NONE)
         self.cs_pin.high()
         self.spi = SPI(1, SPI.MASTER, baudrate=328125, polarity=0, phase=1, bits=8)
-        self.wr(LIS302DL_CTRL_REG1_ADDR, bytearray([LIS302DL_CONF]))
-        if self.read_id()[0] != LIS302DL_WHO_AM_I_VAL:
+        self.write_bytes(LIS302DL_CTRL_REG1_ADDR, bytearray([LIS302DL_CONF]))
+        if self.read_id() != LIS302DL_WHO_AM_I_VAL:
             raise Exception('LIS302DL accelerometer not present')
 
-    def rd(self, addr, nbytes):
+    def read_bytes(self, addr, nbytes):
         if nbytes > 1:
             addr |= READWRITE_CMD | MULTIPLEBYTE_CMD
         else:
             addr |= READWRITE_CMD
         self.cs_pin.low()
         self.spi.send(addr)
-        buf = self.spi.send_recv(bytearray(nbytes * [0])) # read data, MSB first
+        #buf = self.spi.send_recv(bytearray(nbytes * [0])) # read data, MSB first
+        buf = self.spi.recv(nbytes)
         self.cs_pin.high()
         return buf
 
-    def wr(self, addr, buf):
+    def write_bytes(self, addr, buf):
         if len(buf) > 1:
             addr |= MULTIPLEBYTE_CMD
         self.cs_pin.low()
@@ -63,11 +65,19 @@ class STAccel:
         self.cs_pin.high()
 
     def read_id(self):
-        return self.rd(LIS302DL_WHO_AM_I_ADDR, 1)
+        return self.read_bytes(LIS302DL_WHO_AM_I_ADDR, 1)[0]
 
-    def get_xyz(self):
-        val = self.rd(LIS302DL_OUT_X, 5)
-        x = signed8(val[0]) * 18.0 / 1000
-        y = signed8(val[2]) * 18.0 / 1000
-        z = signed8(val[4]) * 18.0 / 1000
-        return [x, y, z]
+    def x(self):
+        return convert_raw_to_g(self.read_bytes(LIS302DL_OUT_X, 1)[0])
+
+    def y(self):
+        return convert_raw_to_g(self.read_bytes(LIS302DL_OUT_Y, 1)[0])
+
+    def z(self):
+        return convert_raw_to_g(self.read_bytes(LIS302DL_OUT_Z, 1)[0])
+
+    def xyz(self):
+        val = self.read_bytes(LIS302DL_OUT_X, 5)
+        return [convert_raw_to_g(val[0]),
+                convert_raw_to_g(val[2]),
+                convert_raw_to_g(val[4])]