From 2b4af54992e1a4f2a3e02d5b72045cde60196e84 Mon Sep 17 00:00:00 2001 From: Hirotaka Kawata Date: Thu, 11 Sep 2014 16:32:37 +0900 Subject: [PATCH] Add LIS3DSH accelometer support to staccel.py --- stmhal/boards/STM32F4DISC/staccel.py | 54 +++++++++++++++++----------- 1 file changed, 34 insertions(+), 20 deletions(-) diff --git a/stmhal/boards/STM32F4DISC/staccel.py b/stmhal/boards/STM32F4DISC/staccel.py index eb36f0343..7296662ca 100644 --- a/stmhal/boards/STM32F4DISC/staccel.py +++ b/stmhal/boards/STM32F4DISC/staccel.py @@ -1,7 +1,6 @@ """ Driver for accelerometer on STM32F4 Discover board. -Assumes it's a LIS302DL MEMS device. Sets accelerometer range at +-2g. Returns list containing X,Y,Z axis acceleration values in 'g' units (9.8m/s^2). @@ -20,28 +19,46 @@ from pyb import SPI READWRITE_CMD = const(0x80) MULTIPLEBYTE_CMD = const(0x40) -LIS302DL_WHO_AM_I_ADDR = const(0x0f) +WHO_AM_I_ADDR = const(0x0f) +OUT_X_ADDR = const(0x29) +OUT_Y_ADDR = const(0x2b) +OUT_Z_ADDR = const(0x2d) +OUT_T_ADDR = const(0x0c) + 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 = const(0b01000111) -def convert_raw_to_g(x): - if x & 0x80: - x = x - 256 - return x * 18 / 1000 +LIS3DSH_WHO_AM_I_VAL = const(0x3f) +LIS3DSH_CTRL_REG4_ADDR = const(0x20) +LIS3DSH_CTRL_REG5_ADDR = const(0x24) +# Configuration for 100Hz sampling rate, +-2g range +LIS3DSH_CTRL_REG4_CONF = const(0b01100111) +LIS3DSH_CTRL_REG5_CONF = const(0b00000000) 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.write_bytes(LIS302DL_CTRL_REG1_ADDR, bytearray([LIS302DL_CONF])) - if self.read_id() != LIS302DL_WHO_AM_I_VAL: - raise Exception('LIS302DL accelerometer not present') + + self.who_am_i = self.read_id() + + if self.who_am_i == LIS302DL_WHO_AM_I_VAL: + self.write_bytes(LIS302DL_CTRL_REG1_ADDR, bytearray([LIS302DL_CONF])) + self.sensitivity = 18 + elif self.who_am_i == LIS3DSH_WHO_AM_I_VAL: + self.write_bytes(LIS3DSH_CTRL_REG4_ADDR, bytearray([LIS3DSH_CTRL_REG4_CONF])) + self.write_bytes(LIS3DSH_CTRL_REG5_ADDR, bytearray([LIS3DSH_CTRL_REG5_CONF])) + self.sensitivity = 0.06 * 256 + else: + raise Exception('LIS302DL or LIS3DSH accelerometer not present') + + def convert_raw_to_g(self, x): + if x & 0x80: + x = x - 256 + return x * self.sensitivity / 1000 def read_bytes(self, addr, nbytes): if nbytes > 1: @@ -65,19 +82,16 @@ class STAccel: self.cs_pin.high() def read_id(self): - return self.read_bytes(LIS302DL_WHO_AM_I_ADDR, 1)[0] + return self.read_bytes(WHO_AM_I_ADDR, 1)[0] def x(self): - return convert_raw_to_g(self.read_bytes(LIS302DL_OUT_X, 1)[0]) + return self.convert_raw_to_g(self.read_bytes(OUT_X_ADDR, 1)[0]) def y(self): - return convert_raw_to_g(self.read_bytes(LIS302DL_OUT_Y, 1)[0]) + return self.convert_raw_to_g(self.read_bytes(OUT_Y_ADDR, 1)[0]) def z(self): - return convert_raw_to_g(self.read_bytes(LIS302DL_OUT_Z, 1)[0]) + return self.convert_raw_to_g(self.read_bytes(OUT_Z_ADDR, 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])] + return (self.x(), self.y(), self.z())