diff --git a/examples/c/CMakeLists.txt b/examples/c/CMakeLists.txt
index b9bea28a..a7382cee 100644
--- a/examples/c/CMakeLists.txt
+++ b/examples/c/CMakeLists.txt
@@ -122,6 +122,7 @@ add_example (sht1x)
 add_example (water)
 add_example (yg1006)
 add_example (biss0001)
+add_example (bmi160)
 
 # Custom examples
 add_custom_example (nmea_gps_i2c-example-c nmea_gps_i2c.c nmea_gps)
diff --git a/examples/c/bmi160.c b/examples/c/bmi160.c
new file mode 100644
index 00000000..6dace688
--- /dev/null
+++ b/examples/c/bmi160.c
@@ -0,0 +1,94 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * 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 <unistd.h>
+#include <signal.h>
+
+#include "bmi160.h"
+#include "upm_utilities.h"
+
+bool shouldRun = true;
+
+void sig_handler(int signo)
+{
+  if (signo == SIGINT)
+    shouldRun = false;
+}
+
+int main()
+{
+    signal(SIGINT, sig_handler);
+
+    //! [Interesting]
+
+    // Instantiate a BMI160 sensor on I2C bus 0, address 0x69, and
+    // disable the magnetometer (since it is an optional component
+    // that may not be present).  Since we are using I2C, pass a valid
+    // I2C address, and supply -1 as the cs_pin.
+    bmi160_context sensor = bmi160_init(0, 0x69, -1, false);
+
+    // For SPI, you would use something like this, using gpio 10 as
+    // the cs pin.  Passing -1 as the address indicates SPI operation.
+    //
+//    bmi160_context sensor = bmi160_init(0, -1, 10, false);
+
+    if (!sensor)
+    {
+        printf("bmi160_init() failed.\n");
+        return(1);
+    }
+
+    // Every half second, update and print values
+
+    while (shouldRun)
+    {
+        bmi160_update(sensor);
+
+        float x, y, z;
+
+        bmi160_get_accelerometer(sensor, &x, &y, &z);
+        printf("Acceleration x = %f y = %f z = %f\n",
+               x, y, z);
+
+        bmi160_get_gyroscope(sensor, &x, &y, &z);
+        printf("Gyroscope    x = %f y = %f z = %f\n",
+               x, y, z);
+
+        //bmi160_get_magnetometer(sensor, &x, &y, &z);
+        //printf("Magnetometer x = %f y = %f z = %f\n",
+        //       x, y, z);
+
+        printf("\n");
+
+        upm_delay_ms(500);
+    }
+
+    //! [Interesting]
+
+    printf("Exiting...\n");
+
+    bmi160_close(sensor);
+
+    return 0;
+}
diff --git a/src/bmi160/CMakeLists.txt b/src/bmi160/CMakeLists.txt
index 42868519..52bddad1 100644
--- a/src/bmi160/CMakeLists.txt
+++ b/src/bmi160/CMakeLists.txt
@@ -1,5 +1,9 @@
-set (libname "bmi160")
-set (libdescription "Bosch BMI160 Accelerometer, Gyroscope and BMM150 Magnetometer")
-set (module_src ${libname}.cxx bosch_bmi160.c)
-set (module_hpp ${libname}.hpp)
-upm_module_init()
+upm_mixed_module_init (NAME bmi160
+    DESCRIPTION "UPM driver for the BMI160 accel/gyro/mag"
+    C_HDR bmi160.h bosch_bmi160.h
+    C_SRC bmi160.c bosch_bmi160.c
+    CPP_HDR bmi160.hpp
+    CPP_SRC bmi160.cxx
+#    FTI_SRC bmi160_fti.c
+    CPP_WRAPS_C
+    REQUIRES mraa)
diff --git a/src/bmi160/bmi160.c b/src/bmi160/bmi160.c
new file mode 100644
index 00000000..8e2b86aa
--- /dev/null
+++ b/src/bmi160/bmi160.c
@@ -0,0 +1,595 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * 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 <unistd.h>
+#include <string.h>
+#include <assert.h>
+#include <mraa/i2c.h>
+#include <mraa/spi.h>
+#include <mraa/gpio.h>
+
+#include "bmi160.h"
+
+#include <upm_utilities.h>
+
+// we have to do it the old skool way.  Note, this also means that
+// only one instance of the bmi160 driver can be active at a time.
+static mraa_i2c_context i2cContext = NULL;
+static mraa_spi_context spiContext = NULL;
+/* this is used for chip-select when using SPI */
+static mraa_gpio_context gpioContext = NULL;
+
+// whether we are doing I2C or SPI
+static bool isSPI = false;
+
+// Our bmi160 info structure
+struct bmi160_t s_bmi160;
+
+// For SPI, these are our CS on/off functions, if needed
+static void bmi160_cs_on()
+{
+    if (gpioContext)
+        mraa_gpio_write(gpioContext, 0);
+}
+
+static void bmi160_cs_off()
+{
+    if (gpioContext)
+        mraa_gpio_write(gpioContext, 1);
+}
+
+// i2c bus read and write functions for use with the bmi driver code
+s8 bmi160_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
+{
+    printf("%s: ENTER reg %02x cnt %02x.\n", __FUNCTION__,
+           reg_addr, cnt);
+
+    if (isSPI)
+    {
+        if (!spiContext)
+        {
+            printf("%s: spiContext is NULL.\n", __FUNCTION__);
+            return 1;
+        }
+
+        reg_addr |= 0x80; // needed for read
+
+        uint8_t sbuf[cnt + 1];
+        memset((char *)sbuf, 0, cnt + 1);
+        sbuf[0] = reg_addr;
+
+        bmi160_cs_on();
+
+        if (mraa_spi_transfer_buf(spiContext, sbuf, sbuf, cnt + 1))
+        {
+            bmi160_cs_off();
+            printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
+            return 1;
+        }
+        bmi160_cs_off();
+
+      // now copy it into user buffer
+        int i;
+        for (i=0; i<cnt; i++)
+            reg_data[i] = sbuf[i + 1];
+
+        return 0;
+    }
+
+    // doing I2C
+
+    if (!i2cContext)
+    {
+        printf("%s: i2cContext is NULL.\n", __FUNCTION__);
+        return 1;
+    }
+
+    if (mraa_i2c_read_bytes_data(i2cContext, reg_addr, reg_data, cnt) < 0)
+    {
+        printf("%s: mraa_i2c_read_bytes() failed.\n", __FUNCTION__);
+        return 1;
+    }
+
+    return 0;
+}
+
+s8 bmi160_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
+{
+    printf("%s: ENTER reg %02x cnt %02x.\n", __FUNCTION__,
+           reg_addr, cnt);
+
+    if (isSPI)
+    {
+        if (!spiContext)
+        {
+            printf("%s: spiContext is NULL.\n", __FUNCTION__);
+            return 1;
+        }
+
+        reg_addr &= 0x7f; // mask off 0x80 for writing
+
+        uint8_t sbuf[cnt + 1];
+        memset((char *)sbuf, 0, cnt + 1);
+        sbuf[0] = reg_addr;
+
+        // copy in the data to write...
+        int i;
+        for (i=0; i<cnt; i++)
+            sbuf[i + 1] = reg_data[i];
+
+        bmi160_cs_on();
+
+        if (mraa_spi_transfer_buf(spiContext, sbuf, sbuf, cnt + 1))
+        {
+            bmi160_cs_off();
+            printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
+            return 1;
+        }
+        bmi160_cs_off();
+
+        return 0;
+    }
+
+    // I2C...
+    if (!i2cContext)
+    {
+        printf("%s: i2cContext is NULL.\n", __FUNCTION__);
+        return 1;
+    }
+
+    uint8_t buffer[cnt + 1];
+
+    buffer[0] = reg_addr;
+    int i;
+    for (i=0; i<cnt; i++)
+        buffer[i+1] = reg_data[i];
+
+    mraa_result_t rv = mraa_i2c_write(i2cContext, buffer, cnt+1);
+
+    if (rv != MRAA_SUCCESS)
+    {
+        printf("%s: mraa_i2c_write() failed.\n", __FUNCTION__);
+        return 1;
+    }
+
+    return 0;
+}
+
+// delay for some milliseconds
+void bmi160_delay_ms(u32 msek)
+{
+  upm_delay_ms(msek);
+}
+
+
+bmi160_context bmi160_init(unsigned int bus, int address, int cs_pin,
+                           bool enable_mag)
+{
+    bmi160_context dev =
+        (bmi160_context)malloc(sizeof(struct _bmi160_context));
+
+    if (!dev)
+        return NULL;
+
+    // zero out context
+    memset((void *)dev, 0, sizeof(struct _bmi160_context));
+
+    // make sure MRAA is initialized
+    int mraa_rv;
+    if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
+    {
+        printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
+        bmi160_close(dev);
+        return NULL;
+    }
+
+    if (address > 0)
+    {
+        // we are doing I2C
+        isSPI = false;
+
+        if (!(i2cContext = mraa_i2c_init(bus)))
+        {
+            printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__);
+            bmi160_close(dev);
+            return NULL;
+        }
+
+        if (mraa_i2c_address(i2cContext, address) != MRAA_SUCCESS)
+        {
+            printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__);
+            bmi160_close(dev);
+            return NULL;
+        }
+    }
+    else
+    {
+        // we are doing SPI
+        isSPI = true;
+        printf("DOING SPI!\n");//FIXME
+        if (!(spiContext = mraa_spi_init(bus)))
+        {
+            printf("%s: mraa_spi_init() failed.\n", __FUNCTION__);
+            bmi160_close(dev);
+            return NULL;
+        }
+
+        // Only create cs context if we are actually using a valid pin.
+        // A hardware controlled pin should specify cs as -1.
+        if (cs_pin >= 0)
+        {
+            if (!(gpioContext = mraa_gpio_init(cs_pin)))
+            {
+                printf("%s: mraa_gpio_init() failed.\n", __FUNCTION__);
+                bmi160_close(dev);
+                return NULL;
+            }
+
+            mraa_gpio_dir(gpioContext, MRAA_GPIO_OUT);
+            bmi160_cs_off();
+        }
+
+        if (mraa_spi_mode(spiContext, MRAA_SPI_MODE0))
+        {
+            printf("%s: mraa_spi_mode() failed.\n", __FUNCTION__);
+            bmi160_close(dev);
+            return NULL;
+        }
+
+        if (mraa_spi_frequency(spiContext, 5000000))
+        {
+            printf("%s: mraa_spi_frequency() failed.\n", __FUNCTION__);
+            bmi160_close(dev);
+            return NULL;
+        }
+    }
+
+    // init the driver interface functions
+    s_bmi160.bus_write = bmi160_bus_write;
+    s_bmi160.bus_read = bmi160_bus_read;
+    s_bmi160.delay_msec = bmi160_delay_ms;
+    if (isSPI)
+        s_bmi160.dev_addr = 0;
+    else
+        s_bmi160.dev_addr = address & 0xff;
+
+    // Init our driver interface pointers
+    if (bmi160_init_bus(&s_bmi160))
+    {
+        printf("%s: bmi160_bus_init() failed.\n", __FUNCTION__);
+        bmi160_close(dev);
+        return NULL;
+    }
+
+    // bmi160_init_bus will read the chip Id and deposit into our
+    // interface struct.  So, check it out and make sure it's correct.
+    printf("%s: CHIP_ID = %02x\n", __FUNCTION__, s_bmi160.chip_id);
+    if (s_bmi160.chip_id != BMI160_CHIP_ID)
+    {
+        printf("%s: Error: expected chip id %02x, but got %02x.\n",
+               __FUNCTION__, BMI160_CHIP_ID, s_bmi160.chip_id);
+        bmi160_close(dev);
+        return NULL;
+    }
+
+    dev->accelScale = 1.0;
+    dev->gyroScale = 1.0;
+    dev->magEnabled = false;
+
+    // This should be interesting...
+    const u32 C_BMI160_THIRTY_U8X = 30;
+
+    bmi160_enable_magnetometer(dev, enable_mag);
+
+    /* Set the accel mode as Normal write in the register 0x7E */
+    bmi160_set_command_register(ACCEL_MODE_NORMAL);
+
+    /* bmi160_delay_ms in ms */
+    bmi160_delay_ms(C_BMI160_THIRTY_U8X);
+
+    /* Set the gyro mode as Normal write in the register 0x7E */
+    bmi160_set_command_register(GYRO_MODE_NORMAL);
+
+    /* bmi160_delay_ms in ms */
+    bmi160_delay_ms(C_BMI160_THIRTY_U8X);
+
+    /* Set the accel bandwidth as OSRS4 */
+    bmi160_set_accel_bw(BMI160_ACCEL_OSR4_AVG1);
+    bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+
+    /* Set the gryo bandwidth as Normal */
+    bmi160_set_gyro_bw(BMI160_GYRO_NORMAL_MODE);
+    bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+
+    /* set gyro data rate as 200Hz */
+    bmi160_set_gyro_output_data_rate(BMI160_GYRO_OUTPUT_DATA_RATE_200HZ);
+    bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+
+    /* set accel data rate as 200Hz */
+    bmi160_set_accel_output_data_rate(BMI160_ACCEL_OUTPUT_DATA_RATE_200HZ,
+                                      BMI160_ACCEL_OSR4_AVG1);
+    bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+
+    bmi160_set_accelerometer_scale(dev, BMI160_ACC_RANGE_2G);
+    bmi160_set_gyroscope_scale(dev, BMI160_GYRO_RANGE_125);
+
+    return dev;
+}
+
+void bmi160_close(bmi160_context dev)
+{
+    assert(dev != NULL);
+
+    if (i2cContext)
+        mraa_i2c_stop(i2cContext);
+    i2cContext = NULL;
+
+    if (spiContext)
+        mraa_spi_stop(spiContext);
+    spiContext = NULL;
+
+    if (gpioContext)
+        mraa_gpio_close(gpioContext);
+    gpioContext = NULL;
+
+    free(dev);
+}
+
+void bmi160_update(const bmi160_context dev)
+{
+    assert(dev != NULL);
+
+    struct bmi160_gyro_t gyroxyz;
+    struct bmi160_accel_t accelxyz;
+    struct bmi160_mag_xyz_s32_t magxyz;
+
+    // read gyro data
+    bmi160_read_gyro_xyz(&gyroxyz);
+
+    // read accel data
+    bmi160_read_accel_xyz(&accelxyz);
+
+    // read mag data
+    if (dev->magEnabled)
+        bmi160_bmm150_mag_compensate_xyz(&magxyz);
+
+    // read the sensor time
+    u32 v_sensor_time;
+    bmi160_get_sensor_time(&v_sensor_time);
+    dev->sensorTime = (unsigned int)v_sensor_time;
+
+    dev->accelX = (float)accelxyz.x;
+    dev->accelY = (float)accelxyz.y;
+    dev->accelZ = (float)accelxyz.z;
+
+    dev->gyroX = (float)gyroxyz.x;
+    dev->gyroY = (float)gyroxyz.y;
+    dev->gyroZ = (float)gyroxyz.z;
+
+    if (dev->magEnabled)
+    {
+        dev->magX = (float)magxyz.x;
+        dev->magY = (float)magxyz.y;
+        dev->magZ = (float)magxyz.z;
+    }
+}
+
+void bmi160_set_accelerometer_scale(const bmi160_context dev,
+                                    BMI160_ACC_RANGE_T scale)
+{
+    assert(dev != NULL);
+
+    s8 v_range = BMI160_ACCEL_RANGE_2G;
+    // store scaling factor
+
+    switch (scale)
+    {
+    case BMI160_ACC_RANGE_2G:
+        v_range = BMI160_ACCEL_RANGE_2G;
+        dev->accelScale = 16384.0;
+        break;
+
+    case BMI160_ACC_RANGE_4G:
+        v_range = BMI160_ACCEL_RANGE_4G;
+        dev->accelScale = 8192.0;
+        break;
+
+    case BMI160_ACC_RANGE_8G:
+        v_range = BMI160_ACCEL_RANGE_8G;
+        dev->accelScale = 4096.0;
+        break;
+
+    case BMI160_ACC_RANGE_16G:
+        v_range = BMI160_ACCEL_RANGE_16G;
+        dev->accelScale = 2048.0;
+        break;
+
+    default: // should never occur, but...
+        dev->accelScale = 1.0;        // set a safe, though incorrect value
+        printf("%s: internal error, unsupported scale.\n", __FUNCTION__);
+        break;
+    }
+
+    bmi160_set_accel_range(v_range);
+
+    return;
+}
+
+void bmi160_set_gyroscope_scale(const bmi160_context dev,
+                                BMI160_GYRO_RANGE_T scale)
+{
+    assert(dev != NULL);
+
+    u8 v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
+
+    // store scaling factor
+
+    switch (scale)
+    {
+    case BMI160_GYRO_RANGE_125:
+        v_range = BMI160_GYRO_RANGE_125_DEG_SEC;
+        dev->gyroScale = 262.4;
+        break;
+
+    case BMI160_GYRO_RANGE_250:
+        v_range = BMI160_GYRO_RANGE_250_DEG_SEC;
+        dev->gyroScale = 131.2;
+        break;
+
+    case BMI160_GYRO_RANGE_500:
+        v_range = BMI160_GYRO_RANGE_500_DEG_SEC;
+        dev->gyroScale = 65.6;
+        break;
+
+    case BMI160_GYRO_RANGE_1000:
+        v_range = BMI160_GYRO_RANGE_1000_DEG_SEC;
+        dev->gyroScale = 32.8;
+        break;
+
+    case BMI160_GYRO_RANGE_2000:
+        v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
+        dev->gyroScale = 16.4;
+        break;
+
+    default: // should never occur, but...
+        dev->gyroScale = 1.0;        // set a safe, though incorrect value
+        printf("%s: internal error, unsupported scale.\n", __FUNCTION__);
+        break;
+    }
+
+    bmi160_set_gyro_range(v_range);
+
+    return;
+}
+
+void bmi160_get_accelerometer(const bmi160_context dev, float *x, float *y,
+                              float *z)
+{
+    assert(dev != NULL);
+
+    if (x)
+        *x = dev->accelX / dev->accelScale;
+
+    if (y)
+        *y = dev->accelY / dev->accelScale;
+
+    if (z)
+        *z = dev->accelZ / dev->accelScale;
+}
+
+void bmi160_get_gyroscope(const bmi160_context dev, float *x, float *y,
+                          float *z)
+{
+    assert(dev != NULL);
+
+    if (x)
+        *x = dev->gyroX / dev->gyroScale;
+
+    if (y)
+        *y = dev->gyroY / dev->gyroScale;
+
+    if (z)
+        *z = dev->gyroZ / dev->gyroScale;
+}
+
+void bmi160_get_magnetometer(const bmi160_context dev, float *x, float *y,
+                             float *z)
+{
+    assert(dev != NULL);
+
+    if (x)
+        *x = dev->magX;
+
+    if (y)
+        *y = dev->magY;
+
+    if (z)
+        *z = dev->magZ;
+}
+
+#if 0
+float *bmi160_get_ccelerometer()
+{
+    float *values = new float[3]; // x, y, and then z
+
+    getAccelerometer(&values[0], &values[1], &values[2]);
+
+    return values;
+}
+
+float *bmi160_getGyroscope()
+{
+    float *values = new float[3]; // x, y, and then z
+
+    getGyroscope(&values[0], &values[1], &values[2]);
+
+    return values;
+}
+
+float *bmi160_getMagnetometer()
+{
+    float *values = new float[3]; // x, y, and then z
+
+    getMagnetometer(&values[0], &values[1], &values[2]);
+
+    return values;
+}
+#endif
+
+void bmi160_enable_magnetometer(const bmi160_context dev, bool enable)
+{
+    assert(dev != NULL);
+
+    // butchered from support example
+    if (!enable)
+    {
+        bmi160_set_bmm150_mag_and_secondary_if_power_mode(MAG_SUSPEND_MODE);
+        bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+        bmi160_set_if_mode(0x00);
+        bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+
+        dev->magEnabled = false;
+        dev->magX = 0;
+        dev->magY = 0;
+        dev->magZ = 0;
+    }
+    else
+    {
+        u8 v_bmm_chip_id_u8 = BMI160_INIT_VALUE;
+        /* Init the magnetometer */
+        bmi160_bmm150_mag_interface_init(&v_bmm_chip_id_u8);
+
+        /* bmi160_delay_ms in ms*/
+        bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
+
+        dev->magEnabled = true;
+    }
+}
+
+unsigned int bmi160_get_time(const bmi160_context dev)
+{
+    assert(dev != NULL);
+
+    return dev->sensorTime;
+}
diff --git a/src/bmi160/bmi160.cxx b/src/bmi160/bmi160.cxx
index d9f35b77..79426e8e 100644
--- a/src/bmi160/bmi160.cxx
+++ b/src/bmi160/bmi160.cxx
@@ -27,354 +27,57 @@
 #include <stdexcept>
 #include <string>
 
