1 files changed, 433 insertions, 0 deletions
diff --git a/peripheral/libupm/src/bmi160/bmi160.cxx b/peripheral/libupm/src/bmi160/bmi160.cxx
new file mode 100644
index 0000000..d9f35b7
--- /dev/null
+++ b/peripheral/libupm/src/bmi160/bmi160.cxx
@@ -0,0 +1,433 @@
+/*
+ * 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 <iostream>
+#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)
+{
+  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");
+    }
+}
+
+BMI160::~BMI160()
+{
+  mraa_i2c_stop(i2cContext);
+  i2cContext = NULL;
+}
+
+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);
+    }
+}
+
+void BMI160::setAccelerometerScale(ACCEL_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;
+}
+
+void BMI160::setGyroscopeScale(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;
+}
+
+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;
+}
+
+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;
+}
+
+void BMI160::getMagnetometer(float *x, float *y, float *z)
+{
+  if (x)
+    *x = m_magX;
+
+  if (y)
+    *y = m_magY;
+
+  if (z)
+    *z = m_magZ;
+}
+
+float *BMI160::getAccelerometer()
+{
+  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;
+}
+
+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;
+    }
+}
+
+unsigned int BMI160::getSensorTime()
+{
+  return m_sensorTime;
+}