From bed76f3a0b3d112f4d61c79e6682e470604e1dfb Mon Sep 17 00:00:00 2001 From: Bruce Beare Date: Fri, 25 Dec 2015 11:59:51 -0800 Subject: libupm: Add LSM303D support BUG=none Signed-off-by: Bruce Beare Change-Id: Ib0af179086e335715a12cf7f5dc9621af89caca9 --- peripheral/libupm/src/lsm303d/CMakeLists.txt | 5 + peripheral/libupm/src/lsm303d/javaupm_lsm303d.i | 22 +++ peripheral/libupm/src/lsm303d/jsupm_lsm303d.i | 15 ++ peripheral/libupm/src/lsm303d/lsm303d.cxx | 213 ++++++++++++++++++++++ peripheral/libupm/src/lsm303d/lsm303d.h | 226 ++++++++++++++++++++++++ peripheral/libupm/src/lsm303d/pyupm_lsm303d.i | 16 ++ 6 files changed, 497 insertions(+) create mode 100644 peripheral/libupm/src/lsm303d/CMakeLists.txt create mode 100644 peripheral/libupm/src/lsm303d/javaupm_lsm303d.i create mode 100644 peripheral/libupm/src/lsm303d/jsupm_lsm303d.i create mode 100644 peripheral/libupm/src/lsm303d/lsm303d.cxx create mode 100644 peripheral/libupm/src/lsm303d/lsm303d.h create mode 100644 peripheral/libupm/src/lsm303d/pyupm_lsm303d.i diff --git a/peripheral/libupm/src/lsm303d/CMakeLists.txt b/peripheral/libupm/src/lsm303d/CMakeLists.txt new file mode 100644 index 0000000..7a7a58f --- /dev/null +++ b/peripheral/libupm/src/lsm303d/CMakeLists.txt @@ -0,0 +1,5 @@ +set (libname "lsm303d") +set (libdescription "upm lsm303d") +set (module_src ${libname}.cxx) +set (module_h ${libname}.h) +upm_module_init() diff --git a/peripheral/libupm/src/lsm303d/javaupm_lsm303d.i b/peripheral/libupm/src/lsm303d/javaupm_lsm303d.i new file mode 100644 index 0000000..7d938b8 --- /dev/null +++ b/peripheral/libupm/src/lsm303d/javaupm_lsm303d.i @@ -0,0 +1,22 @@ +%module javaupm_lsm303d +%include "../upm.i" + +%{ + #include "lsm303d.h" +%} + +%typemap(jni) int16_t* "jintArray" +%typemap(jstype) int16_t* "int[]" +%typemap(jtype) int16_t* "int[]" + +%typemap(javaout) int16_t* { + return $jnicall; +} + +%typemap(out) int16_t *getRawAccelData { + $result = JCALL1(NewIntArray, jenv, 3); + JCALL4(SetIntArrayRegion, jenv, $result, 0, 3, (const signed int*)$1); + //delete [] $1; +} + +%include "lsm303d.h" diff --git a/peripheral/libupm/src/lsm303d/jsupm_lsm303d.i b/peripheral/libupm/src/lsm303d/jsupm_lsm303d.i new file mode 100644 index 0000000..cfdb1d1 --- /dev/null +++ b/peripheral/libupm/src/lsm303d/jsupm_lsm303d.i @@ -0,0 +1,15 @@ +%module jsupm_lsm303d +%include "../upm.i" +%include "../carrays_int16_t.i" + +// Adding this typemap because SWIG is converting int16 into a short by default +// This forces SWIG to convert it correctly +%typemap(out) int16_t* { + jsresult = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_int16Array, 0 | 0 ); +} + +%{ + #include "lsm303d.h" +%} + +%include "lsm303d.h" diff --git a/peripheral/libupm/src/lsm303d/lsm303d.cxx b/peripheral/libupm/src/lsm303d/lsm303d.cxx new file mode 100644 index 0000000..ba9e303 --- /dev/null +++ b/peripheral/libupm/src/lsm303d/lsm303d.cxx @@ -0,0 +1,213 @@ +/* + * Author: Brendan Le Foll + * Copyright (c) 2014 Intel Corporation. + * + * Code based on LSM303DLH sample by Jim Lindblom SparkFun Electronics + * and the CompensatedCompass.ino by Frankie Chu from SeedStudio + * + * Further modifications to port to the LSM303d by + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include +#include +#include +#include +#include + +#include "lsm303d.h" + +using namespace upm; + +LSM303d::LSM303d(int bus, int addr, int accScale) : m_i2c(bus) +{ + m_addr = addr; + + uint8_t afs_bits = 0; // LM303D_SCALE_2G - see the data sheet + const uint8_t abw_bits = 3; // 50hz Anti-alias filter bandwidth - see the data sheet + + // 0x27 is the 'normal' mode with X/Y/Z enable + // Data is available at 100HZ. + // See the data sheet for higher data rates. + setRegisterSafe(m_addr, CTRL_REG1, 0x67); + + // scale can be 2, 4 or 8 + switch (accScale) { + case LM303D_SCALE_2G: + afs_bits = 0; + break; + case LM303D_SCALE_4G: + afs_bits = 1; + break; + case LM303D_SCALE_6G: + afs_bits = 2; + break; + case LM303D_SCALE_8G: + afs_bits = 3; + break; + case LM303D_SCALE_16G: + afs_bits = 4; + break; + default: + throw std::invalid_argument(std::string(__FUNCTION__) + + ": failed to specify scaling correctly"); + break; + } + setRegisterSafe(m_addr, CTRL_REG2, (abw_bits<<6)|(afs_bits<<3)); + + // Enable Mag. + const uint8_t mag_resolution_bits = 3 << 5; // high resolution + const uint8_t mag_data_rate_bits = 4 << 2; // 50 hz + const uint8_t mag_sensor_mode = 0; // continuous conversion + + setRegisterSafe(m_addr, CTRL_REG5, mag_resolution_bits|mag_data_rate_bits); + setRegisterSafe(m_addr, CTRL_REG7, mag_sensor_mode); +} + +float +LSM303d::getHeading() +{ + if (getCoordinates() != mraa::SUCCESS) { + return -1.0; + } + + float heading = 180.0 * atan2(double(coor[Y]), double(coor[X]))/M_PI; + + if (heading < 0.0) + heading += 360.0; + + return heading; +} + +int16_t* +LSM303d::getRawAccelData() +{ + return &accel[0]; +} + +int16_t* +LSM303d::getRawCoorData() +{ + return &coor[0]; +} + +int16_t +LSM303d::getAccelX() +{ + return accel[X]; +} + +int16_t +LSM303d::getAccelY() +{ + return accel[Y]; +} + +int16_t +LSM303d::getAccelZ() +{ + return accel[Z]; +} + +mraa::Result +LSM303d::getCoordinates() +{ + mraa::Result ret = mraa::SUCCESS; + uint8_t status = writeThenRead(STATUS_M); + + coor[X] = (int16_t(writeThenRead(OUT_X_H_M)) << 8) + | int16_t(writeThenRead(OUT_X_L_M)); + coor[Y] = (int16_t(writeThenRead(OUT_Y_H_M)) << 8) + | int16_t(writeThenRead(OUT_Y_L_M)); + coor[Z] = (int16_t(writeThenRead(OUT_Z_H_M)) << 8) + | int16_t(writeThenRead(OUT_Z_L_M)); + //printf("status=0x%x, X=%d, Y=%d, Z=%d\n", status, coor[X], coor[Y], coor[Z]); + + return ret; +} + + +int16_t +LSM303d::getCoorX() { + return coor[X]; +} + +int16_t +LSM303d::getCoorY() { + return coor[Y]; +} + +int16_t +LSM303d::getCoorZ() { + return coor[Z]; +} + +// helper function that writes a value to the acc and then reads +int +LSM303d::writeThenRead(uint8_t reg) +{ + m_i2c.address(m_addr); + m_i2c.writeByte(reg); + m_i2c.address(m_addr); + return (int) m_i2c.readByte(); +} + +mraa::Result +LSM303d::getAcceleration() +{ + mraa::Result ret = mraa::SUCCESS; + + accel[X] = (int16_t(writeThenRead(OUT_X_H_A)) << 8) + | int16_t(writeThenRead(OUT_X_L_A)); + accel[Y] = (int16_t(writeThenRead(OUT_Y_H_A)) << 8) + | int16_t(writeThenRead(OUT_Y_L_A)); + accel[Z] = (int16_t(writeThenRead(OUT_Z_H_A)) << 8) + | int16_t(writeThenRead(OUT_Z_L_A)); + //printf("X=%x, Y=%x, Z=%x\n", accel[X], accel[Y], accel[Z]); + + return ret; +} + +// helper function that sets a register and then checks the set was succesful +mraa::Result +LSM303d::setRegisterSafe(uint8_t slave, uint8_t sregister, uint8_t data) +{ + buf[0] = sregister; + buf[1] = data; + + if (m_i2c.address(slave) != mraa::SUCCESS) { + throw std::invalid_argument(std::string(__FUNCTION__) + + ": mraa_i2c_address() failed"); + return mraa::ERROR_INVALID_HANDLE; + } + if (m_i2c.write(buf, 2) != mraa::SUCCESS) { + throw std::invalid_argument(std::string(__FUNCTION__) + + ": mraa_i2c_write() failed"); + return mraa::ERROR_INVALID_HANDLE; + } + uint8_t val = m_i2c.readReg(sregister); + if (val != data) { + throw std::invalid_argument(std::string(__FUNCTION__) + + ": failed to set register correctly"); + return mraa::ERROR_UNSPECIFIED; + } + return mraa::SUCCESS; +} diff --git a/peripheral/libupm/src/lsm303d/lsm303d.h b/peripheral/libupm/src/lsm303d/lsm303d.h new file mode 100644 index 0000000..79d3204 --- /dev/null +++ b/peripheral/libupm/src/lsm303d/lsm303d.h @@ -0,0 +1,226 @@ +/* + * Author: Brendan Le Foll + * Copyright (c) 2014 Intel Corporation. + * + * Code based on LSM303DLH sample by Jim Lindblom SparkFun Electronics + * and the CompensatedCompass.ino by Frankie Chu from SeedStudio + * + * Further modifications to port to the LSM303d by + * + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the + * "Software"), to deal in the Software without restriction, including + * without limitation the rights to use, copy, modify, merge, publish, + * distribute, sublicense, and/or sell copies of the Software, and to + * permit persons to whom the Software is furnished to do so, subject to + * the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE + * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION + * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ +#pragma once + +#include +#include +#include + +namespace upm { +/** + * @brief LSM303d Accelerometer/Compass library + * @defgroup lsm303d libupm-lsm303d + * @ingroup seeed adafruit i2c accelerometer compass + */ + +/** + * @library lsm303d + * @sensor lsm303d + * @comname LSM303d Accelerometer & Compass + * @altname Grove 6-Axis Accelerometer & Compass + * @type accelerometer compass + * @man seeed adafruit + * @web http://www.seeedstudio.com/wiki/Grove_-_6-Axis_Accelerometer%26Compass + * @con i2c + * + * @brief API for the LSM303d Accelerometer & Compass + * + * This module defines the LSM303d 3-axis magnetometer/3-axis accelerometer. + * This module was tested with the Seeed Studio* Grove 6-Axis Accelerometer & Compass + * version 2.0 module used over I2C. The magnometer and acceleromter are accessed + * at two seperate I2C addresses. + * + * @image html lsm303d.jpeg + * @snippet lsm303d.cxx Interesting + */ +class LSM303d { + public: + + /* Address definitions for the grove 6DOF v2.0 */ + typedef enum { + LSM303d_ADDR = 0x1E + } GROVE_6DOF_ADDRS_T; + + typedef enum { + LM303D_SCALE_2G = 2, + LM303D_SCALE_4G = 4, + LM303D_SCALE_6G = 6, + LM303D_SCALE_8G = 8, + LM303D_SCALE_16G = 16 + } LSM303D_SCALE_T; + + typedef enum { + X = 0, + Y = 1, + Z = 2 + } XYZ_T; + + /** + * Instantiates an LSM303d object + * + * @param i2c bus + * @param addr Magnetometer + * @param addr Accelerometer + */ + LSM303d (int bus, + int addr=LSM303d_ADDR, + int accScale=LM303D_SCALE_4G); + + /** + * LSM303d object destructor + * where is no more need for this here - I2c connection will be stopped + * automatically when m_i2c variable will go out of scope + * ~LSM303d (); + **/ + + /** + * Gets the current heading; headings <0 indicate an error has occurred + * + * @return float + */ + float getHeading(); + + /** + * Gets the coordinates in the XYZ order + */ + mraa::Result getCoordinates(); + + /** + * Gets accelerometer values + * Should be called before other "get" functions for acceleration + */ + mraa::Result getAcceleration(); + + /** + * Gets raw coordinate data; it is updated when getCoordinates() is called + */ + int16_t* getRawCoorData(); + + /** + * Gets the X component of the coordinates data + */ + int16_t getCoorX(); + + /** + * Gets the Y component of the coordinates data + */ + int16_t getCoorY(); + + /** + * Gets the Z component of the coordinates data + */ + int16_t getCoorZ(); + + /** + * Gets raw accelerometer data; it is updated when getAcceleration() is called + */ + int16_t* getRawAccelData(); + + /** + * Gets the X component of the acceleration data + */ + int16_t getAccelX(); + + /** + * Gets the Y component of the acceleration data + */ + int16_t getAccelY(); + + /** + * Gets the Z component of the acceleration data + */ + int16_t getAccelZ(); + + private: + + /* LSM303d Register definitions */ + typedef enum { + STATUS_M = 0x7, + OUT_X_L_M = 0x8, + OUT_X_H_M = 0x9, + OUT_Y_L_M = 0xA, + OUT_Y_H_M = 0xB, + OUT_Z_L_M = 0xC, + OUT_Z_H_M = 0xD, + + CTRL_REG0 = 0x1f, + CTRL_REG1 = 0x20, + CTRL_REG2 = 0x21, + CTRL_REG3 = 0x22, + CTRL_REG4 = 0x23, + CTRL_REG5 = 0x24, + CTRL_REG6 = 0x25, + CTRL_REG7 = 0x26, + + STATUS_REGA = 0x27, + + OUT_X_L_A = 0x28, + OUT_X_H_A = 0x29, + OUT_Y_L_A = 0x2A, + OUT_Y_H_A = 0x2B, + OUT_Z_L_A = 0x2C, + OUT_Z_H_A = 0x2D, + + FIFO_CTRL = 0x2E, + FIFO_SRC = 0x2F, + + IG_CFG1 = 0x30, + IG_SRC1 = 0x31, + IG_THS1 = 0x32, + IG_DUR1 = 0x33, + + IG_CFG2 = 0x34, + IG_SRC2 = 0x35, + IG_THS2 = 0x36, + IG_DUR2 = 0x37, + + CLICK_CFG = 0x38, + CLICK_SRC = 0x39, + CLICK_THS = 0x3A, + + TIME_LIMIT = 0x3B, + TIME_LATEN = 0x3C, + TIME_WINDO = 0x3D, + + ACT_THS = 0x3E, + ACT_DUR = 0x3F, + } LSM303d_REGS_T; + + int writeThenRead(uint8_t reg); + mraa::Result setRegisterSafe(uint8_t slave, uint8_t sregister, uint8_t data); + + mraa::I2c m_i2c; + int m_addr; + + uint8_t buf[6]; + int16_t coor[3]; + int16_t accel[3]; +}; + +} diff --git a/peripheral/libupm/src/lsm303d/pyupm_lsm303d.i b/peripheral/libupm/src/lsm303d/pyupm_lsm303d.i new file mode 100644 index 0000000..92ec782 --- /dev/null +++ b/peripheral/libupm/src/lsm303d/pyupm_lsm303d.i @@ -0,0 +1,16 @@ +%module pyupm_lsm303d +%include "../upm.i" +%include "../carrays_int16_t.i" + +%feature("autodoc", "3"); + +// Adding this typemap because SWIG is converting int16 into a short by default +// This forces SWIG to convert it correctly +%typemap(out) int16_t* { + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_int16Array, 0 | 0 ); +} + +%include "lsm303d.h" +%{ + #include "lsm303d.h" +%} -- cgit v1.2.3