-// we have to do it the old skool way
-#include <mraa/i2c.h>
-
 #include "bmi160.hpp"
 
-extern "C" {
-#include "bosch_bmi160.h"
-}
-
-// We do not need this define anyway.  It conflicts with mraa::SUCCESS.
-#undef SUCCESS
-
 using namespace upm;
 using namespace std;
 
-static mraa_i2c_context i2cContext = NULL;
-
-// Our bmi160 info structure
-struct bmi160_t s_bmi160;
-
-// bus read and write functions for use with the bmi driver code
-s8 bmi160_i2c_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
+BMI160::BMI160(int bus, int address, int csPin, bool enableMag) :
+    m_bmi160(bmi160_init(bus, address, csPin, enableMag))
 {
-  if (!i2cContext)
-    {
-      throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": i2c context is NULL");
-    }
-
-  int retries = 10;
-
-  // There seems to be some occasional flakyness with reads when
-  // moving the sensor around
-  while (retries >= 0)
-    {
-      int rv = mraa_i2c_read_bytes_data(i2cContext, reg_addr, reg_data, cnt);
-
-      if (rv < 0)
-        {
-          usleep(100000);
-          retries--;
-        }
-      else
-        return 0;
-    }
-
-  throw std::runtime_error(std::string(__FUNCTION__) +
-                           ": mraa_i2c_read_bytes_data() failed");
-
-  return 0;
-}
-
-s8 bmi160_i2c_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt)
-{
-  if (!i2cContext)
-    {
-      throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": i2c context is NULL");
-    }
-
-  // FIXME  fprintf(stderr, "%s: %02x: cnt %d\n", __FUNCTION__, reg_addr, cnt);
-  uint8_t buffer[cnt + 1];
-
-  buffer[0] = reg_addr;
-  for (int i=0; i<cnt; i++)
-    buffer[i+1] = reg_data[i];
-
-  mraa_result_t rv = mraa_i2c_write(i2cContext, buffer, cnt+1);
-
-  if (rv != MRAA_SUCCESS)
-    {
-      throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": mraa_i2c_write() failed");
-    }
-
-  return 0;
-}
-
-// delay for some milliseconds
-void bmi160_delay_ms(u32 msek)
-{
-  usleep(msek * 1000);
-}
-
-
-BMI160::BMI160(int bus, uint8_t address)
-{
-  m_addr = address;
-
-  // We need to use the C MRAA interface to avoid issue with C++ <-> C
-  // calling convention issues, also we need a global
-  // mraa_i2c_context
-
-  if (!(i2cContext = mraa_i2c_init(bus)))
-    {
-      throw std::invalid_argument(std::string(__FUNCTION__) +
-                                  ": mraa_i2c_init() failed");
-    }
-
-  if (mraa_i2c_address(i2cContext, m_addr) != MRAA_SUCCESS)
-    {
-      throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": mraa_i2c_address() failed");
-      return;
-    }
-
-  // init the driver interface functions
-  s_bmi160.bus_write = bmi160_i2c_bus_write;
-  s_bmi160.bus_read = bmi160_i2c_bus_read;
-  s_bmi160.delay_msec = bmi160_delay_ms;
-  s_bmi160.dev_addr = m_addr;
-
-  // Init our driver interface pointers
-  bmi160_init(&s_bmi160);
-
-  m_accelX = 0.0;
-  m_accelY = 0.0;
-  m_accelZ = 0.0;
-
-  m_gyroX = 0.0;
-  m_gyroY = 0.0;
-  m_gyroZ = 0.0;
-
-  m_magX = 0.0;
-  m_magY = 0.0;
-  m_magZ = 0.0;
-
-  m_accelScale = 1.0;
-  m_gyroScale = 1.0;
-
-  m_magEnabled = false;
-
-  if (!init())
-    {
-      throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": init() failed");
-    }
+    if (!m_bmi160)
+        throw std::runtime_error(string(__FUNCTION__)
+                                 + ": bmi160_init() failed");
 }
 
 BMI160::~BMI160()
 {
-  mraa_i2c_stop(i2cContext);
-  i2cContext = NULL;
+    bmi160_close(m_bmi160);
 }
 
