From 89b8124560f3730af1f3a374c205af85d8b212c6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jun 2018 15:59:18 +1000 Subject: [PATCH] AP_InertialSensor: added BMI055 IMU driver --- .../AP_InertialSensor/AP_InertialSensor.cpp | 5 + .../AP_InertialSensor_BMI055.cpp | 340 ++++++++++++++++++ .../AP_InertialSensor_BMI055.h | 69 ++++ .../AP_InertialSensor_Backend.h | 1 + 4 files changed, 415 insertions(+) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 56bce77aab..50d2ec362b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -22,6 +22,7 @@ #include "AP_InertialSensor_SITL.h" #include "AP_InertialSensor_RST.h" #include "AP_InertialSensor_Revo.h" +#include "AP_InertialSensor_BMI055.h" /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop. * Output is on the debug console. */ @@ -743,6 +744,10 @@ AP_InertialSensor::detect_backends(void) _fast_sampling_mask.set_default(1); ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20689"), ROTATION_NONE)); ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602"), ROTATION_NONE)); + ADD_BACKEND(AP_InertialSensor_BMI055::probe(*this, + hal.spi->get_device("bmi055_a"), + hal.spi->get_device("bmi055_g"), + ROTATION_ROLL_180_YAW_90)); break; case AP_BoardConfig::PX4_BOARD_SP01: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp new file mode 100644 index 0000000000..9c55d2f921 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp @@ -0,0 +1,340 @@ +/* + * 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 +#include + +#include "AP_InertialSensor_BMI055.h" + +/* + device registers, names follow datasheet conventions, with REGA_ + prefix for accel, and REGG_ prefix for gyro + */ +#define REGA_BGW_CHIPID 0x00 +#define REGA_ACCD_X_LSB 0x02 +#define REGA_ACCD_TEMP 0x08 +#define REGA_INT_STATUS_0 0x09 +#define REGA_INT_STATUS_1 0x0A +#define REGA_INT_STATUS_2 0x0B +#define REGA_INT_STATUS_3 0x0C +#define REGA_FIFO_STATUS 0x0E +#define REGA_PMU_RANGE 0x0F +#define REGA_PMU_BW 0x10 +#define REGA_PMU_LPW 0x11 +#define REGA_ACCD_HBW 0x13 +#define REGA_BGW_SOFTRESET 0x14 +#define REGA_OUT_CTRL 0x20 +#define REGA_EST_LATCH 0x21 +#define REGA_FIFO_CONFIG_0 0x30 +#define REGA_PMU_SELF_TEST 0x32 +#define REGA_FIFO_CONFIG_1 0x3E +#define REGA_FIFO_DATA 0x3F + +#define REGG_CHIPID 0x00 +#define REGA_RATE_X_LSB 0x02 +#define REGG_INT_STATUS_0 0x09 +#define REGG_INT_STATUS_1 0x0A +#define REGG_INT_STATUS_2 0x0B +#define REGG_INT_STATUS_3 0x0C +#define REGG_FIFO_STATUS 0x0E +#define REGG_RANGE 0x0F +#define REGG_BW 0x10 +#define REGG_LPM1 0x11 +#define REGG_RATE_HBW 0x13 +#define REGG_BGW_SOFTRESET 0x14 +#define REGG_FIFO_CONFIG_1 0x3E +#define REGG_FIFO_DATA 0x3F + +extern const AP_HAL::HAL& hal; + +#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) + +AP_InertialSensor_BMI055::AP_InertialSensor_BMI055(AP_InertialSensor &imu, + AP_HAL::OwnPtr _dev_accel, + AP_HAL::OwnPtr _dev_gyro, + enum Rotation _rotation) + : AP_InertialSensor_Backend(imu) + , dev_accel(std::move(_dev_accel)) + , dev_gyro(std::move(_dev_gyro)) + , rotation(_rotation) +{ +} + +AP_InertialSensor_Backend * +AP_InertialSensor_BMI055::probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev_accel, + AP_HAL::OwnPtr dev_gyro, + enum Rotation rotation) +{ + if (!dev_accel || !dev_gyro) { + return nullptr; + } + auto sensor = new AP_InertialSensor_BMI055(imu, std::move(dev_accel), std::move(dev_gyro), rotation); + + if (!sensor) { + return nullptr; + } + + if (!sensor->init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +void AP_InertialSensor_BMI055::start() +{ + accel_instance = _imu.register_accel(2000, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055)); + gyro_instance = _imu.register_gyro(2000, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055)); + + // setup sensor rotations from probe() + set_gyro_orientation(gyro_instance, rotation); + set_accel_orientation(accel_instance, rotation); + + // setup callbacks + dev_accel->register_periodic_callback(1000, + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_accel, void)); + dev_gyro->register_periodic_callback(1000, + FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_gyro, void)); +} + +/* + probe and initialise accelerometer + */ +bool AP_InertialSensor_BMI055::accel_init() +{ + dev_accel->get_semaphore()->take_blocking(); + + uint8_t v; + if (!dev_accel->read_registers(REGA_BGW_CHIPID, &v, 1) || v != 0xFA) { + goto failed; + } + + if (!dev_accel->write_register(REGA_BGW_SOFTRESET, 0xB6)) { + goto failed; + } + hal.scheduler->delay(10); + + dev_accel->setup_checked_registers(5, 20); + + // setup 16g range + if (!dev_accel->write_register(REGA_PMU_RANGE, 0x0C, true)) { + goto failed; + } + + // setup filter bandwidth 1kHz + if (!dev_accel->write_register(REGA_PMU_BW, 0x0F, true)) { + goto failed; + } + + // disable low-power mode + if (!dev_accel->write_register(REGA_PMU_LPW, 0, true)) { + goto failed; + } + + // setup for unfiltered data + if (!dev_accel->write_register(REGA_ACCD_HBW, 0x80, true)) { + goto failed; + } + + // setup FIFO for streaming X,Y,Z + if (!dev_accel->write_register(REGA_FIFO_CONFIG_1, 0x80, true)) { + goto failed; + } + + + hal.console->printf("BMI055: found accel\n"); + + dev_accel->get_semaphore()->give(); + return true; + +failed: + dev_accel->get_semaphore()->give(); + return false; +} + +/* + probe and initialise gyro + */ +bool AP_InertialSensor_BMI055::gyro_init() +{ + dev_gyro->get_semaphore()->take_blocking(); + + uint8_t v; + if (!dev_gyro->read_registers(REGG_CHIPID, &v, 1) || v != 0x0F) { + goto failed; + } + + if (!dev_gyro->write_register(REGG_BGW_SOFTRESET, 0xB6)) { + goto failed; + } + hal.scheduler->delay(10); + + dev_gyro->setup_checked_registers(5, 20); + + // setup 2000dps range + if (!dev_gyro->write_register(REGG_RANGE, 0x00, true)) { + goto failed; + } + + // setup filter bandwidth 230Hz, no decimation + if (!dev_gyro->write_register(REGG_BW, 0x81, true)) { + goto failed; + } + + // disable low-power mode + if (!dev_gyro->write_register(REGG_LPM1, 0, true)) { + goto failed; + } + + // setup for filtered data + if (!dev_gyro->write_register(REGG_RATE_HBW, 0x00, true)) { + goto failed; + } + + // setup FIFO for streaming X,Y,Z + if (!dev_gyro->write_register(REGG_FIFO_CONFIG_1, 0x80, true)) { + goto failed; + } + + hal.console->printf("BMI055: found gyro\n"); + + dev_gyro->get_semaphore()->give(); + return true; + +failed: + dev_gyro->get_semaphore()->give(); + return false; +} + +bool AP_InertialSensor_BMI055::init() +{ + dev_accel->set_read_flag(0x80); + dev_gyro->set_read_flag(0x80); + + return accel_init() && gyro_init(); +} + +/* + read accel fifo + */ +void AP_InertialSensor_BMI055::read_fifo_accel(void) +{ + uint8_t num_frames; + if (!dev_accel->read_registers(REGA_FIFO_STATUS, &num_frames, 1)) { + _inc_accel_error_count(accel_instance); + return; + } + num_frames &= 0x7F; + + // don't read more than 8 frames at a time + if (num_frames > 8) { + num_frames = 8; + } + + if (num_frames == 0) { + return; + } + + uint8_t data[6*num_frames]; + if (!dev_accel->read_registers(REGA_FIFO_DATA, data, num_frames*6)) { + _inc_accel_error_count(accel_instance); + return; + } + // data is 12 bits with 16g range, 7.81mg/LSB + const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0; + for (uint8_t i = 0; i < num_frames; i++) { + const uint8_t *d = &data[i*6]; + int16_t xyz[3] { + int16_t(uint16_t((d[0]&0xF0) | (d[1]<<8))), + int16_t(uint16_t((d[2]&0xF0) | (d[3]<<8))), + int16_t(uint16_t((d[4]&0xF0) | (d[5]<<8))) }; + Vector3f accel(xyz[0], xyz[1], xyz[2]); + + accel *= scale; + + _rotate_and_correct_accel(accel_instance, accel); + _notify_new_accel_raw_sample(accel_instance, accel); + } + + if (temperature_counter++ == 100) { + temperature_counter = 0; + int8_t t; + if (!dev_accel->read_registers(REGA_ACCD_TEMP, (uint8_t *)&t, 1)) { + _inc_accel_error_count(accel_instance); + } else { + float temp_degc = (0.5 * t) + 23.0; + _publish_temperature(accel_instance, temp_degc); + } + } + + if (!dev_accel->check_next_register()) { + _inc_accel_error_count(accel_instance); + } +} + +/* + read gyro fifo + */ +void AP_InertialSensor_BMI055::read_fifo_gyro(void) +{ + uint8_t num_frames; + if (!dev_gyro->read_registers(REGG_FIFO_STATUS, &num_frames, 1)) { + _inc_gyro_error_count(gyro_instance); + return; + } + num_frames &= 0x7F; + + // don't read more than 8 frames at a time + if (num_frames > 8) { + num_frames = 8; + } + if (num_frames == 0) { + return; + } + uint8_t data[6*num_frames]; + if (!dev_gyro->read_registers(REGG_FIFO_DATA, data, num_frames*6)) { + _inc_gyro_error_count(gyro_instance); + return; + } + + // data is 16 bits with 2000dps range + const float scale = radians(2000.0) / 32767.0; + for (uint8_t i = 0; i < num_frames; i++) { + const uint8_t *d = &data[i*6]; + int16_t xyz[3] { + int16_t(uint16_t(d[0] | d[1]<<8)), + int16_t(uint16_t(d[2] | d[3]<<8)), + int16_t(uint16_t(d[4] | d[5]<<8)) }; + Vector3f gyro(xyz[0], xyz[1], xyz[2]); + gyro *= scale; + + _rotate_and_correct_gyro(gyro_instance, gyro); + _notify_new_gyro_raw_sample(gyro_instance, gyro); + } + + if (!dev_gyro->check_next_register()) { + _inc_gyro_error_count(gyro_instance); + } +} + +bool AP_InertialSensor_BMI055::update() +{ + update_accel(accel_instance); + update_gyro(gyro_instance); + return true; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h new file mode 100644 index 0000000000..8f7254a1fc --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h @@ -0,0 +1,69 @@ +/* + * 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 . + */ +/* + the BMI055 is unusual as it has separate chip-select for accel and + gyro, which means it needs two SPIDevice pointers + */ +#pragma once + +#include + +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +class AP_InertialSensor_BMI055 : public AP_InertialSensor_Backend { +public: + static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev_accel, + AP_HAL::OwnPtr dev_gyro, + enum Rotation rotation = ROTATION_NONE); + + /** + * Configure the sensors and start reading routine. + */ + void start() override; + bool update() override; + +private: + AP_InertialSensor_BMI055(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev_accel, + AP_HAL::OwnPtr dev_gyro, + enum Rotation rotation = ROTATION_NONE); + + /* + initialise hardware layer + */ + bool accel_init(); + bool gyro_init(); + + /* + initialise driver + */ + bool init(); + + /* + read data from the FIFOs + */ + void read_fifo_accel(); + void read_fifo_gyro(); + + AP_HAL::OwnPtr dev_accel; + AP_HAL::OwnPtr dev_gyro; + + uint8_t accel_instance; + uint8_t gyro_instance; + enum Rotation rotation; + uint8_t temperature_counter; +}; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 804e408494..1bc4e540ae 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -104,6 +104,7 @@ public: DEVTYPE_GYR_LSM9DS1 = 0x26, DEVTYPE_INS_ICM20789 = 0x27, DEVTYPE_INS_ICM20689 = 0x28, + DEVTYPE_INS_BMI055 = 0x29, }; protected: