libupm: Add LSM303D support

BUG=none
Signed-off-by: Bruce Beare <bruce.j.beare@intel.com>
Change-Id: Ib0af179086e335715a12cf7f5dc9621af89caca9

6 files changed, 497 insertions, 0 deletions

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 <brendan.le.foll@intel.com>
+ * 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 <bruce.j.beare@intel.com>
+ *
+ * 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 <iostream>
+#include <string>
+#include <stdexcept>
+#include <unistd.h>
+#include <stdlib.h>
+
+#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<brendan.le.foll@intel.com>
+ * 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 <bruce.j.beare@intel.com>
+ *
+ * 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 <string.h>
+#include <mraa/i2c.hpp>
+#include <math.h>
+
+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"
+%}