-bool BMI160::init()
-{
-  // This should be interesting...
-  const u32 C_BMI160_THIRTY_U8X = 30;
-
-  enableMagnetometer(true);
-
-  /*Set the accel mode as Normal write in the register 0x7E*/
-  bmi160_set_command_register(ACCEL_MODE_NORMAL);
-
-  /* bmi160_delay_ms in ms*/
-  bmi160_delay_ms(C_BMI160_THIRTY_U8X);
-
-  /*Set the gyro mode as Normal write in the register 0x7E*/
-  bmi160_set_command_register(GYRO_MODE_NORMAL);
-
-  /* bmi160_delay_ms in ms*/
-  bmi160_delay_ms(C_BMI160_THIRTY_U8X);
-
-  /* Set the accel bandwidth as OSRS4 */
-  bmi160_set_accel_bw(BMI160_ACCEL_OSR4_AVG1);
-  bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-
-  /* Set the gryo bandwidth as Normal */
-  bmi160_set_gyro_bw(BMI160_GYRO_NORMAL_MODE);
-  bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-
-  /* set gyro data rate as 200Hz*/
-  bmi160_set_gyro_output_data_rate(BMI160_GYRO_OUTPUT_DATA_RATE_200HZ);
-  bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-
-  /* set accel data rate as 200Hz*/
-  bmi160_set_accel_output_data_rate(BMI160_ACCEL_OUTPUT_DATA_RATE_200HZ,
-                                    BMI160_ACCEL_OSR4_AVG1);
-  bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-
-  setAccelerometerScale(ACCEL_RANGE_2G);
-  setGyroscopeScale(GYRO_RANGE_125);
-
-  return true;
-}
-
-
 void BMI160::update()
 {
-  struct bmi160_gyro_t gyroxyz;
-  struct bmi160_accel_t accelxyz;
-  struct bmi160_mag_xyz_s32_t magxyz;
-
-  // read gyro data
-  bmi160_read_gyro_xyz(&gyroxyz);
-
-  // read accel data
-  bmi160_read_accel_xyz(&accelxyz);
-
-  // read mag data
-  if (m_magEnabled)
-    bmi160_bmm150_mag_compensate_xyz(&magxyz);
-
-  // read the sensor time
-  u32 v_sensor_time;
-  bmi160_get_sensor_time(&v_sensor_time);
-  m_sensorTime = (unsigned int)v_sensor_time;
-
-  m_accelX = float(accelxyz.x);
-  m_accelY = float(accelxyz.y);
-  m_accelZ = float(accelxyz.z);
-
-  m_gyroX = float(gyroxyz.x);
-  m_gyroY = float(gyroxyz.y);
-  m_gyroZ = float(gyroxyz.z);
-
-  if (m_magEnabled)
-    {
-      m_magX = float(magxyz.x);
-      m_magY = float(magxyz.y);
-      m_magZ = float(magxyz.z);
-    }
+    bmi160_update(m_bmi160);
 }
 
