From d301c22665b87b9263bc8aca1a6e7312761af172 Mon Sep 17 00:00:00 2001 From: Karl Schwabe Date: Wed, 31 Jul 2019 10:44:07 +0200 Subject: [PATCH] Bosch BMI088 initial driver --- boards/px4/fmu-v5x/default.cmake | 2 +- src/drivers/drv_sensor.h | 1 + src/drivers/imu/bmi088/BMI088.hpp | 96 ++++ src/drivers/imu/bmi088/BMI088_accel.cpp | 614 ++++++++++++++++++++++++ src/drivers/imu/bmi088/BMI088_accel.hpp | 268 +++++++++++ src/drivers/imu/bmi088/BMI088_gyro.cpp | 506 +++++++++++++++++++ src/drivers/imu/bmi088/BMI088_gyro.hpp | 261 ++++++++++ src/drivers/imu/bmi088/CMakeLists.txt | 43 ++ src/drivers/imu/bmi088/bmi088_main.cpp | 413 ++++++++++++++++ 9 files changed, 2203 insertions(+), 1 deletion(-) create mode 100644 src/drivers/imu/bmi088/BMI088.hpp create mode 100644 src/drivers/imu/bmi088/BMI088_accel.cpp create mode 100644 src/drivers/imu/bmi088/BMI088_accel.hpp create mode 100644 src/drivers/imu/bmi088/BMI088_gyro.cpp create mode 100644 src/drivers/imu/bmi088/BMI088_gyro.hpp create mode 100644 src/drivers/imu/bmi088/CMakeLists.txt create mode 100644 src/drivers/imu/bmi088/bmi088_main.cpp diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 2483395f09..eae18644ba 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -30,7 +30,7 @@ px4_add_board( imu/adis16448 imu/adis16497 #imu # all available imu drivers -# TBD imu/bmi088 - needs bus selection + imu/bmi088 # TBD imu/ism330dlc - needs bus selection imu/mpu6000 irlock diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5428e0871a..4f5b23d3cc 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -113,6 +113,7 @@ #define DRV_ACC_DEVTYPE_ADIS16497 0x63 #define DRV_GYR_DEVTYPE_ADIS16497 0x64 #define DRV_BARO_DEVTYPE_BAROSIM 0x65 +#define DRV_DEVTYPE_BMI088 0x66 /* * ioctl() definitions diff --git a/src/drivers/imu/bmi088/BMI088.hpp b/src/drivers/imu/bmi088/BMI088.hpp new file mode 100644 index 0000000000..d21ea014cf --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +//Soft-reset command Value +#define BMI088_SOFT_RESET 0xB6 + +#define BMI088_BUS_SPEED 10*1000*1000 + +#define BMI088_TIMER_REDUCTION 200 + +class BMI088 : public device::SPI +{ + +protected: + + uint8_t _whoami; /** whoami result */ + + uint8_t _register_wait; + uint64_t _reset_wait; + + enum Rotation _rotation; + + uint8_t _checked_next; + + /** + * Read a register from the BMI088 + * + * @param The register to read. + * @return The value that was read. + */ + virtual uint8_t read_reg(unsigned reg); // This needs to be declared as virtual, because the + virtual uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the BMI088 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /* do not allow to copy this class due to pointer data members */ + BMI088(const BMI088 &); + BMI088 operator=(const BMI088 &); + +public: + + BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency, + enum Rotation rotation); + + virtual ~BMI088() = default; + + +}; diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp new file mode 100644 index 0000000000..14d4320870 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_accel.cpp @@ -0,0 +1,614 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMI088_accel.hpp" + +/* + * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver. The variable is changed in bmi055_accel.cpp. + * This is a HACK! The driver should be rewritten with the gyro as subdriver. + */ +extern float _accel_last_temperature_copy; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS] = {BMI088_ACC_CHIP_ID, + BMI088_ACC_CONF, + BMI088_ACC_RANGE, + BMI088_ACC_INT1_IO_CONF, + BMI088_ACC_INT1_INT2_MAP_DATA, + BMI088_ACC_PWR_CONF, + BMI088_ACC_PWR_CTRL, + }; + +BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : + BMI088("BMI088_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")), + _measure_interval(perf_alloc(PC_INTERVAL, "bmi088_accel_measure_interval")), + _bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")), + _duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")), + _got_duplicate(false) +{ + _px4_accel.set_device_type(DRV_DEVTYPE_BMI088); +} + +BMI088_accel::~BMI088_accel() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_measure_interval); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_duplicates); +} + +int +BMI088_accel::init() +{ + /* do SPI init (and probe) first */ + int ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + return reset(); +} + +uint8_t +BMI088_accel::read_reg(unsigned reg) +{ + // For the BMI088, you need to read out a dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet) + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0}; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[2]; // Skip dummy byte in cmd[1] and read out actual data +} + +uint16_t +BMI088_accel::read_reg16(unsigned reg) +{ + // For the BMI088, you need to read out the dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet) + uint8_t cmd[4] = { (uint8_t)(reg | DIR_READ), 0, 0, 0 }; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[2] << 8) | cmd[3]; // Skip dummy byte in cmd[1] +} + +int BMI088_accel::reset() +{ + write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); // Soft-reset + /* After a POR or soft-reset, the sensor needs up to 1ms boot time + * (see section "Power Modes: Accelerometer" in the BMI088 datasheet). + * Based off of testing it seems this value needs to be increased from 1ms. + * Setting it to 5ms. + */ + + up_udelay(5000); + + // Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset + // The dummy read basically pulls the chip select line low and then high + // See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details. + read_reg(BMI088_ACC_CHIP_ID); + + // Enable Accelerometer + // The accelerometer needs to be enabled first, before writing to its registers + write_checked_reg(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_CTRL_EN); + /* After changing power modes, the sensor requires up to 5ms to settle. + * Any communication with the sensor during this time should be avoided + * (see section "Power Modes: Acceleromter" in the BMI datasheet) */ + + up_udelay(5000); + + // Set the PWR CONF to be active + write_checked_reg(BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_CONF_ACTIVE); // Sets the accelerometer to active mode + + // Write accel bandwidth and output data rate + // ToDo set the bandwidth + accel_set_sample_rate(BMI088_ACCEL_DEFAULT_RATE); //set accel ODR + set_accel_range(BMI088_ACCEL_DEFAULT_RANGE_G); //set accel range + + // Configure the accel INT1 + write_checked_reg(BMI088_ACC_INT1_IO_CONF, + BMI088_ACC_INT1_IO_CONF_INT1_OUT | BMI088_ACC_INT1_IO_CONF_PP | + BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH); // Configure INT1 pin as output, push-pull, active high + write_checked_reg(BMI088_ACC_INT1_INT2_MAP_DATA, + BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY); // Map DRDY interrupt on pin INT1 + + uint8_t retries = 10; + + while (retries--) { + bool all_ok = true; + + for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) { + if (read_reg(_checked_registers[i]) != _checked_values[i]) { + write_reg(_checked_registers[i], _checked_values[i]); + all_ok = false; + } + } + + if (all_ok) { + break; + } + } + + return OK; +} + +int +BMI088_accel::probe() +{ + // Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset + // The dummy read basically pulls the chip select line low and then high + // See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details. + read_reg(BMI088_ACC_CHIP_ID); + + /* look for device ID */ + _whoami = read_reg(BMI088_ACC_CHIP_ID); + + // verify product revision + switch (_whoami) { + case BMI088_ACC_WHO_AM_I: + memset(_checked_values, 0, sizeof(_checked_values)); + memset(_checked_bad, 0, sizeof(_checked_bad)); + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + return OK; + } + + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + return -EIO; +} + +int +BMI088_accel::accel_set_sample_rate(float frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = 0x0F; + + if (frequency < 25) { + setbits |= BMI088_ACC_CONF_ODR_12_5; + //_accel_sample_rate = 12.5f; + + } else if (frequency < 50) { + setbits |= BMI088_ACC_CONF_ODR_25; + //_accel_sample_rate = 25.f; + + } else if (frequency < 100) { + setbits |= BMI088_ACC_CONF_ODR_50; + //_accel_sample_rate = 50.f; + + } else if (frequency < 200) { + setbits |= BMI088_ACC_CONF_ODR_100; + //_accel_sample_rate = 100.f; + + } else if (frequency < 400) { + setbits |= BMI088_ACC_CONF_ODR_200; + //_accel_sample_rate = 200.f; + + } else if (frequency < 800) { + setbits |= BMI088_ACC_CONF_ODR_400; + //_accel_sample_rate = 400.f; + + } else if (frequency < 1600) { + setbits |= BMI088_ACC_CONF_ODR_800; + //_accel_sample_rate = 800.f; + + } else if (frequency >= 1600) { + setbits |= BMI088_ACC_CONF_ODR_1600; + //_accel_sample_rate = 1600.f; + + } else { + printf("Set sample rate error \n"); + return -EINVAL; + } + + /* Write accel ODR */ + modify_reg(BMI088_ACC_CONF, clearbits, setbits); + + return OK; +} + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +BMI088_accel::test_error() +{ + write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); + ::printf("error triggered\n"); + print_registers(); +} + +void +BMI088_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +BMI088_accel::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + } + } +} + +int +BMI088_accel::set_accel_range(unsigned max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI088_ACCEL_RANGE_24_G; + float lsb_per_g; + + if (max_g == 0) { + max_g = 24; + } + + if (max_g <= 3) { + //max_accel_g = 3; + setbits |= BMI088_ACCEL_RANGE_3_G; + lsb_per_g = 10922.67; + + } else if (max_g <= 6) { + //max_accel_g = 6; + setbits |= BMI088_ACCEL_RANGE_6_G; + lsb_per_g = 5461.33; + + } else if (max_g <= 12) { + //max_accel_g = 12; + setbits |= BMI088_ACCEL_RANGE_12_G; + lsb_per_g = 2730.67; + + } else if (max_g <= 24) { + //max_accel_g = 24; + setbits |= BMI088_ACCEL_RANGE_24_G; + lsb_per_g = 1365.33; + + } else { + return -EINVAL; + } + + _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g); + + modify_reg(BMI088_ACC_RANGE, clearbits, setbits); + + return OK; +} + +void +BMI088_accel::start() +{ + /* make sure we are stopped first */ + stop(); + + // Reset the accelerometer + reset(); + + /* start polling at the specified rate */ + ScheduleOnInterval(BMI088_ACCEL_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000); + +} + +void +BMI088_accel::stop() +{ + ScheduleClear(); +} + +void +BMI088_accel::Run() +{ + /* make another measurement */ + measure(); +} + +void +BMI088_accel::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % BMI088_ACCEL_NUM_CHECKED_REGISTERS; +} + +void +BMI088_accel::measure() +{ + perf_count(_measure_interval); + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + // Checking the status of new data + uint8_t status; + status = read_reg(BMI088_ACC_STATUS); + + if (!(status & BMI088_ACC_STATUS_DRDY)) { + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + + _got_duplicate = false; + + /* + * Fetch the full set of measurements from the BMI088 in one pass. + */ + uint8_t index = 0; + uint8_t accel_data[8]; // Need an extra byte for the command, and an an extra dummy byte for the read (see section "SPI interface of accelerometer part" of the BMI088 datasheet) + accel_data[index] = BMI088_ACC_X_L | DIR_READ; + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) { + return; + } + + check_registers(); + + /* Extracting accel data from the read data */ + index = 2; // Skip the dummy byte at index=1 + uint16_t lsb, msb, msblsb; + + lsb = (uint16_t)accel_data[index++]; + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_x = (int16_t)msblsb; /* Data in X axis */ + + lsb = (uint16_t)accel_data[index++]; + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_y = (int16_t)msblsb; /* Data in Y axis */ + + lsb = (uint16_t)accel_data[index++]; + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_z = (int16_t)msblsb; /* Data in Z axis */ + + // Extract the temperature data + // Note: the temp sensor data is only updated every 1.28s (see "Register 0x22-0x23 Temperature Sensor Data" section in BMI088 Datasheet) + index = 0; + accel_data[index] = BMI088_ACC_TEMP_H | DIR_READ; + + // Need to perform a dummy read, hence the num bytes to read is 3 (plus 1 send byte) + if (OK != transfer(accel_data, accel_data, 4)) { + return; + } + + index = 2; + msb = (uint16_t)accel_data[index++]; + lsb = (uint16_t)accel_data[index++]; + uint16_t temp = msb * 8 + lsb / 32; + + if (temp > 1023) { + report.temp = temp - 2048; + + } else { + report.temp = temp; + } + + + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the bmi088 accel does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again, but don't return any data yet + _register_wait--; + return; + } + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_accel.set_error_count(error_count); + + // Convert the bit-wise representation of temperature to degrees C + _accel_last_temperature_copy = (report.temp * 0.125f) + 23.0f; + _px4_accel.set_temperature(_accel_last_temperature_copy); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + */ + _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +BMI088_accel::print_info() +{ + PX4_INFO("Accel"); + + perf_print_counter(_sample_perf); + perf_print_counter(_measure_interval); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + perf_print_counter(_duplicates); + + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + + if (v != _checked_bad[i]) { + ::printf("reg %02x:%02x was bad %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_bad[i]); + } + } + + _px4_accel.print_status(); +} + +void +BMI088_accel::print_registers() +{ + uint8_t index = 0; + printf("BMI088 accel registers\n"); + + uint8_t reg = _checked_registers[index++]; + uint8_t v = read_reg(reg); + printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Conf: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Int1 Conf: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Int1-Int2_Map-Data: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Pwr Conf: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Pwr Ctrl: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + + + printf("\n"); +} diff --git a/src/drivers/imu/bmi088/BMI088_accel.hpp b/src/drivers/imu/bmi088/BMI088_accel.hpp new file mode 100644 index 0000000000..20e89ff009 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_accel.hpp @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "BMI088.hpp" + +#include +#include + +#define BMI088_DEVICE_PATH_ACCEL "/dev/bmi088_accel" +#define BMI088_DEVICE_PATH_ACCEL_EXT "/dev/bmi088_accel_ext" + +// BMI088 Accel registers +#define BMI088_ACC_CHIP_ID 0x00 + +#define BMI088_ACC_ERR_REG 0x02 +#define BMI088_ACC_STATUS 0x03 +#define BMI088_ACC_X_L 0x12 +#define BMI088_ACC_X_H 0x13 +#define BMI088_ACC_Y_L 0x14 +#define BMI088_ACC_Y_H 0x15 +#define BMI088_ACC_Z_L 0x16 +#define BMI088_ACC_Z_H 0x17 +#define BMI088_ACC_SENSORTIME_0 0x18 +#define BMI088_ACC_SENSORTIME_1 0x19 +#define BMI088_ACC_SENSORTIME_2 0x1A +#define BMI088_ACC_INT_STAT_1 0x1D +#define BMI088_ACC_TEMP_H 0x22 +#define BMI088_ACC_TEMP_L 0x23 +#define BMI088_ACC_CONF 0x40 +#define BMI088_ACC_RANGE 0x41 +#define BMI088_ACC_INT1_IO_CONF 0x53 +#define BMI088_ACC_INT2_IO_CONF 0x54 +#define BMI088_ACC_INT1_INT2_MAP_DATA 0x58 +#define BMI088_ACC_SELF_TEST 0x6D +#define BMI088_ACC_PWR_CONF 0x7C +#define BMI088_ACC_PWR_CTRL 0x7D +#define BMI088_ACC_SOFTRESET 0x7E + +// BMI088 Accelerometer Chip-Id +#define BMI088_ACC_WHO_AM_I 0x1E + +// BMI088_ACC_ERR_REG 0x02 +#define BMI088_ACC_ERR_REG_NO_ERROR (0x00<<2) +#define BMI088_ACC_ERR_REG_ERROR (0x01<<2) + +#define BMI088_ACC_ERR_REG_FATAL_ERROR (0x01<<0) + +// BMI088_ACC_STATUS 0x03 +#define BMI088_ACC_STATUS_DRDY (0x01<<7) + +// BMI088_ACC_INT_STAT_1 0x01D +#define BMI088_ACC_INT_STAT_1_DRDY (0x01<<7) + +// BMI088_ACC_CONF 0x40 +#define BMI088_ACC_CONF_BWP_4 (0x08<<4) +#define BMI088_ACC_CONF_BWP_2 (0x09<<4) +#define BMI088_ACC_CONF_BWP_NORMAL (0x0A<<4) + +#define BMI088_ACC_CONF_ODR_12_5 (0x05<<0) +#define BMI088_ACC_CONF_ODR_25 (0x06<<0) +#define BMI088_ACC_CONF_ODR_50 (0x07<<0) +#define BMI088_ACC_CONF_ODR_100 (0x08<<0) +#define BMI088_ACC_CONF_ODR_200 (0x09<<0) +#define BMI088_ACC_CONF_ODR_400 (0x0A<<0) +#define BMI088_ACC_CONF_ODR_800 (0x0B<<0) +#define BMI088_ACC_CONF_ODR_1600 (0x0C<<0) + +// BMI088_ACC_RANGE 0x41 +#define BMI088_ACCEL_RANGE_3_G (0x00<<0) +#define BMI088_ACCEL_RANGE_6_G (0x01<<0) +#define BMI088_ACCEL_RANGE_12_G (0x02<<0) +#define BMI088_ACCEL_RANGE_24_G (0x03<<0) + +// BMI088_ACC_INT1_IO_CONF 0x53 +#define BMI088_ACC_INT1_IO_CONF_INT1_IN (0x01<<4) + +#define BMI088_ACC_INT1_IO_CONF_INT1_OUT (0x01<<3) + +#define BMI088_ACC_INT1_IO_CONF_PP (0x00<<2) +#define BMI088_ACC_INT1_IO_CONF_OD (0x01<<2) + +#define BMI088_ACC_INT1_IO_CONF_ACTIVE_LOW (0x00<<1) +#define BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH (0x01<<1) + +// BMI088_ACC_INT2_IO_CONF 0x54 +#define BMI088_ACC_INT2_IO_CONF_INT1_IN (0x01<<4) + +#define BMI088_ACC_INT2_IO_CONF_INT1_OUT (0x01<<3) + +#define BMI088_ACC_INT2_IO_CONF_PP (0x00<<2) +#define BMI088_ACC_INT2_IO_CONF_OD (0x01<<2) + +#define BMI088_ACC_INT2_IO_CONF_ACTIVE_LOW (0x00<<1) +#define BMI088_ACC_INT2_IO_CONF_ACTIVE_HIGH (0x01<<1) + +// BMI088_ACC_INT1_INT2_MAP_DATA 0x54 +#define BMI088_ACC_INT1_INT2_MAP_DATA_INT2_DRDY (0x01<<6) +#define BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY (0x01<<2) + +// BMI088_ACC_SELF_TEST 0x6D +#define BMI088_ACC_SELF_TEST_OFF (0x00<<0) +#define BMI088_ACC_SELF_TEST_POSITIVE (0x0D<<0) +#define BMI088_ACC_SELF_TEST_NEGATIVE (0x09<<0) + +// BMI088_ACC_PWR_CONF 0x7C +#define BMI088_ACC_PWR_CONF_SUSPEND (0x03<<0) +#define BMI088_ACC_PWR_CONF_ACTIVE (0x00<<0) + +// BMI088_ACC_PWR_CTRL 0x7D +#define BMI088_ACC_PWR_CTRL_EN (0x04<<0) + + +/////// +// To Do check these defaults and maks below +/// + +// Default and Max values +#define BMI088_ACCEL_DEFAULT_RANGE_G 24 +#define BMI088_ACCEL_DEFAULT_RATE 800 +#define BMI088_ACCEL_MAX_RATE 800 +#define BMI088_ACCEL_MAX_PUBLISH_RATE 800 + +#define BMI088_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50 + +class BMI088_accel : public BMI088, public px4::ScheduledWorkItem +{ +public: + BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation); + virtual ~BMI088_accel(); + + virtual int init(); + + // Start automatic measurement. + void start(); + + // We need to override the read_reg function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation + virtual uint8_t read_reg(unsigned reg); + + // We need to override the read_reg16 function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation + virtual uint16_t read_reg16(unsigned reg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + + virtual int probe(); + +private: + + PX4Accelerometer _px4_accel; + + perf_counter_t _sample_perf; + perf_counter_t _measure_interval; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + perf_counter_t _duplicates; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define BMI088_ACCEL_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[BMI088_ACCEL_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[BMI088_ACCEL_NUM_CHECKED_REGISTERS]; + + bool _got_duplicate; + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + void Run() override; + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Modify a register in the BMI088_accel + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the BMI088_accel, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the BMI088_accel measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -EINVAL otherwise. + */ + int set_accel_range(unsigned max_g); + + /** + * Set accel sample rate + */ + int accel_set_sample_rate(float desired_sample_rate_hz); + + /* + * check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + BMI088_accel(const BMI088_accel &); + BMI088_accel operator=(const BMI088_accel &); + +}; diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp new file mode 100644 index 0000000000..8068003694 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp @@ -0,0 +1,506 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMI088_gyro.hpp" +#include "BMI088_accel.hpp" + + +/* + * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver. + * This is a ugly HACK! The driver should potentially be rewritten with the gyro as subdriver. + */ +__EXPORT float _accel_last_temperature_copy = 0; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ + +const uint8_t BMI088_gyro::_checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS] = { BMI088_GYR_CHIP_ID, + BMI088_GYR_LPM1, + BMI088_GYR_BW, + BMI088_GYR_RANGE, + BMI088_GYR_INT_EN_0, + BMI088_GYR_INT_EN_1, + BMI088_GYR_INT_MAP_1 + }; + +BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : + BMI088("BMI088_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), + _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")), + _measure_interval(perf_alloc(PC_INTERVAL, "bmi088_gyro_measure_interval")), + _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")) +{ + _px4_gyro.set_device_type(DRV_DEVTYPE_BMI088); +} + +BMI088_gyro::~BMI088_gyro() +{ + /* make sure we are truly inactive */ + stop(); + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_measure_interval); + perf_free(_bad_transfers); + perf_free(_bad_registers); +} + +int +BMI088_gyro::init() +{ + /* do SPI init (and probe) first */ + int ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + return reset(); +} + +int BMI088_gyro::reset() +{ + write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);//Soft-reset + usleep(5000); + write_checked_reg(BMI088_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate()) + write_checked_reg(BMI088_GYR_RANGE, 0);// Write Gyro range + write_checked_reg(BMI088_GYR_INT_EN_0, BMI088_GYR_DRDY_INT_EN); //Enable DRDY interrupt + write_checked_reg(BMI088_GYR_INT_MAP_1, BMI088_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1 + + set_gyro_range(BMI088_GYRO_DEFAULT_RANGE_DPS);// set Gyro range + gyro_set_sample_rate(BMI088_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth + + //Enable Gyroscope in normal mode + write_reg(BMI088_GYR_LPM1, BMI088_GYRO_NORMAL); + up_udelay(1000); + + uint8_t retries = 10; + + while (retries--) { + bool all_ok = true; + + for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) { + if (read_reg(_checked_registers[i]) != _checked_values[i]) { + write_reg(_checked_registers[i], _checked_values[i]); + all_ok = false; + } + } + + if (all_ok) { + break; + } + } + + return OK; +} + +int +BMI088_gyro::probe() +{ + /* look for device ID */ + _whoami = read_reg(BMI088_GYR_CHIP_ID); + + // verify product revision + switch (_whoami) { + case BMI088_GYR_WHO_AM_I: + memset(_checked_values, 0, sizeof(_checked_values)); + memset(_checked_bad, 0, sizeof(_checked_bad)); + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + return OK; + } + + printf("unexpected whoami 0x%02x\n", _whoami); + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + return -EIO; +} + +int +BMI088_gyro::gyro_set_sample_rate(float frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI088_GYRO_BW_MASK; + + if (frequency <= 100) { + setbits |= BMI088_GYRO_RATE_100; /* 32 Hz cutoff */ + //_gyro_sample_rate = 100; + + } else if (frequency <= 250) { + setbits |= BMI088_GYRO_RATE_400; /* 47 Hz cutoff */ + //_gyro_sample_rate = 400; + + } else if (frequency <= 1000) { + setbits |= BMI088_GYRO_RATE_1000; /* 116 Hz cutoff */ + //_gyro_sample_rate = 1000; + + } else if (frequency > 1000) { + setbits |= BMI088_GYRO_RATE_2000; /* 230 Hz cutoff */ + //_gyro_sample_rate = 2000; + + } else { + return -EINVAL; + } + + modify_reg(BMI088_GYR_BW, clearbits, setbits); + + return OK; +} + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +BMI088_gyro::test_error() +{ + write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET); + ::printf("error triggered\n"); + print_registers(); +} + +void +BMI088_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +BMI088_gyro::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + } + } +} + +int +BMI088_gyro::set_gyro_range(unsigned max_dps) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI088_GYRO_RANGE_125_DPS | BMI088_GYRO_RANGE_250_DPS; + float lsb_per_dps; + + if (max_dps == 0) { + max_dps = 2000; + } + + if (max_dps <= 125) { + //max_gyro_dps = 125; + lsb_per_dps = 262.4; + setbits |= BMI088_GYRO_RANGE_125_DPS; + + } else if (max_dps <= 250) { + //max_gyro_dps = 250; + lsb_per_dps = 131.2; + setbits |= BMI088_GYRO_RANGE_250_DPS; + + } else if (max_dps <= 500) { + //max_gyro_dps = 500; + lsb_per_dps = 65.6; + setbits |= BMI088_GYRO_RANGE_500_DPS; + + } else if (max_dps <= 1000) { + //max_gyro_dps = 1000; + lsb_per_dps = 32.8; + setbits |= BMI088_GYRO_RANGE_1000_DPS; + + } else if (max_dps <= 2000) { + //max_gyro_dps = 2000; + lsb_per_dps = 16.4; + setbits |= BMI088_GYRO_RANGE_2000_DPS; + + } else { + return -EINVAL; + } + + _px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps)); + + modify_reg(BMI088_GYR_RANGE, clearbits, setbits); + + return OK; +} + +void +BMI088_gyro::start() +{ + /* make sure we are stopped first */ + stop(); + + /* start polling at the specified rate */ + ScheduleOnInterval(BMI088_GYRO_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000); +} + +void +BMI088_gyro::stop() +{ + ScheduleClear(); +} + +void +BMI088_gyro::Run() +{ + /* make another measurement */ + measure(); +} + +void +BMI088_gyro::measure_trampoline(void *arg) +{ + BMI088_gyro *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +BMI088_gyro::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET); + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % BMI088_GYRO_NUM_CHECKED_REGISTERS; +} + +void +BMI088_gyro::measure() +{ + perf_count(_measure_interval); + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct BMI_GyroReport bmi_gyroreport; + + struct Report { + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the BMI088 gyro in one pass. + */ + bmi_gyroreport.cmd = BMI088_GYR_X_L | DIR_READ; + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) { + return; + } + + check_registers(); + + + // Get the last temperature from the accelerometer (the Gyro does not have its own temperature measurement) + _last_temperature = _accel_last_temperature_copy; + + report.gyro_x = bmi_gyroreport.gyro_x; + report.gyro_y = bmi_gyroreport.gyro_y; + report.gyro_z = bmi_gyroreport.gyro_z; + + if (report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably an SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the bmi088 does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again, but don't return any data yet + _register_wait--; + return; + } + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_gyro.set_error_count(error_count); + + // Get the temperature from the accelerometer part of the BMI088, because the gyro part does not have a temperature register + _px4_gyro.set_temperature(_accel_last_temperature_copy); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +BMI088_gyro::print_info() +{ + PX4_INFO("Gyro"); + + perf_print_counter(_sample_perf); + perf_print_counter(_measure_interval); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + + if (v != _checked_bad[i]) { + ::printf("reg %02x:%02x was bad %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_bad[i]); + } + } + + _px4_gyro.print_status(); +} + +void +BMI088_gyro::print_registers() +{ + uint8_t index = 0; + printf("BMI088 gyro registers\n"); + + uint8_t reg = _checked_registers[index++]; + uint8_t v = read_reg(reg); + printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + + printf("\n"); +} diff --git a/src/drivers/imu/bmi088/BMI088_gyro.hpp b/src/drivers/imu/bmi088/BMI088_gyro.hpp new file mode 100644 index 0000000000..7b7c73b622 --- /dev/null +++ b/src/drivers/imu/bmi088/BMI088_gyro.hpp @@ -0,0 +1,261 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "BMI088.hpp" + +#define BMI088_DEVICE_PATH_GYRO "/dev/bmi088_gyro" +#define BMI088_DEVICE_PATH_GYRO_EXT "/dev/bmi088_gyro_ext" + +// BMI088 Gyro registers +#define BMI088_GYR_CHIP_ID 0x00 +#define BMI088_GYR_X_L 0x02 +#define BMI088_GYR_X_H 0x03 +#define BMI088_GYR_Y_L 0x04 +#define BMI088_GYR_Y_H 0x05 +#define BMI088_GYR_Z_L 0x06 +#define BMI088_GYR_Z_H 0x07 +#define BMI088_GYR_INT_STATUS_0 0x09 +#define BMI088_GYR_INT_STATUS_1 0x0A +#define BMI088_GYR_INT_STATUS_2 0x0B +#define BMI088_GYR_INT_STATUS_3 0x0C +#define BMI088_GYR_FIFO_STATUS 0x0E +#define BMI088_GYR_RANGE 0x0F +#define BMI088_GYR_BW 0x10 +#define BMI088_GYR_LPM1 0x11 +#define BMI088_GYR_LPM2 0x12 +#define BMI088_GYR_RATE_HBW 0x13 +#define BMI088_GYR_SOFTRESET 0x14 +#define BMI088_GYR_INT_EN_0 0x15 +#define BMI088_GYR_INT_EN_1 0x16 +#define BMI088_GYR_INT_MAP_0 0x17 +#define BMI088_GYR_INT_MAP_1 0x18 +#define BMI088_GYR_INT_MAP_2 0x19 +#define BMI088_GYRO_0_REG 0x1A +#define BMI088_GYRO_1_REG 0x1B +#define BMI088_GYRO_2_REG 0x1C +#define BMI088_GYRO_3_REG 0x1E +#define BMI088_GYR_INT_LATCH 0x21 +#define BMI088_GYR_INT_LH_0 0x22 +#define BMI088_GYR_INT_LH_1 0x23 +#define BMI088_GYR_INT_LH_2 0x24 +#define BMI088_GYR_INT_LH_3 0x25 +#define BMI088_GYR_INT_LH_4 0x26 +#define BMI088_GYR_INT_LH_5 0x27 +#define BMI088_GYR_SOC 0x31 +#define BMI088_GYR_A_FOC 0x32 +#define BMI088_GYR_TRIM_NVM_CTRL 0x33 +#define BMI088_BGW_SPI3_WDT 0x34 +#define BMI088_GYR_OFFSET_COMP 0x36 +#define BMI088_GYR_OFFSET_COMP_X 0x37 +#define BMI088_GYR_OFFSET_COMP_Y 0x38 +#define BMI088_GYR_OFFSET_COMP_Z 0x39 +#define BMI088_GYR_TRIM_GPO 0x3A +#define BMI088_GYR_TRIM_GP1 0x3B +#define BMI088_GYR_SELF_TEST 0x3C +#define BMI088_GYR_FIFO_CONFIG_0 0x3D +#define BMI088_GYR_FIFO_CONFIG_1 0x3E +#define BMI088_GYR_FIFO_DATA 0x3F + +// BMI088 Gyroscope Chip-Id +#define BMI088_GYR_WHO_AM_I 0x0F + +//ODR & DLPF filter bandwidth settings (they are coupled) +#define BMI088_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0) +#define BMI088_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0) +#define BMI088_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0) +#define BMI088_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0) +#define BMI088_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0) + +//BMI088_GYR_LPM1 0x11 +#define BMI088_GYRO_NORMAL (0<<7) | (0<<5) +#define BMI088_GYRO_DEEP_SUSPEND (0<<7) | (1<<5) +#define BMI088_GYRO_SUSPEND (1<<7) | (0<<5) + +//BMI088_GYR_RANGE 0x0F +#define BMI088_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0) +#define BMI088_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0) +#define BMI088_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0) +#define BMI088_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0) +#define BMI088_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0) + +//BMI088_GYR_INT_EN_0 0x15 +#define BMI088_GYR_DRDY_INT_EN (1<<7) + +//BMI088_GYR_INT_MAP_1 0x18 +#define BMI088_GYR_DRDY_INT1 (1<<0) + +// Default and Max values +#define BMI088_GYRO_DEFAULT_RANGE_DPS 2000 +#define BMI088_GYRO_DEFAULT_RATE 1000 +#define BMI088_GYRO_MAX_RATE 1000 +#define BMI088_GYRO_MAX_PUBLISH_RATE 280 + +#define BMI088_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50 + +/* Mask definitions for Gyro bandwidth */ +#define BMI088_GYRO_BW_MASK 0x0F + +class BMI088_gyro : public BMI088, public px4::ScheduledWorkItem +{ +public: + BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation); + virtual ~BMI088_gyro(); + + virtual int init(); + + // Start automatic measurement. + void start(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + + virtual int probe(); + +private: + + PX4Gyroscope _px4_gyro; + + perf_counter_t _sample_perf; + perf_counter_t _measure_interval; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define BMI088_GYRO_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[BMI088_GYRO_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[BMI088_GYRO_NUM_CHECKED_REGISTERS]; + + // last temperature reading for print_info() + float _last_temperature; + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + void Run() override; + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Modify a register in the BMI088_gyro + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the BMI088_gyro, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the BMI088_gyro measurement range. + * + * @param max_dps The maximum DPS value the range must support. + * @return OK if the value can be supported, -EINVAL otherwise. + */ + int set_gyro_range(unsigned max_dps); + + /* + * set gyro sample rate + */ + int gyro_set_sample_rate(float desired_sample_rate_hz); + + /* + * check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + BMI088_gyro(const BMI088_gyro &); + BMI088_gyro operator=(const BMI088_gyro &); + +#pragma pack(push, 1) + /** + * Report conversation within the BMI088_gyro, including command byte and + * interrupt status. + */ + struct BMI_GyroReport { + uint8_t cmd; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + }; +#pragma pack(pop) + +}; diff --git a/src/drivers/imu/bmi088/CMakeLists.txt b/src/drivers/imu/bmi088/CMakeLists.txt new file mode 100644 index 0000000000..74cafdc933 --- /dev/null +++ b/src/drivers/imu/bmi088/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__bmi88 + MAIN bmi088 + STACK_MAIN 1500 + COMPILE_FLAGS + -Wno-cast-align # TODO: fix and enable + SRCS + BMI088_accel.cpp + BMI088_gyro.cpp + bmi088_main.cpp + ) diff --git a/src/drivers/imu/bmi088/bmi088_main.cpp b/src/drivers/imu/bmi088/bmi088_main.cpp new file mode 100644 index 0000000000..fe4e1a74ee --- /dev/null +++ b/src/drivers/imu/bmi088/bmi088_main.cpp @@ -0,0 +1,413 @@ +/**************************************************************************** + * + * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BMI088_accel.hpp" +#include "BMI088_gyro.hpp" + +/** driver 'main' command */ +extern "C" { __EXPORT int bmi088_main(int argc, char *argv[]); } + +enum sensor_type { + BMI088_NONE = 0, + BMI088_ACCEL = 1, + BMI088_GYRO +}; + +namespace bmi088 +{ + +BMI088_accel *g_acc_dev_int; // on internal bus (accel) +BMI088_accel *g_acc_dev_ext; // on external bus (accel) +BMI088_gyro *g_gyr_dev_int; // on internal bus (gyro) +BMI088_gyro *g_gyr_dev_ext; // on external bus (gyro) + +void start(bool, enum Rotation, enum sensor_type); +void stop(bool, enum sensor_type); +void info(bool, enum sensor_type); +void regdump(bool, enum sensor_type); +void testerror(bool, enum sensor_type); +void usage(); + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(bool external_bus, enum Rotation rotation, enum sensor_type sensor) +{ + + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + const char *path_accel = external_bus ? BMI088_DEVICE_PATH_ACCEL_EXT : BMI088_DEVICE_PATH_ACCEL; + + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + const char *path_gyro = external_bus ? BMI088_DEVICE_PATH_GYRO_EXT : BMI088_DEVICE_PATH_GYRO; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr != nullptr) + /* if already started, the still command succeeded */ + { + errx(0, "bmi088 accel sensor already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_EXT, path_accel, PX4_SPIDEV_EXT_BMI, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_acc_ptr = new BMI088_accel(PX4_SPI_BUS_SENSORS3, path_accel, PX4_SPIDEV_BMI088_ACC, rotation); + } + + if (*g_dev_acc_ptr == nullptr) { + goto fail_accel; + } + + if (OK != (*g_dev_acc_ptr)->init()) { + goto fail_accel; + } + + // start automatic data collection + (*g_dev_acc_ptr)->start(); + } + + if (sensor == BMI088_GYRO) { + + if (*g_dev_gyr_ptr != nullptr) { + errx(0, "bmi088 gyro sensor already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_ptr = new BMI088_gyro(PX4_SPI_BUS_EXT, path_gyro, PX4_SPIDEV_EXT_BMI, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_gyr_ptr = new BMI088_gyro(PX4_SPI_BUS_SENSORS3, path_gyro, PX4_SPIDEV_BMI088_GYR, rotation); + } + + if (*g_dev_gyr_ptr == nullptr) { + goto fail_gyro; + } + + if (OK != (*g_dev_gyr_ptr)->init()) { + goto fail_gyro; + } + + // start automatic data collection + (*g_dev_gyr_ptr)->start(); + } + + exit(PX4_OK); + +fail_accel: + + if (*g_dev_acc_ptr != nullptr) { + delete (*g_dev_acc_ptr); + *g_dev_acc_ptr = nullptr; + } + + PX4_WARN("No BMI088 accel found"); + exit(PX4_ERROR); + +fail_gyro: + + if (*g_dev_gyr_ptr != nullptr) { + delete (*g_dev_gyr_ptr); + *g_dev_gyr_ptr = nullptr; + } + + PX4_WARN("No BMI088 gyro found"); + exit(PX4_ERROR); +} + +void +stop(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr != nullptr) { + delete *g_dev_acc_ptr; + *g_dev_acc_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("bmi088 accel sensor already stopped."); + } + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr != nullptr) { + delete *g_dev_gyr_ptr; + *g_dev_gyr_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("bmi088 gyro sensor already stopped."); + } + } + + exit(0); + +} + +/** + * Print a little info about the driver. + */ +void +info(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi088 accel driver not running"); + } + + printf("state @ %p\n", *g_dev_acc_ptr); + (*g_dev_acc_ptr)->print_info(); + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi088 gyro driver not running"); + } + + printf("state @ %p\n", *g_dev_gyr_ptr); + (*g_dev_gyr_ptr)->print_info(); + } + + exit(0); +} + +/** + * Dump the register information + */ +void +regdump(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi088 accel driver not running"); + } + + printf("regdump @ %p\n", *g_dev_acc_ptr); + (*g_dev_acc_ptr)->print_registers(); + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi088 gyro driver not running"); + } + + printf("regdump @ %p\n", *g_dev_gyr_ptr); + (*g_dev_gyr_ptr)->print_registers(); + } + + exit(0); +} + +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus, enum sensor_type sensor) +{ + BMI088_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI088_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI088_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi088 accel driver not running"); + } + + (*g_dev_acc_ptr)->test_error(); + } + + if (sensor == BMI088_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi088 gyro driver not running"); + } + + (*g_dev_gyr_ptr)->test_error(); + } + + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'"); + warnx("options:"); + warnx(" -X (external bus)"); + warnx(" -R rotation"); + warnx(" -A (Enable Accelerometer)"); + warnx(" -G (Enable Gyroscope)"); +} + +}//namespace ends + + +BMI088::BMI088(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, + uint32_t frequency, enum Rotation rotation): + SPI(name, devname, bus, device, mode, frequency), + _whoami(0), + _register_wait(0), + _reset_wait(0), + _rotation(rotation), + _checked_next(0) +{ +} + +uint8_t +BMI088::read_reg(unsigned reg) +{ + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +uint16_t +BMI088::read_reg16(unsigned reg) +{ + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + +void +BMI088::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +int +bmi088_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + enum sensor_type sensor = BMI088_NONE; + int myoptind = 1; + const char *myoptarg = NULL; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "XR:AG", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + case 'A': + sensor = BMI088_ACCEL; + break; + + case 'G': + sensor = BMI088_GYRO; + break; + + default: + bmi088::usage(); + exit(0); + } + } + + const char *verb = argv[myoptind]; + + if (sensor == BMI088_NONE) { + bmi088::usage(); + exit(0); + } + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + bmi088::start(external_bus, rotation, sensor); + } + + /* + * Stop the driver. + */ + if (!strcmp(verb, "stop")) { + bmi088::stop(external_bus, sensor); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + bmi088::info(external_bus, sensor); + } + + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) { + bmi088::regdump(external_bus, sensor); + } + + if (!strcmp(verb, "testerror")) { + bmi088::testerror(external_bus, sensor); + } + + bmi088::usage(); + exit(1); +}