| /* |
| * 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; |
| } |