diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 145048df28..e9b8e32367 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -11,6 +11,7 @@ #include #include "AP_InertialSensor.h" +#include "AP_InertialSensor_BMI160.h" #include "AP_InertialSensor_Backend.h" #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_L3G4200D.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp new file mode 100644 index 0000000000..4826c8a15f --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.cpp @@ -0,0 +1,501 @@ +/* + * Copyright (C) 2016 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include +#include +#include + +#include "AP_InertialSensor_BMI160.h" + +/* Registers and bits definitions. The indented ones are the bits for the upper + * register. */ +#define BMI160_REG_CHIPID 0x00 +#define BMI160_CHIPID 0xD1 +#define BMI160_REG_ERR_REG 0x02 +#define BMI160_REG_FIFO_LENGTH 0x22 +#define BMI160_REG_FIFO_DATA 0x24 +#define BMI160_REG_ACC_CONF 0x40 +#define BMI160_REG_ACC_RANGE 0x41 + /* For convenience, use log2(range) - 1 instead of bits defined in + * the datasheet. See _configure_accel(). */ +#define BMI160_ACC_RANGE_16G 3 +#define BMI160_REG_GYR_CONF 0x42 +#define BMI160_REG_GYR_RANGE 0x43 +#define BMI160_GYR_RANGE_2000DPS 0x00 +#define BMI160_REG_FIFO_CONFIG_0 0x46 +#define BMI160_REG_FIFO_CONFIG_1 0x47 +#define BMI160_FIFO_ACC_EN 0x40 +#define BMI160_FIFO_GYR_EN 0x80 +#define BMI160_REG_INT_EN_1 0x51 +#define BMI160_INT_FWM_EN 0x40 +#define BMI160_REG_INT_OUT_CTRL 0x53 +#define BMI160_INT1_LVL 0x02 +#define BMI160_INT1_OUTPUT_EN 0x08 +#define BMI160_REG_INT_MAP_1 0x56 +#define BMI160_INT_MAP_INT1_FWM 0x40 +#define BMI160_REG_CMD 0x7E +#define BMI160_CMD_ACCEL_NORMAL_POWER_MODE 0x11 +#define BMI160_CMD_GYRO_NORMAL_POWER_MODE 0x15 +#define BMI160_CMD_FIFO_FLUSH 0xB0 +#define BMI160_CMD_SOFTRESET 0xB6 + +#define BMI160_OSR_NORMAL 0x20 +#define BMI160_ODR_1600HZ 0x0C + +/* Datasheet says that the device powers up in less than 10ms, so waiting for + * 10 ms before initialization is enough. */ +#define BMI160_POWERUP_DELAY_MSEC 10 +/* TODO: Investigate this. The delay below is way too high and with that + * there's still at least 1% of failures on initialization. Lower values + * increase that percentage. */ +#define BMI160_SOFTRESET_DELAY_MSEC 100 +/* Define a little bit more than the maximum value in the datasheet's timing + * table. The datasheet recommends adding 300 us to the time for startup + * occasions. */ +#define BMI160_ACCEL_NORMAL_POWER_MODE_DELAY_MSEC 4 +#define BMI160_GYRO_NORMAL_POWER_MODE_DELAY_MSEC 81 + +#define BMI160_OSR BMI160_OSR_NORMAL +#define BMI160_ODR BMI160_ODR_1600HZ +#define BMI160_ACC_RANGE BMI160_ACC_RANGE_16G +#define BMI160_GYR_RANGE BMI160_GYR_RANGE_2000DPS + +/* By looking at the datasheet, the accel range i (as defined by the macros + * BMI160_ACC_RANGE_*G) maps to the range bits by the function f defined: + * f(0) = 3; f(i) = f(i - 1) + i + 1 + * Which can be written as the closed formula: + * f(i) = (i * (i + 3)) / 2 + 3 */ +#define BMI160_ACC_RANGE_BITS \ + (BMI160_ACC_RANGE * (BMI160_ACC_RANGE + 3) / 2 + 3) + +/* The rate in Hz based on the ODR bits can be calculated with + * 100 / (2 ^ (8 - odr) */ +#define BMI160_ODR_TO_HZ(odr_) \ + (uint16_t)(odr_ > 8 ? 100 << (odr_ - 8) : 100 >> (8 - odr_)) + +/* This number of samples should provide only one read burst operation on the + * FIFO most of the time (99.99%). */ +#define BMI160_MAX_FIFO_SAMPLES 8 + +#define BMI160_READ_FLAG 0x80 +#define BMI160_HARDWARE_INIT_MAX_TRIES 5 + +#define BMI160_INT1_GPIO -1 + +extern const AP_HAL::HAL& hal; + +struct PACKED RawData { + struct { + le16_t x; + le16_t y; + le16_t z; + } gyro; + struct { + le16_t x; + le16_t y; + le16_t z; + } accel; +}; + +AP_InertialSensor_BMI160::AP_InertialSensor_BMI160(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev) + : AP_InertialSensor_Backend(imu) + , _dev(std::move(dev)) +{ +} + +AP_InertialSensor_Backend * +AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev) +{ + auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev)); + + if (!sensor) { + return nullptr; + } + + if (!sensor->_init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +void AP_InertialSensor_BMI160::start() +{ + bool r; + + hal.scheduler->suspend_timer_procs(); + if (!_dev->get_semaphore()->take(100)) { + AP_HAL::panic("BMI160: Unable to get semaphore"); + } + + r = _configure_accel(); + if (!r) { + AP_HAL::panic("BMI160: Unable to configure accelerometer"); + } + + r = _configure_gyro(); + if (!r) { + AP_HAL::panic("BMI160: Unable to configure gyroscope"); + } + + r = _configure_fifo(); + if (!r) { + AP_HAL::panic("BMI160: Unable to configure FIFO"); + } + + if (BMI160_INT1_GPIO >= 0) { + r = _configure_int1_pin(); + if (!r) { + AP_HAL::panic("BMI160: unable to configure INT1 pin"); + } + } + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + _dev->get_semaphore()->give(); + + _accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR)); + _gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR)); + + hal.scheduler->resume_timer_procs(); + + hal.scheduler->register_timer_process( + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI160::_poll_data, void)); +} + +bool AP_InertialSensor_BMI160::update() +{ + update_accel(_accel_instance); + update_gyro(_gyro_instance); + return true; +} + +void AP_InertialSensor_BMI160::_check_err_reg() +{ +#ifdef BMI160_DEBUG + uint8_t v; + bool r; + + r = _dev->read_registers(BMI160_REG_ERR_REG, &v, 1); + if (!r) { + AP_HAL::panic("BMI160: couldn't read ERR_REG\n"); + } + if (v) { + AP_HAL::panic("BMI160: error detected on ERR_REG\n"); + } +#endif +} + +bool AP_InertialSensor_BMI160::_configure_accel() +{ + bool r; + + r = _dev->write_register(BMI160_REG_ACC_CONF, BMI160_OSR | BMI160_ODR); + if (!r) { + return false; + } + hal.scheduler->delay(1); + + _check_err_reg(); + + r = _dev->write_register(BMI160_REG_ACC_RANGE, BMI160_ACC_RANGE_BITS); + if (!r) { + return false; + } + hal.scheduler->delay(1); + + /* The sensitivity in LSb/g for an accel range i (as defined by the macros + * BMI160_ACC_RANGE_*G) can be calculated with: + * 2 ^ 16 / (2 * 2 ^ (i + 1)) = 2 ^(14 - i) + * That matches the typical values in the datasheet. */ + _accel_scale = GRAVITY_MSS / (1 << (14 - BMI160_ACC_RANGE)); + + return true; +} + +bool AP_InertialSensor_BMI160::_configure_gyro() +{ + bool r; + + r = _dev->write_register(BMI160_REG_GYR_CONF, BMI160_OSR | BMI160_ODR); + if (!r) { + return false; + } + hal.scheduler->delay(1); + + _check_err_reg(); + + r = _dev->write_register(BMI160_REG_GYR_RANGE, BMI160_GYR_RANGE); + if (!r) { + return false; + } + hal.scheduler->delay(1); + + /* The sensitivity in LSb/degrees/s a gyro range i can be calculated with: + * 2 ^ 16 / (2 * 2000 / 2 ^ i) = 2 ^ (14 + i) / 1000 + * The scale is the inverse of that. */ + _gyro_scale = radians(1000.f / (1 << (14 + BMI160_GYR_RANGE))); + + return true; +} + +bool AP_InertialSensor_BMI160::_configure_int1_pin() +{ + bool r; + + r = _dev->write_register(BMI160_REG_INT_EN_1, BMI160_INT_FWM_EN); + if (!r) { + hal.console->printf("BMI160: Unable to enable FIFO watermark interrupt engine\n"); + return false; + } + hal.scheduler->delay(1); + + r = _dev->write_register(BMI160_REG_INT_MAP_1, BMI160_INT_MAP_INT1_FWM); + if (!r) { + hal.console->printf("BMI160: Unable to configure interrupt mapping\n"); + return false; + } + hal.scheduler->delay(1); + + r = _dev->write_register(BMI160_REG_INT_OUT_CTRL, + BMI160_INT1_OUTPUT_EN | BMI160_INT1_LVL); + if (!r) { + hal.console->printf("BMI160: Unable to configure interrupt output\n"); + return false; + } + hal.scheduler->delay(1); + + _int1_pin = hal.gpio->channel(BMI160_INT1_GPIO); + if (_int1_pin == nullptr) { + hal.console->printf("BMI160: Couldn't request data ready GPIO channel\n"); + return false; + } + _int1_pin->mode(HAL_GPIO_INPUT); + + return true; +} + +bool AP_InertialSensor_BMI160::_configure_fifo() +{ + bool r; + + /* The unit for the FIFO watermark is 4 bytes. */ + r = _dev->write_register(BMI160_REG_FIFO_CONFIG_0, + sizeof(struct RawData) / 4); + if (!r) { + hal.console->printf("BMI160: Unable to configure FIFO watermark level\n"); + return false; + } + hal.scheduler->delay(1); + + r = _dev->write_register(BMI160_REG_FIFO_CONFIG_1, + BMI160_FIFO_ACC_EN | BMI160_FIFO_GYR_EN); + if (!r) { + hal.console->printf("BMI160: Unable to enable FIFO\n"); + return false; + } + hal.scheduler->delay(1); + + _check_err_reg(); + + r = _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); + if (!r) { + hal.console->printf("BMI160: Unable to flush FIFO\n"); + return false; + } + + return true; +} + +void AP_InertialSensor_BMI160::_read_fifo() +{ + struct RawData raw_data[BMI160_MAX_FIFO_SAMPLES]; + uint16_t num_bytes; + uint16_t excess; + uint8_t num_samples = 0; + bool r = true; + + static_assert(sizeof(raw_data) <= 100, "Too big to keep on stack"); + + /* If FIFO watermark not surpassed. */ + if (_int1_pin && _int1_pin->read() == 0) { + goto read_fifo_end; + } + + r = _dev->read_registers(BMI160_REG_FIFO_LENGTH, + (uint8_t *)&num_bytes, + sizeof(num_bytes)); + if (!r) { + goto read_fifo_end; + } + + num_bytes = le16toh(num_bytes); + if (!num_bytes) { + goto read_fifo_end; + } + +read_fifo_read_data: + if (num_bytes > sizeof(raw_data)) { + excess = num_bytes - sizeof(raw_data); + num_bytes = sizeof(raw_data); + } else { + excess = 0; + } + + r = _dev->read_registers(BMI160_REG_FIFO_DATA, + (uint8_t *)raw_data, + num_bytes); + if (!r) { + goto read_fifo_end; + } + + /* Read again just once */ + if (excess && num_samples) { + hal.console->printf("BMI160: dropping %u samples from fifo\n", + (uint8_t)(excess / sizeof(struct RawData))); + _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); + excess = 0; + } + + num_samples = num_bytes / sizeof(struct RawData); + for (uint8_t i = 0; i < num_samples; i++) { + Vector3f accel{(float)(int16_t)le16toh(raw_data[i].accel.x), + (float)(int16_t)le16toh(raw_data[i].accel.y), + (float)(int16_t)le16toh(raw_data[i].accel.z)}; + Vector3f gyro{(float)(int16_t)le16toh(raw_data[i].gyro.x), + (float)(int16_t)le16toh(raw_data[i].gyro.y), + (float)(int16_t)le16toh(raw_data[i].gyro.z)}; + + accel *= _accel_scale; + gyro *= _gyro_scale; + + _rotate_and_correct_accel(_accel_instance, accel); + _rotate_and_correct_gyro(_gyro_instance, gyro); + + _notify_new_accel_raw_sample(_accel_instance, accel); + _notify_new_gyro_raw_sample(_gyro_instance, gyro); + } + + if (excess) { + num_bytes = excess; + goto read_fifo_read_data; + } + +read_fifo_end: + if (!r) { + hal.console->printf("BMI160: error on reading FIFO\n"); + } +} + +void AP_InertialSensor_BMI160::_poll_data() +{ + if (!_dev->get_semaphore()->take_nonblocking()) { + return; + } + + _read_fifo(); + + _dev->get_semaphore()->give(); +} + +bool AP_InertialSensor_BMI160::_hardware_init() +{ + bool ret = false; + + hal.scheduler->delay(BMI160_POWERUP_DELAY_MSEC); + + if (!_dev->get_semaphore()->take(100)) { + AP_HAL::panic("BMI160: Unable to get semaphore"); + } + + _dev->set_speed(AP_HAL::Device::SPEED_LOW); + + for (unsigned i = 0; i < BMI160_HARDWARE_INIT_MAX_TRIES; i++) { + uint8_t v; + ret = _dev->write_register(BMI160_REG_CMD, + BMI160_CMD_SOFTRESET); + + if (!ret) { + continue; + } + hal.scheduler->delay(BMI160_SOFTRESET_DELAY_MSEC); + + /* The datasheet recommends doing a read operation on the register 0x7F + * in order to guarantee the sensor works using the SPI protocol. This + * shouldn't have side effects for I2C. */ + ret = _dev->read_registers(0x7F, &v, 1); + if (!ret) { + continue; + } + + ret = _dev->read_registers(BMI160_REG_CHIPID, &v, 1); + if (!ret) { + continue; + } + if (v != BMI160_CHIPID) { + ret = false; + continue; + } + + ret = _dev->write_register(BMI160_REG_CMD, + BMI160_CMD_ACCEL_NORMAL_POWER_MODE); + if (!ret) { + continue; + } + hal.scheduler->delay(BMI160_ACCEL_NORMAL_POWER_MODE_DELAY_MSEC); + + ret = _dev->write_register(BMI160_REG_CMD, + BMI160_CMD_GYRO_NORMAL_POWER_MODE); + if (!ret) { + continue; + } + hal.scheduler->delay(BMI160_GYRO_NORMAL_POWER_MODE_DELAY_MSEC); + + break; + } + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + _dev->get_semaphore()->give(); + return ret; +} + +bool AP_InertialSensor_BMI160::_init() +{ + bool ret = false; + _dev->set_read_flag(BMI160_READ_FLAG); + + hal.scheduler->suspend_timer_procs(); + ret = _hardware_init(); + hal.scheduler->resume_timer_procs(); + + if (!ret) { + hal.console->printf("BMI160: failed to init\n"); + } + + return ret; +} + +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h new file mode 100644 index 0000000000..f39d3d0f89 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI160.h @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2016 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +class AP_InertialSensor_BMI160 : public AP_InertialSensor_Backend { +public: + static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev); + + /** + * Configure the sensors and start reading routine. + */ + void start() override; + + bool update() override; + +private: + AP_InertialSensor_BMI160(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev); + + /** + * If the macro BMI160_DEBUG is defined, check if there are errors reported + * on the device's error register and panic if so. The implementation is + * empty if the BMI160_DEBUG isn't defined. + */ + void _check_err_reg(); + + /** + * Try to perform initialization of the BMI160 device. + * + * The device semaphore must be taken and released by the caller. + * + * @return true on success, false otherwise. + */ + bool _hardware_init(); + + /** + * Try to initialize this driver. + * + * Do sensor and other required initializations. + * + * @return true on success, false otherwise. + */ + bool _init(); + + /** + * Configure accelerometer sensor. The device semaphore must already be + * taken before calling this function. + * + * @return true on success, false otherwise. + */ + bool _configure_accel(); + + /** + * Configure gyroscope sensor. The device semaphore must already be + * taken before calling this function. + * + * @return true on success, false otherwise. + */ + bool _configure_gyro(); + + /** + * Configure INT1 pin as watermark interrupt pin at the level of one sample + * if using fifo or data-ready pin otherwise. + * + * @return true on success, false otherwise. + */ + bool _configure_int1_pin(); + + /** + * Configure FIFO. + * + * @return true on success, false otherwise. + */ + bool _configure_fifo(); + + /** + * Timer routine to read data from the sensors. + */ + void _poll_data(); + + /** + * Read samples from fifo. + */ + void _read_fifo(); + + AP_HAL::OwnPtr _dev; + + uint8_t _accel_instance; + float _accel_scale; + + uint8_t _gyro_instance; + float _gyro_scale; + + AP_HAL::DigitalSource *_int1_pin; +}; + +#endif