-void BMI160::setAccelerometerScale(ACCEL_RANGE_T scale)
+void BMI160::setAccelerometerScale(BMI160_ACC_RANGE_T scale)
 {
-  s8 v_range = BMI160_ACCEL_RANGE_2G;
-  // store scaling factor
-
-  switch (scale)
-    {
-    case ACCEL_RANGE_2G:
-      v_range = BMI160_ACCEL_RANGE_2G;
-      m_accelScale = 16384.0;
-      break;
-
-    case ACCEL_RANGE_4G:
-      v_range = BMI160_ACCEL_RANGE_4G;
-      m_accelScale = 8192.0;
-      break;
-
-    case ACCEL_RANGE_8G:
-      v_range = BMI160_ACCEL_RANGE_8G;
-      m_accelScale = 4096.0;
-      break;
-
-    case ACCEL_RANGE_16G:
-      v_range = BMI160_ACCEL_RANGE_16G;
-      m_accelScale = 2048.0;
-      break;
-
-    default: // should never occur, but...
-      m_accelScale = 1.0;        // set a safe, though incorrect value
-      throw std::logic_error(string(__FUNCTION__) +
-                             ": internal error, unsupported scale");
-      break;
-    }
-
-  bmi160_set_accel_range(v_range);
-
-  return;
+    bmi160_set_accelerometer_scale(m_bmi160, scale);
 }
 
-void BMI160::setGyroscopeScale(GYRO_RANGE_T scale)
+void BMI160::setGyroscopeScale(BMI160_GYRO_RANGE_T scale)
 {
-  u8 v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
-
-  // store scaling factor
-
-  switch (scale)
-    {
-    case GYRO_RANGE_125:
-      v_range = BMI160_GYRO_RANGE_125_DEG_SEC;
-      m_gyroScale = 262.4;
-      break;
-
-    case GYRO_RANGE_250:
-      v_range = BMI160_GYRO_RANGE_250_DEG_SEC;
-      m_gyroScale = 131.2;
-      break;
-
-    case GYRO_RANGE_500:
-      v_range = BMI160_GYRO_RANGE_500_DEG_SEC;
-      m_gyroScale = 65.6;
-      break;
-
-    case GYRO_RANGE_1000:
-      v_range = BMI160_GYRO_RANGE_1000_DEG_SEC;
-      m_gyroScale = 32.8;
-      break;
-
-    case GYRO_RANGE_2000:
-      v_range = BMI160_GYRO_RANGE_2000_DEG_SEC;
-      m_gyroScale = 16.4;
-      break;
-
-    default: // should never occur, but...
-      m_gyroScale = 1.0;        // set a safe, though incorrect value
-      throw std::logic_error(string(__FUNCTION__) +
-                             ": internal error, unsupported scale");
-      break;
-    }
-
-  bmi160_set_gyro_range(v_range);
-
-  return;
+    bmi160_set_gyroscope_scale(m_bmi160, scale);
 }
 
 void BMI160::getAccelerometer(float *x, float *y, float *z)
 {
-  if (x)
-    *x = m_accelX / m_accelScale;
-
-  if (y)
-    *y = m_accelY / m_accelScale;
-
-  if (z)
-    *z = m_accelZ / m_accelScale;
+    bmi160_get_accelerometer(m_bmi160, x, y, z);
 }
 
 void BMI160::getGyroscope(float *x, float *y, float *z)
 {
-  if (x)
-    *x = m_gyroX / m_gyroScale;
-
-  if (y)
-    *y = m_gyroY / m_gyroScale;
-
-  if (z)
-    *z = m_gyroZ / m_gyroScale;
+    bmi160_get_gyroscope(m_bmi160, x, y, z);
 }
 
 void BMI160::getMagnetometer(float *x, float *y, float *z)
 {
-  if (x)
-    *x = m_magX;
-
-  if (y)
-    *y = m_magY;
-
-  if (z)
-    *z = m_magZ;
+    bmi160_get_magnetometer(m_bmi160, x, y, z);
 }
 
 float *BMI160::getAccelerometer()
 {
-  float *values = new float[3]; // x, y, and then z
+  static float values[3]; // x, y, and then z
 
   getAccelerometer(&values[0], &values[1], &values[2]);
 
@@ -383,7 +86,7 @@ float *BMI160::getAccelerometer()
 
 float *BMI160::getGyroscope()
 {
-  float *values = new float[3]; // x, y, and then z
+  static float values[3]; // x, y, and then z
 
   getGyroscope(&values[0], &values[1], &values[2]);
 
@@ -392,7 +95,7 @@ float *BMI160::getGyroscope()
 
 float *BMI160::getMagnetometer()
 {
-  float *values = new float[3]; // x, y, and then z
+  static float values[3]; // x, y, and then z
 
   getMagnetometer(&values[0], &values[1], &values[2]);
 
@@ -401,33 +104,41 @@ float *BMI160::getMagnetometer()
 
 void BMI160::enableMagnetometer(bool enable)
 {
-  // butchered from support example
-  if (!enable)
-    {
-      bmi160_set_bmm150_mag_and_secondary_if_power_mode(MAG_SUSPEND_MODE);
-      bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-      bmi160_set_if_mode(0x00);
-      bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-
-      m_magEnabled = false;
-      m_magX = 0;
-      m_magY = 0;
-      m_magZ = 0;
-    }
-  else
-    {
-      u8 v_bmm_chip_id_u8 = BMI160_INIT_VALUE;
-      /* Init the magnetometer */
-      bmi160_bmm150_mag_interface_init(&v_bmm_chip_id_u8);
-
-      /* bmi160_delay_ms in ms*/
-      bmi160_delay_ms(BMI160_GEN_READ_WRITE_DELAY);
-
-      m_magEnabled = true;
-    }
+    bmi160_enable_magnetometer(m_bmi160, enable);
 }
 
 unsigned int BMI160::getSensorTime()
 {
-  return m_sensorTime;
+    return bmi160_get_time(m_bmi160);
+}
+
+string BMI160::busRead(int addr, int reg, int len)
+{
+    u8 dev_addr = (u8)(addr & 0xff);
+    u8 reg_addr = (u8)(reg & 0xff);
+    u8 cnt = (u8)(len & 0xff);
+
+    u8 *data = new u8[cnt];
+
+    if (bmi160_bus_read(dev_addr, reg_addr, data, cnt))
+    {
+        delete [] data;
+        throw std::runtime_error(string(__FUNCTION__)
+                                 + ": bmi160_bus_read() failed");
+    }
+
+    string dataStr((char *)data, cnt);
+    delete [] data;
+
+    return dataStr;
+}
+
+void BMI160::busWrite(int addr, int reg, string data)
+{
+    u8 dev_addr = (u8)(addr & 0xff);
+    u8 reg_addr = (u8)(reg & 0xff);
+
+    if (bmi160_bus_write(dev_addr, reg_addr, (u8 *)data.data(), data.size()))
+        throw std::runtime_error(string(__FUNCTION__)
+                                 + ": bmi160_bus_write() failed");
 }
diff --git a/src/bmi160/bmi160.h b/src/bmi160/bmi160.h
new file mode 100644
index 00000000..37640d2f
--- /dev/null
+++ b/src/bmi160/bmi160.h
@@ -0,0 +1,250 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * 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 <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <upm.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "bosch_bmi160.h"
+
+#define BMI160_CHIP_ID 0xd1
+
+  /**
+   * @brief BMI160 3-axis Accelerometer, Gyroscope and (optionally) a
+   * Magnetometer
+   *
+   * The Bosch BMI160 is a 3-axis Accelerometer and Gyroscope.
+   * Additionally it supports an external Magnetometer, accessed
+   * through the BMI160's register interface.  This driver was
+   * developed with a BMI160 "Shuttle" board, which included a BMM150
+   * Magnetometer.
+   *
+   * The device is driven by either 1.8v or 3.3vdc.  This driver
+   * incorporates the Bosch BMI160 driver code at
+   * https://github.com/BoschSensortec/BMI160_driver
+   *
+   * The Bosch driver code does not provide a mechanism for passing
+   * user data around (like the device context).  For this reason,
+   * only one instance of this driver can be used in a given process,
+   * due to the use of static data in the driver.
+   *
+   * While not all of the functionality of this device is supported
+   * initially, the inclusion of the Bosch driver in the source code
+   * makes it possible to support whatever features are required that
+   * the driver can support.
+   *
+   * @snippet bmi160.c Interesting
+   */
+
+    typedef struct _bmi160_context {
+        // uncompensated accelerometer and gyroscope values
+        float accelX;
+        float accelY;
+        float accelZ;
+
+        float gyroX;
+        float gyroY;
+        float gyroZ;
+
+        float magX;
+        float magY;
+        float magZ;
+
+        unsigned int sensorTime;
+
+        // accelerometer and gyro scaling factors, depending on their Full
+        // Scale (Range) settings.
+        float accelScale;
+        float gyroScale;
+
+        // is the magnetometer enabled?
+        bool magEnabled;
+
+    } *bmi160_context;
+
+    typedef enum {
+      BMI160_ACC_RANGE_2G                        = 0, // 2 Gravities
+      BMI160_ACC_RANGE_4G,
+      BMI160_ACC_RANGE_8G,
+      BMI160_ACC_RANGE_16G
+    } BMI160_ACC_RANGE_T;
+
+    typedef enum {
+      BMI160_GYRO_RANGE_125                      = 0, // 125 degrees/sec
+      BMI160_GYRO_RANGE_250,
+      BMI160_GYRO_RANGE_500,
+      BMI160_GYRO_RANGE_1000,
+      BMI160_GYRO_RANGE_2000
+    } BMI160_GYRO_RANGE_T;
+
+    /**
+     * bmi160 constructor
+     *
+     * @param bus i2c/spi bus to use
+     * @param address The address for this device if using I2C.  If
+     * using SPI, supply -1 for this parameter.
+     * @param cs_pin The GPIO to use for Chip Select (CS).  This is
+     * only needed for SPI, and only if your SPI implementation
+     * requires it.  Otherwise, just pass -1 if not using SPI, or your
+     * CS is handled automatically by your SPI implementation.
+     * @param enable_mag True, if you want to enable the magnetometer
+     * @return an initialized device context on success, NULL on error.
+     */
+    bmi160_context bmi160_init(unsigned int bus, int address, int cs_pin,
+                               bool enable_mag);
+
+    /**
+     * BMI160 Destructor
+     *
+     * @param dev Device context.
+     */
+    void bmi160_close(bmi160_context dev);
+
+    /**
+     * Take a measurement and store the current sensor values
+     * internally.  This function must be called prior to retrieving
+     * any sensor values, for example getAccelerometer().
+     *
+     * @param dev Device context.
+     * @param dev sensor context
+     */
+    void bmi160_update(const bmi160_context dev);
+
+    /**
+     * set the scaling mode of the accelerometer
+     *
+     * @param dev Device context.
+     * @param scale one of the ACCEL_RANGE_T values
+     */
+    void bmi160_set_accelerometer_scale(const bmi160_context dev,
+                                        BMI160_ACC_RANGE_T scale);
+
+    /**
+     * set the scaling mode of the gyroscope
+     *
+     * @param dev Device context.
+     * @param scale one of the GYRO_RANGE_T values
+     */
+    void bmi160_set_gyroscope_scale(const bmi160_context dev,
+                                    BMI160_GYRO_RANGE_T scale);
+
+    /**
+     * Get the Accelerometer values.  The values returned are in
+     * gravities.  update() must have been called prior to calling
+     * this method.
+     *
+     * @param dev Device context.
+     * @param x A pointer into which the X value will be returned
+     * @param y A pointer into which the Y value will be returned
+     * @param z A pointer into which the Z value will be returned
+     */
+    void bmi160_get_accelerometer(const bmi160_context dev,
+                                  float *x, float *y, float *z);
+
+    /**
+     * Get the Gyroscope values.  The values returned are in degrees
+     * per second.  update() must have been called prior to calling
+     * this method.
+     *
+     * @param dev Device context.
+     * @param x A pointer into which the X value will be returned
+     * @param y A pointer into which the Y value will be returned
+     * @param z A pointer into which the Z value will be returned
+     */
+    void bmi160_get_gyroscope(const bmi160_context dev,
+                              float *x, float *y, float *z);
+
+    /**
+     * Get the Magnetometer values.  The values returned are in micro
+     * Teslas.  update() must have been called prior to calling this
+     * method.
+     *
+     * @param dev Device context.
+     * @param x A pointer into which the X value will be returned
+     * @param y A pointer into which the Y value will be returned
+     * @param z A pointer into which the Z value will be returned
+     */
+    void bmi160_get_magnetometer(const bmi160_context dev,
+                                 float *x, float *y, float *z);
+
+    /**
+     * Enable or disable the Magnetometer.  By default, the
+     * magnetometer is enabled.
+     *
+     * @param dev Device context.
+     * @param enable true to enable the magnetometer, false to disable.
+     */
+    void bmi160_enable_magnetometer(const bmi160_context dev, bool enable);
+
+    /**
+     * Return the sensor time.  This is a 24bit value that increments
+     * every 39us.  It will wrap around once the 24b resolution is
+     * exceeded.
+     *
+     * @param dev Device context.
+     * @return The current sensor time.
+     */
+    unsigned int bmi160_get_time(const bmi160_context dev);
+
+    /**
+     * Perform a bus read.  This function is bus agnostic, and is used
+     * by the bosch code to perform bus reads.  It is exposed here for
+     * those users wishing to perform their own low level accesses.
+     * This is a low level function, and should not be used unless you
+     * know what you are doing.
+     *
+     * @param dev_addr For I2C operation, this is the I2C address.
+     * For SPI, this argument is ignored.
+     * @param reg_addr The register address to access.
+     * @param reg_data A pointer to a buffer in which data will be read into.
+     * @param cnt The number of bytes to read.
+     * @return A return of 0 indicates no errors, non-zero indicates an error.
+     */
+    s8 bmi160_bus_read(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
+
+    /**
+     * Perform a bus write.  This function is bus agnostic, and is used
+     * by the bosch code to perform bus writes.  It is exposed here for
+     * those users wishing to perform their own low level accesses.
+     * This is a low level function, and should not be used unless you
+     * know what you are doing.
+     *
+     * @param dev_addr For I2C operation, this is the I2C address.
+     * For SPI, this argument is ignored.
+     * @param reg_addr The register address to access.
+     * @param reg_data A pointer to a buffer containing data to write.
+     * @param cnt The number of bytes to write.
+     * @return A return of 0 indicates no errors, non-zero indicates an error.
+     */
+    s8 bmi160_bus_write(u8 dev_addr, u8 reg_addr, u8 *reg_data, u8 cnt);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/bmi160/bmi160.hpp b/src/bmi160/bmi160.hpp
index c9be4125..e28bed7a 100644
--- a/src/bmi160/bmi160.hpp
+++ b/src/bmi160/bmi160.hpp
@@ -22,230 +22,225 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 #pragma once
-
-#include <stdint.h>
+#include <string>
+#include "bmi160.h"
 
 #define BMI160_I2C_BUS 0
 #define BMI160_DEFAULT_I2C_ADDR 0x69
 
 namespace upm {
 
-  /**
-   * @brief BMI160 3-axis Accelerometer, Gyroscope and Magnetometer
-   * @defgroup bmi160 libupm-bmi160
-   * @ingroup i2c accelerometer compass
-   */
+    /**
+     * @brief BMI160 3-axis Accelerometer, Gyroscope and Magnetometer
+     * @defgroup bmi160 libupm-bmi160
+     * @ingroup i2c accelerometer compass
+     */
 
 
-  /**
-   * @library bmi160
-   * @sensor bmi160
-   * @comname BMI160 3-axis Accelerometer, Gyroscope
-   * and Magnetometer
-   * @type accelerometer compass
-   * @man mouser
-   * @con i2c
-   * @web http://www.mouser.com/ProductDetail/Bosch-Sensortec/0330SB2187/?qs=sGAEpiMZZMvi6wO7nhr1L9JELKA6cYRX60mAGNTn0fQ%3d
-   *
-   * @brief UPM API for the BMI160 3-axis Accelerometer, Gyroscope and
-   * Magnetometer
-   *
-   * The Bosch BMI160 is a 3-axis Accelerometer and Gyroscope.
-   * Additionally it supports an external Magnetometer, accessed
-   * through the BMI160's register interface.  This driver was
-   * developed with a BMI160 "Shuttle" board, which included a BMM150
-   * Magnetometer.
-   *
-   * The device is driven by either 1.8v or 3.3vdc.  This driver
-   * incorporates the Bosch BMI160 driver code at
-   * https://github.com/BoschSensortec/BMI160_driver .
-   *
-   * While not all of the functionality of this device is supported
-   * initially, the inclusion of the Bosch driver in the source code
-   * makes it possible to support whatever features are required that
-   * the driver can support.
-   *
-   * @snippet bmi160.cxx Interesting
-   */
-  class BMI160 {
-  public:
-
-    typedef enum {
-      ACCEL_RANGE_2G                      = 0, // 2 Gravities
-      ACCEL_RANGE_4G,
-      ACCEL_RANGE_8G,
-      ACCEL_RANGE_16G
-    } ACCEL_RANGE_T;
-
-    typedef enum {
-      GYRO_RANGE_125                      = 0, // 125 degrees/sec
-      GYRO_RANGE_250,
-      GYRO_RANGE_500,
-      GYRO_RANGE_1000,
-      GYRO_RANGE_2000
-    } GYRO_RANGE_T;
-
     /**
-     * bmi160 constructor
+     * @library bmi160
+     * @sensor bmi160
+     * @comname BMI160 3-axis Accelerometer, Gyroscope
+     * and Magnetometer
+     * @type accelerometer compass
+     * @man mouser
+     * @con i2c
+     * @web http://www.mouser.com/ProductDetail/Bosch-Sensortec/0330SB2187/?qs=sGAEpiMZZMvi6wO7nhr1L9JELKA6cYRX60mAGNTn0fQ%3d
      *
-     * @param bus i2c bus to use
-     * @param address the address for this device
+     * @brief UPM API for the BMI160 3-axis Accelerometer, Gyroscope and
+     * Magnetometer
+     *
+     * The Bosch BMI160 is a 3-axis Accelerometer and Gyroscope.
+     * Additionally it supports an external Magnetometer, accessed
+     * through the BMI160's register interface.  This driver was
+     * developed with a BMI160 "Shuttle" board, which included a BMM150
+     * Magnetometer.
+     *
+     * The device is driven by either 1.8v or 3.3vdc.  This driver
+     * incorporates the Bosch BMI160 driver code at
+     * https://github.com/BoschSensortec/BMI160_driver
+     *
+     * The Bosch driver code does not provide a mechanism for passing
+     * user data around (like the device context).  For this reason,
+     * only one instance of this driver can be used in a given process,
+     * due to the use of static data in the driver.
+     *
+     * While not all of the functionality of this device is supported
+     * initially, the inclusion of the Bosch driver in the source code
+     * makes it possible to support whatever features are required that
+     * the driver can support.
+     *
+     * @snippet bmi160.cxx Interesting
      */
-    BMI160(int bus=BMI160_I2C_BUS, uint8_t address=BMI160_DEFAULT_I2C_ADDR);
+    class BMI160 {
+    public:
 
-    /**
-     * BMI160 Destructor
-     */
-    ~BMI160();
+        /**
+         * BMI160 constructor.  The default arguments inititialize I2C
+         * operation and the default I2C address.
+         *
+         * @param bus i2c bus to use
+         * @param address The address for this device if using I2C.  If
+         * using SPI, supply -1 for this parameter.
+         * @param cs_pin The GPIO to use for Chip Select (CS).  This is
+         * only needed for SPI, and only if your SPI implementation
+         * requires it.  Otherwise, just pass -1 if not using SPI, or your
+         * CS is handled automatically by your SPI implementation.
+         * @param enableMag true to enable the magnetometer, false otherwise
+         */
+        BMI160(int bus=BMI160_I2C_BUS, int address=BMI160_DEFAULT_I2C_ADDR,
+               int csPin=-1, bool enableMag=true);
 
-    /**
-     * Take a measurement and store the current sensor values
-     * internally.  This function must be called prior to retrieving
-     * any sensor values, for example getAccelerometer().
-     *
-     */
-    void update();
+        /**
+         * BMI160 Destructor
+         */
+        ~BMI160();
 
-    /**
-     * set the scaling mode of the accelerometer
-     *
-     * @param scale one of the ACCEL_RANGE_T values
-     */
-    void setAccelerometerScale(ACCEL_RANGE_T scale);
+        /**
+         * Take a measurement and store the current sensor values
+         * internally.  This function must be called prior to retrieving
+         * any sensor values, for example getAccelerometer().
+         *
+         */
+        void update();
 
-    /**
-     * set the scaling mode of the gyroscope
-     *
-     * @param scale one of the GYRO_RANGE_T values
-     */
-    void setGyroscopeScale(GYRO_RANGE_T scale);
+        /**
+         * set the scaling mode of the accelerometer
+         *
+         * @param scale one of the ACCEL_RANGE_T values
+         */
+        void setAccelerometerScale(BMI160_ACC_RANGE_T scale);
 
-    /**
-     * Get the Accelerometer values.  This function returns a pointer
-     * to 3 floating point values: X, Y, and Z, in that order.  The
-     * values returned are in gravities.  update() must have been
-     * called prior to calling this method.
-     *
-     * The caller is reponsible for freeing the returned pointer.
-     *
-     * @return Pointer to 3 floating point values: X, Y, and Z in
-     * gravities.
-     */
-    float *getAccelerometer();
+        /**
+         * set the scaling mode of the gyroscope
+         *
+         * @param scale one of the GYRO_RANGE_T values
+         */
+        void setGyroscopeScale(BMI160_GYRO_RANGE_T scale);
 
-    /**
-     * Get the Accelerometer values.  The values returned are in
-     * gravities.  update() must have been called prior to calling
-     * this method.
-     *
-     * @param x A pointer into which the X value will be returned
-     * @param y A pointer into which the Y value will be returned
-     * @param z A pointer into which the Z value will be returned
-     */
-    void getAccelerometer(float *x, float *y, float *z);
+        /**
+         * Get the Accelerometer values.  This function returns a pointer
+         * to 3 floating point values: X, Y, and Z, in that order.  The
+         * values returned are in gravities.  update() must have been
+         * called prior to calling this method.
+         *
+         * The caller is reponsible for freeing the returned pointer.
+         *
+         * @return Pointer to 3 floating point values: X, Y, and Z in
+         * gravities.
+         */
+        float *getAccelerometer();
 
-    /**
-     * Get the Gyroscope values.  This function returns a pointer to 3
-     * floating point values: X, Y, and Z, in that order.  The values
-     * values returned are in degrees per second.  update() must have
-     * been called prior to calling this method.
-     *
-     * The caller is reponsible for freeing the returned pointer.
-     *
-     * @return Pointer to 3 floating point values: X, Y, and Z in
-     * degrees per second.
-     */
-    float *getGyroscope();
+        /**
+         * Get the Accelerometer values.  The values returned are in
+         * gravities.  update() must have been called prior to calling
+         * this method.
+         *
+         * @param x A pointer into which the X value will be returned
+         * @param y A pointer into which the Y value will be returned
+         * @param z A pointer into which the Z value will be returned
+         */
+        void getAccelerometer(float *x, float *y, float *z);
 
-    /**
-     * Get the Gyroscope values.  The values returned are in degrees
-     * per second.  update() must have been called prior to calling
-     * this method.
-     *
-     * @param x A pointer into which the X value will be returned
-     * @param y A pointer into which the Y value will be returned
-     * @param z A pointer into which the Z value will be returned
-     */
-    void getGyroscope(float *x, float *y, float *z);
+        /**
+         * Get the Gyroscope values.  This function returns a pointer to 3
+         * floating point values: X, Y, and Z, in that order.  The values
+         * values returned are in degrees per second.  update() must have
+         * been called prior to calling this method.
+         *
+         * The caller is reponsible for freeing the returned pointer.
+         *
+         * @return Pointer to 3 floating point values: X, Y, and Z in
+         * degrees per second.
+         */
+        float *getGyroscope();
 
-    /**
-     * Get the Magnetometer values.  This function returns a pointer
-     * to 3 floating point values: X, Y, and Z, in that order.  The
-     * values values returned are in micro Teslas.  update() must have
-     * been called prior to calling this method.  If the Magnetometer
-     * has been disabled, the return values will always be 0, 0, and
-     * 0.
-     *
-     * The caller is reponsible for freeing the returned pointer.
-     *
-     * @return Pointer to 3 floating point values: X, Y, and Z in
-     * micro Teslas.
-     */
-    float *getMagnetometer();
+        /**
+         * Get the Gyroscope values.  The values returned are in degrees
+         * per second.  update() must have been called prior to calling
+         * this method.
+         *
+         * @param x A pointer into which the X value will be returned
+         * @param y A pointer into which the Y value will be returned
+         * @param z A pointer into which the Z value will be returned
+         */
+        void getGyroscope(float *x, float *y, float *z);
 
-    /**
-     * Get the Magnetometer values.  The values returned are in micro
-     * Teslas.  update() must have been called prior to calling this
-     * method.
-     *
-     * @param x A pointer into which the X value will be returned
-     * @param y A pointer into which the Y value will be returned
-     * @param z A pointer into which the Z value will be returned
-     */
-    void getMagnetometer(float *x, float *y, float *z);
+        /**
+         * Get the Magnetometer values.  This function returns a pointer
+         * to 3 floating point values: X, Y, and Z, in that order.  The
+         * values values returned are in micro Teslas.  update() must have
+         * been called prior to calling this method.  If the Magnetometer
+         * has been disabled, the return values will always be 0, 0, and
+         * 0.
+         *
+         * The caller is reponsible for freeing the returned pointer.
+         *
+         * @return Pointer to 3 floating point values: X, Y, and Z in
+         * micro Teslas.
+         */
+        float *getMagnetometer();
 
-    /**
-     * Enable or disable the Magnetometer.  By default, the
-     * magnetometer is enabled.
-     *
-     * @param enable true to enable the magnetometer, false to disable.
-     */
-    void enableMagnetometer(bool enable);
+        /**
+         * Get the Magnetometer values.  The values returned are in micro
+         * Teslas.  update() must have been called prior to calling this
+         * method.
+         *
+         * @param x A pointer into which the X value will be returned
+         * @param y A pointer into which the Y value will be returned
+         * @param z A pointer into which the Z value will be returned
+         */
+        void getMagnetometer(float *x, float *y, float *z);
 
-    /**
-     * Return the sensor time.  This is a 24bit value that increments
-     * every 39us.  It will wrap around once the 24b resolution is
-     * exceeded.
-     *
-     * @return The current sensor time.
-     */
-    unsigned int getSensorTime();
+        /**
+         * Enable or disable the Magnetometer.  By default, the
+         * magnetometer is enabled.
+         *
+         * @param enable true to enable the magnetometer, false to disable.
+         */
+        void enableMagnetometer(bool enable);
 
-  protected:
-    // uncompensated accelerometer and gyroscope values
-    float m_accelX;
-    float m_accelY;
-    float m_accelZ;
+        /**
+         * Return the sensor time.  This is a 24bit value that increments
+         * every 39us.  It will wrap around once the 24b resolution is
+         * exceeded.
+         *
+         * @return The current sensor time.
+         */
+        unsigned int getSensorTime();
 
-    float m_gyroX;
-    float m_gyroY;
-    float m_gyroZ;
+    protected:
+        bmi160_context m_bmi160;
 
-    float m_magX;
-    float m_magY;
-    float m_magZ;
+        /**
+         * Perform a bus read.  This function is bus agnostic, and is used
+         * by the bosch code to perform bus reads.  It is exposed here for
+         * those users wishing to perform their own low level accesses.
+         * This is a low level function, and should not be used unless you
+         * know what you are doing.
+         *
+         * @param addr For I2C operation, this is the I2C address.
+         * For SPI, this argument is ignored.
+         * @param reg The register address to access.
+         * @param cnt The number of bytes to read.
+         * @return The data read
+         */
+        std::string busRead(int addr, int reg, int len);
 
-    unsigned int m_sensorTime;
+        /**
+         * Perform a bus write.  This function is bus agnostic, and is used
+         * by the bosch code to perform bus writes.  It is exposed here for
+         * those users wishing to perform their own low level accesses.
+         * This is a low level function, and should not be used unless you
+         * know what you are doing.
+         *
+         * @param addr For I2C operation, this is the I2C address.
+         * For SPI, this argument is ignored.
+         * @param addr The register address to access.
+         * @param data The string containing the data to write
+         */
+        void busWrite(int addr, int reg, std::string data);
 
-    // accelerometer and gyro scaling factors, depending on their Full
-    // Scale (Range) settings.
-    float m_accelScale;
-    float m_gyroScale;
-
-    // is the magnetometer enabled?
-    bool m_magEnabled;
-
-    /**
-     * set up initial values and start operation
-     *
-     * @return true if successful
-     */
-    virtual bool init();
-
-  private:
-    // due to the way we need to 'hook' into the bmi driver, the i2c
-    // context is a static variable defined in the .cxx implmentation.
-
-    uint8_t m_addr;
-  };
+    private:
+    };
 }
diff --git a/src/bmi160/bosch_bmi160.c b/src/bmi160/bosch_bmi160.c
index 52c834ae..6fd8489a 100644
--- a/src/bmi160/bosch_bmi160.c
+++ b/src/bmi160/bosch_bmi160.c
@@ -101,7 +101,8 @@ struct yas_vector fifo_vector_xyz;
  *	(Better case don't change the reference value of the parameter)
  *
 */
-BMI160_RETURN_FUNCTION_TYPE bmi160_init(struct bmi160_t *bmi160)
+/* JET - renamed from bmi160_init to avoid conflict with UPM code */
+BMI160_RETURN_FUNCTION_TYPE bmi160_init_bus(struct bmi160_t *bmi160)
 {
 	/* variable used for return the status of communication result*/
 	BMI160_RETURN_FUNCTION_TYPE com_rslt = E_BMI160_COMM_RES;
diff --git a/src/bmi160/bosch_bmi160.h b/src/bmi160/bosch_bmi160.h
index 96f79daa..87ca2249 100644
--- a/src/bmi160/bosch_bmi160.h
+++ b/src/bmi160/bosch_bmi160.h
@@ -3724,7 +3724,8 @@ u8 mag_r_y2_msb;
  *	(Better case don't change the reference value of the parameter)
  *
 */
-BMI160_RETURN_FUNCTION_TYPE bmi160_init(struct bmi160_t *bmi160);
+/* renamed from bmi160_init to avoid conflict with UPM code */
+BMI160_RETURN_FUNCTION_TYPE bmi160_init_bus(struct bmi160_t *bmi160);
 /**************************************************/
 /**\name	 FUNCTION FOR READ AND WRITE REGISTERS  */
 /*************************************************/
@@ -11788,7 +11789,8 @@ u8 v_command_reg_data_u8);
  *
  *
  */
-static void xy1y2_to_xyz(u16 *xy1y2, s32 *xyz);
+/*JET
+static void xy1y2_to_xyz(u16 *xy1y2, s32 *xyz);*/
 /*!
  *	@brief This function used for read the
  *	YAMAHA YAS537 xy1y2 data
@@ -11837,8 +11839,9 @@ u8 *v_ouflow_u8, struct yas_vector *vector_xyz);
  *
  *
  */
+/* JET
 static BMI160_RETURN_FUNCTION_TYPE invalid_magnetic_field(
-u16 *v_cur_u16, u16 *v_last_u16);
+  u16 *v_cur_u16, u16 *v_last_u16);*/
 /***************************************************/
 /**\name	FUNCTIONS FOR FIFO DATA READ */
 /***************************************************/
diff --git a/src/bmi160/javaupm_bmi160.i b/src/bmi160/javaupm_bmi160.i
index 478b58fc..b4fe4bdd 100644
--- a/src/bmi160/javaupm_bmi160.i
+++ b/src/bmi160/javaupm_bmi160.i
@@ -3,6 +3,7 @@
 %include "typemaps.i"
 %include "arrays_java.i"
 %include "../java_buffer.i"
+%include "std_string.i"
 
 %{
     #include "bmi160.hpp"
@@ -20,7 +21,6 @@
 %typemap(out) float * {
     $result = JCALL1(NewFloatArray, jenv, 3);
     JCALL4(SetFloatArrayRegion, jenv, $result, 0, 3, $1);
-    delete [] $1;
 }
 
 %ignore getAccelerometer(float *, float *, float *);
diff --git a/src/bmi160/jsupm_bmi160.i b/src/bmi160/jsupm_bmi160.i
index d7287160..3c491e8a 100644
--- a/src/bmi160/jsupm_bmi160.i
+++ b/src/bmi160/jsupm_bmi160.i
@@ -1,6 +1,7 @@
 %module jsupm_bmi160
 %include "../upm.i"
 %include "cpointer.i"
+%include "std_string.i"
 
 %pointer_functions(float, floatp);
 
diff --git a/src/bmi160/pyupm_bmi160.i b/src/bmi160/pyupm_bmi160.i
index e5cd91ed..34873d6a 100644
--- a/src/bmi160/pyupm_bmi160.i
+++ b/src/bmi160/pyupm_bmi160.i
@@ -3,6 +3,7 @@
 %module pyupm_bmi160
 %include "../upm.i"
 %include "cpointer.i"
+%include "std_string.i"
 
 %include "stdint.i"