diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index 7d5f3987c67..68934812344 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -32,7 +32,7 @@ px4_add_board( imu/adis16448 imu/adis16477 imu/adis16497 - imu/bmi088 + imu/bosch/bmi088 imu/invensense/icm20649 imu/invensense/icm20689 irlock diff --git a/boards/hex/cube-orange/default.cmake b/boards/hex/cube-orange/default.cmake index b65ef0d2913..54835b25d64 100644 --- a/boards/hex/cube-orange/default.cmake +++ b/boards/hex/cube-orange/default.cmake @@ -33,7 +33,6 @@ px4_add_board( imu/adis16448 imu/adis16477 imu/adis16497 - imu/bmi088 imu/invensense/icm20602 imu/invensense/icm20649 imu/invensense/icm20948 diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index f337484fe29..f223fe721f6 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -31,7 +31,7 @@ px4_add_board( imu/adis16448 imu/adis16477 imu/adis16497 - imu/bmi088 + imu/bosch/bmi088 imu/invensense/icm20689 irlock lights/blinkm diff --git a/boards/holybro/durandal-v1/init/rc.board_sensors b/boards/holybro/durandal-v1/init/rc.board_sensors index df752f632f0..88f65ac26d3 100644 --- a/boards/holybro/durandal-v1/init/rc.board_sensors +++ b/boards/holybro/durandal-v1/init/rc.board_sensors @@ -7,9 +7,9 @@ adc start # Internal SPI bus ICM-20689 icm20689 -R 2 -s start -# Internal SPI bus BMI088 accel/gyro -bmi088 -A -R 10 -s start -bmi088 -G -R 10 -s start +# Internal SPI BMI088 +bmi088 -A -R 2 -s start +bmi088 -G -R 2 -s start # internal compass ist8310 -I start diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 44bf1edf1bc..fcc3b6c0194 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -32,7 +32,7 @@ px4_add_board( #imu/adis16448 #imu/adis16477 #imu/adis16497 - #imu/bmi088 + #imu/bosch/bmi088 imu/invensense/icm20689 #irlock #lights/blinkm diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index aa2d42dee34..0d053d385dc 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -24,7 +24,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers dshot gps - imu/bmi088 + imu/bosch/bmi088 imu/invensense/icm20602 imu/invensense/icm42688p irlock diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 3091895108f..0c61baa8648 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -15,9 +15,9 @@ icm20602 -R 12 -s start # Internal SPI bus ICM-42688 icm42688p -R 12 -s start -# Internal SPI bus BMI088 accel/gyro -bmi088 -A -R 4 -s start -bmi088 -G -R 4 -s start +# Internal SPI4 BMI088 +bmi088 -A -R 12 -s start +bmi088 -G -R 12 -s start # Internal I2C Baro bmp388 -I start diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 830a7d2adfe..06abd5e87cf 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/bmi088 + imu/bosch/bmi088 imu/invensense/icm20602 imu/invensense/icm20948 irlock diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_sensors b/boards/mro/ctrl-zero-f7/init/rc.board_sensors index 079bd61945c..19ca45eb416 100644 --- a/boards/mro/ctrl-zero-f7/init/rc.board_sensors +++ b/boards/mro/ctrl-zero-f7/init/rc.board_sensors @@ -9,8 +9,8 @@ adc start icm20602 -s -R 8 start # Internal SPI bus BMI088 accel & gyro -bmi088 -A -R 10 -s start -bmi088 -G -R 10 -s start +bmi088 -A -R 8 -s start +bmi088 -G -R 8 -s start # Internal ICM-20948 (with magnetometer) icm20948 -s -R 8 -M start diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index b9375ad8a5a..e1f9d7bd5db 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -31,7 +31,7 @@ px4_add_board( imu/adis16448 imu/adis16477 imu/adis16497 - imu/bmi088 + imu/bosch/bmi088 imu/invensense/icm20602 imu/st/ism330dlc irlock diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index d4c6cd5314d..6fb1790f5ce 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -31,7 +31,7 @@ px4_add_board( imu/adis16448 imu/adis16477 imu/adis16497 - imu/bmi088 + imu/bosch/bmi088 imu/invensense/icm20602 imu/st/ism330dlc irlock diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index de751d32233..3cf56cb7147 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -15,9 +15,9 @@ icm20602 -R 2 -s start # Internal SPI bus ISM300DLC ism330dlc -s start -# Internal SPI bus BMI088 accel/gyro -bmi088 -A -R 12 -s start -bmi088 -G -R 12 -s start +# Internal SPI BMI088 +bmi088 -A -R 4 -s start +bmi088 -G -R 4 -s start # Possible internal compass bmm150 -I start diff --git a/boards/px4/fmu-v5x/src/spi.cpp b/boards/px4/fmu-v5x/src/spi.cpp index dd88c873223..989e479c150 100644 --- a/boards/px4/fmu-v5x/src/spi.cpp +++ b/boards/px4/fmu-v5x/src/spi.cpp @@ -44,7 +44,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { }, {GPIO::PortD, GPIO::Pin15}), initSPIBus(SPI::Bus::SPI3, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}), }, {GPIO::PortE, GPIO::Pin7}), // initSPIBus(SPI::Bus::SPI4, { // // no devices diff --git a/platforms/common/include/px4_platform_common/log.h b/platforms/common/include/px4_platform_common/log.h index 73080ff94d9..db759370bf9 100644 --- a/platforms/common/include/px4_platform_common/log.h +++ b/platforms/common/include/px4_platform_common/log.h @@ -122,6 +122,7 @@ __END_DECLS #include #include +#include __BEGIN_DECLS diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp deleted file mode 100644 index 6ae7c85e209..00000000000 --- a/src/drivers/imu/bmi088/BMI088_accel.cpp +++ /dev/null @@ -1,584 +0,0 @@ -/**************************************************************************** - * - * 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" - -using namespace time_literals; - -/* - * 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(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, - enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode) : - BMI088("bmi088_accel", path_accel, bus_option, bus, DRV_ACC_DEVTYPE_BMI088, device, spi_mode, bus_frequency, rotation), - _px4_accel(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), - _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")), - _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) -{ -} - -BMI088_accel::~BMI088_accel() -{ - /* delete the perf counter */ - perf_free(_sample_perf); - 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. - */ - - px4_usleep(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) */ - - px4_usleep(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() -{ - // Reset the accelerometer - reset(); - - /* start polling at the specified rate */ - ScheduleOnInterval((1_s / BMI088_ACCEL_DEFAULT_RATE) / 2, 1000); - -} - -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::RunImpl() -{ - 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; - } - - // don't publish duplicated reads - if ((report.accel_x == _accel_prev[0]) && (report.accel_y == _accel_prev[1]) && (report.accel_z == _accel_prev[2])) { - perf_count(_duplicates); - perf_end(_sample_perf); - return; - - } else { - _accel_prev[0] = report.accel_x; - _accel_prev[1] = report.accel_y; - _accel_prev[2] = report.accel_z; - } - - // 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_status() -{ - I2CSPIDriverBase::print_status(); - PX4_INFO("Accel"); - - perf_print_counter(_sample_perf); - perf_print_counter(_bad_transfers); - perf_print_counter(_bad_registers); - perf_print_counter(_duplicates); - _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 deleted file mode 100644 index 7fd03a2c61e..00000000000 --- a/src/drivers/imu/bmi088/BMI088_accel.hpp +++ /dev/null @@ -1,256 +0,0 @@ -/**************************************************************************** - * - * 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: - BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode); - virtual ~BMI088_accel(); - - int init() override; - - // 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 - uint8_t read_reg(unsigned reg) override; - - // 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 - uint16_t read_reg16(unsigned reg) override; - - void print_status() override; - - void print_registers(); - - // deliberately cause a sensor error - void test_error(); - - void RunImpl() override; -protected: - - int probe() override; - -private: - - PX4Accelerometer _px4_accel; - - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _bad_registers; - perf_counter_t _duplicates; - - int16_t _accel_prev[3] {}; - - // 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; - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - /** - * 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 deleted file mode 100644 index 102c8d85b40..00000000000 --- a/src/drivers/imu/bmi088/BMI088_gyro.cpp +++ /dev/null @@ -1,469 +0,0 @@ -/**************************************************************************** - * - * 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" - -using namespace time_literals; - -/* - * 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(I2CSPIBusOption bus_option, int bus, const char *path_gyro, uint32_t device, - enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) : - BMI088("bmi088_gyro", path_gyro, bus_option, bus, DRV_GYR_DEVTYPE_BMI088, device, spi_mode, bus_frequency, - rotation), - _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), - _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")), - _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")), - _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")), - _duplicates(perf_alloc(PC_COUNT, "bmi088_gyro_duplicates")) -{ -} - -BMI088_gyro::~BMI088_gyro() -{ - /* delete the perf counter */ - perf_free(_sample_perf); - perf_free(_bad_transfers); - perf_free(_bad_registers); - perf_free(_duplicates); -} - -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); - - px4_usleep(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() -{ - /* start polling at the specified rate */ - ScheduleOnInterval((1_s / BMI088_GYRO_DEFAULT_RATE) / 2, 1000); -} - -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::RunImpl() -{ - 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_CHIP_ID | 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 ((bmi_gyroreport.chip_id) != BMI088_GYR_WHO_AM_I) { - // If chip id is not right, assume a bus transfer 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; - } - - // don't publish duplicated reads - if ((report.gyro_x == _gyro_prev[0]) && (report.gyro_y == _gyro_prev[1]) && (report.gyro_z == _gyro_prev[2])) { - perf_count(_duplicates); - perf_end(_sample_perf); - return; - - } else { - _gyro_prev[0] = report.gyro_x; - _gyro_prev[1] = report.gyro_y; - _gyro_prev[2] = report.gyro_z; - } - - // 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_status() -{ - I2CSPIDriverBase::print_status(); - PX4_INFO("Gyro"); - - perf_print_counter(_sample_perf); - perf_print_counter(_bad_transfers); - perf_print_counter(_bad_registers); - perf_print_counter(_duplicates); - - _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 deleted file mode 100644 index 85ed826ab70..00000000000 --- a/src/drivers/imu/bmi088/BMI088_gyro.hpp +++ /dev/null @@ -1,252 +0,0 @@ -/**************************************************************************** - * - * 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: - BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation, - int bus_frequency, spi_mode_e spi_mode); - virtual ~BMI088_gyro(); - - int init() override; - - // Start automatic measurement. - void start() override; - - void print_status() override; - - void print_registers() override; - - // deliberately cause a sensor error - void test_error() override; - - void RunImpl() override; -protected: - - int probe() override; - -private: - - PX4Gyroscope _px4_gyro; - - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _bad_registers; - perf_counter_t _duplicates; - - int16_t _gyro_prev[3] {}; - - // 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; - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - /** - * 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); - - /** - * 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; - uint8_t chip_id; // Read to check if bus is workig - uint8_t ununsed; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - }; -#pragma pack(pop) - -}; diff --git a/src/drivers/imu/bosch/bmi088/BMI088.cpp b/src/drivers/imu/bosch/bmi088/BMI088.cpp new file mode 100644 index 00000000000..e5cb4d69f93 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/BMI088.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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.hpp" + +#include "BMI088_Accelerometer.hpp" +#include "BMI088_Gyroscope.hpp" + +I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + BMI088 *instance = nullptr; + + if (cli.type == DRV_ACC_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(iterator.configuredBusOption(), iterator.bus(), + iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + + } else if (cli.type == DRV_GYR_DEVTYPE_BMI088) { + instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(iterator.configuredBusOption(), iterator.bus(), + iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + } + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +BMI088::BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, + enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) : + SPI(devtype, name, bus, device, mode, frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype), + _drdy_gpio(drdy_gpio) +{ +} + +int BMI088::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMI088::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} diff --git a/src/drivers/imu/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp similarity index 60% rename from src/drivers/imu/bmi088/BMI088.hpp rename to src/drivers/imu/bosch/bmi088/BMI088.hpp index df02abb6baf..e403983f25b 100644 --- a/src/drivers/imu/bmi088/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2020 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 @@ -33,66 +33,54 @@ #pragma once -#include -#include -#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 +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } class BMI088 : public device::SPI, public I2CSPIDriver { public: - BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, uint8_t type, uint32_t device, - enum spi_mode_e mode, uint32_t frequency, enum Rotation rotation); + BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode, + uint32_t frequency, spi_drdy_gpio_t drdy_gpio); + virtual ~BMI088() = default; static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - virtual void start() = 0; - virtual void RunImpl() = 0; + int init() override; + virtual void print_status() = 0; + protected: - virtual void print_registers() = 0; - virtual void test_error() = 0; - void custom_method(const BusCLIArguments &cli) override; + bool Reset(); - uint8_t _whoami; ///< whoami result + const spi_drdy_gpio_t _drdy_gpio; - uint8_t _register_wait; - uint64_t _reset_wait; + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + unsigned _consecutive_failures{0}; + unsigned _total_failures{0}; - enum Rotation _rotation; + px4::atomic _drdy_fifo_read_samples{0}; + bool _data_ready_interrupt_enabled{false}; - uint8_t _checked_next; + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + }; - /** - * Read a register from the BMI088 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg) override; - virtual uint16_t read_reg16(unsigned reg); + STATE _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval - /** - * Write a register in the BMI088 - * - * @param reg The register to write. - * @param value The new value to write. - * @return OK on success, negative errno otherwise. - */ - int write_reg(unsigned reg, uint8_t value) override; }; diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp new file mode 100644 index 00000000000..88b71bc4227 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -0,0 +1,601 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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_Accelerometer.hpp" + +#include // CONSTANTS_ONE_G + +using namespace time_literals; + +namespace Bosch::BMI088::Accelerometer +{ + +BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, + int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : + BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), + _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation) +{ + if (drdy_gpio != 0) { + _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_accel: DRDY interval"); + } + + ConfigureSampleRate(); +} + +BMI088_Accelerometer::~BMI088_Accelerometer() +{ + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_interval_perf); +} + +void BMI088_Accelerometer::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI088_Accelerometer::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_interval_perf); + + _px4_accel.print_status(); +} + +int BMI088_Accelerometer::probe() +{ + /* 6.1 Serial Peripheral Interface (SPI) + * ... the accelerometer part starts always in I2C mode + * (regardless of the level of the PS pin) and needs to be changed to SPI + * mode actively by sending a rising edge on the CSB1 pin + * (chip select of the accelerometer), on which the accelerometer part + * switches to SPI mode and stays in this mode until the next power-up-reset. + * + * To change the sensor to SPI mode in the initialization phase, the user + * could perfom a dummy SPI read operation, e.g. of register ACC_CHIP_ID + * (the obtained value will be invalid).In case of read operations, + */ + RegisterRead(Register::ACC_CHIP_ID); + + const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID); + + if (ACC_CHIP_ID != ID) { + DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID); + return PX4_ERROR; + } + + return PX4_OK; +} + +void BMI088_Accelerometer::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor + RegisterWrite(Register::ACC_SOFTRESET, 0xB6); + _reset_timestamp = now; + _consecutive_failures = 0; + _total_failures = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value. + break; + + case STATE::WAIT_FOR_RESET: + if (RegisterRead(Register::ACC_CHIP_ID) == ID) { + // ACC_PWR_CONF: Power on sensor + RegisterWrite(Register::ACC_PWR_CONF, 0); + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(10_ms); + } + + break; + + case STATE::FIFO_READ: { + uint8_t samples = 0; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_fifo_read_samples was set + if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_accel_samples) { + samples = _fifo_accel_samples; + perf_count_interval(_drdy_interval_perf, now); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + bool success = false; + + if (!_data_ready_interrupt_enabled || (samples == 0)) { + // manually check FIFO count if no samples from DRDY + const uint16_t fifo_byte_counter = FIFOReadCount(); + samples = fifo_byte_counter / sizeof(FIFO::DATA); + } + + if (samples > FIFO_MAX_SAMPLES) { + // not necessarily an actual FIFO overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (samples == 0) { + perf_count(_fifo_empty_perf); + + } else if (samples >= 1) { + if (FIFORead(now, samples)) { + success = true; + _consecutive_failures = 0; + } + } + + if (!success) { + _consecutive_failures++; + _total_failures++; + + // full reset if things are failing consistently + if (_consecutive_failures > 100 || _total_failures > 1000) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void BMI088_Accelerometer::ConfigureAccel() +{ + const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0); + + switch (ACC_RANGE) { + case acc_range_3g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(3.f); + break; + + case acc_range_6g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(6.f); + break; + + case acc_range_12g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(12.f); + break; + + case acc_range_24g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f); + _px4_accel.set_range(24.f); + break; + } +} + +void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 800; // default to 800 Hz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_accel_samples = math::min((float)_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual accel sample limit + _fifo_empty_interval_us = _fifo_accel_samples * (1e6f / ACCEL_RATE); + + ConfigureFIFOWatermark(_fifo_accel_samples); +} + +void BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO_WTM: 13 bit FIFO watermark level value + // unit of the fifo watermark is one byte + const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); + + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_WTM_0) { + // fifo_water_mark[7:0] + r.set_bits = fifo_watermark_threshold & 0x00FF; + r.clear_bits = ~r.set_bits; + + } else if (r.reg == Register::FIFO_WTM_1) { + // fifo_water_mark[12:8] + r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8; + r.clear_bits = ~r.set_bits; + } + } +} + +bool BMI088_Accelerometer::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + ConfigureAccel(); + + return success; +} + +int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void BMI088_Accelerometer::DataReady() +{ + uint8_t expected = 0; + + if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_accel_samples)) { + ScheduleNow(); + } +} + +bool BMI088_Accelerometer::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool BMI088_Accelerometer::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI088_Accelerometer::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t BMI088_Accelerometer::RegisterRead(Register reg) +{ + // 6.1.2 SPI interface of accelerometer part + // + // In case of read operations of the accelerometer part, the requested data + // is not sent immediately, but instead first a dummy byte is sent, and + // after this dummy byte the actual requested register content is transmitted. + uint8_t cmd[3] {}; + cmd[0] = static_cast(reg) | DIR_READ; + // cmd[1] dummy byte + transfer(cmd, cmd, sizeof(cmd)); + return cmd[2]; +} + +void BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] {(uint8_t)reg, value}; + transfer(cmd, cmd, sizeof(cmd)); +} + +void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +uint16_t BMI088_Accelerometer::FIFOReadCount() +{ + // FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte + uint8_t fifo_len_buf[4] {}; + fifo_len_buf[0] = static_cast(Register::FIFO_LENGTH_0) | DIR_READ; + // fifo_len_buf[1] dummy byte + + if (transfer(&fifo_len_buf[0], &fifo_len_buf[0], sizeof(fifo_len_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2]; // fifo_byte_counter[7:0] + const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8] + + const uint16_t fifo_byte_counter = combine(FIFO_LENGTH_1, FIFO_LENGTH_0); + + // An empty FIFO corresponds to 0x8000 + if (fifo_byte_counter == 0x8000) { + return 0; + } + + return fifo_byte_counter / sizeof(FIFO::DATA); +} + +bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE); + + perf_begin(_transfer_perf); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); + return false; + } + + perf_end(_transfer_perf); + + PX4Accelerometer::FIFOSample accel; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT; + + const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0); + + // An empty FIFO corresponds to 0x8000 + if (fifo_byte_counter == 0x8000) { + perf_count(_fifo_empty_perf); + return false; + + } else if (fifo_byte_counter >= FIFO::SIZE) { + perf_count(_fifo_overflow_perf); + return false; + } + + // first find all sensor data frames in the buffer + uint8_t *data_buffer = (uint8_t *)&buffer.f[0]; + unsigned fifo_buffer_index = 0; // start of buffer + + while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) { + // look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2 + switch (data_buffer[fifo_buffer_index] & 0xFC) { + case FIFO::header::sensor_data_frame: { + // Acceleration sensor data frame + // Frame length: 7 bytes (1 byte header + 6 bytes payload) + + FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index]; + const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB); + const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB); + const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[accel.samples] = accel_x; + accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.samples++; + + fifo_buffer_index += 7; // move forward to next record + } + break; + + case FIFO::header::skip_frame: + // Skip Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("Skip Frame"); + fifo_buffer_index += 2; + break; + + case FIFO::header::sensor_time_frame: + // Sensortime Frame + // Frame length: 4 bytes (1 byte header + 3 bytes payload) + PX4_DEBUG("Sensortime Frame"); + fifo_buffer_index += 4; + break; + + case FIFO::header::FIFO_input_config_frame: + // FIFO input config Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("FIFO input config Frame"); + fifo_buffer_index += 2; + break; + + case FIFO::header::sample_drop_frame: + // Sample drop Frame + // Frame length: 2 bytes (1 byte header + 1 byte payload) + PX4_DEBUG("Sample drop Frame"); + fifo_buffer_index += 2; + break; + + default: + fifo_buffer_index++; + break; + } + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + _px4_accel.updateFIFO(accel); + return true; + } + + return false; +} + +void BMI088_Accelerometer::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E). + RegisterWrite(Register::ACC_SOFTRESET, 0xB0); + + // reset while FIFO is disabled + _drdy_fifo_read_samples.store(0); +} + +void BMI088_Accelerometer::UpdateTemperature() +{ + // stored in an 11-bit value in 2’s complement format + uint8_t temperature_buf[4] {}; + temperature_buf[0] = static_cast(Register::TEMP_MSB) | DIR_READ; + // temperature_buf[1] dummy byte + + if (transfer(&temperature_buf[0], &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) { + return; + } + + const uint8_t TEMP_MSB = temperature_buf[2]; + const uint8_t TEMP_LSB = temperature_buf[3]; + + // Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data + uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32); + int16_t Temp_int11 = 0; + + if (Temp_uint11 > 1023) { + Temp_int11 = Temp_uint11 - 2048; + + } else { + Temp_int11 = Temp_uint11; + } + + float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C + + if (PX4_ISFINITE(temperature)) { + _px4_accel.set_temperature(temperature); + } +} + +} // namespace Bosch::BMI088::Accelerometer diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp new file mode 100644 index 00000000000..bc9485dd894 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp @@ -0,0 +1,134 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "Bosch_BMI088_Accelerometer_Registers.hpp" + +namespace Bosch::BMI088::Accelerometer +{ + +class BMI088_Accelerometer : public BMI088 +{ +public: + BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ~BMI088_Accelerometer() override; + + void RunImpl() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr uint32_t ACCEL_RATE{1600}; // 1600 Hz accel + static constexpr float FIFO_SAMPLE_DT{1e6f / ACCEL_RATE}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Accelerometer::FIFOSample::x) / sizeof(PX4Accelerometer::FIFOSample::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::FIFO_LENGTH_0) | DIR_READ}; + uint8_t dummy{0}; + uint8_t FIFO_LENGTH_0{0}; + uint8_t FIFO_LENGTH_1{0}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Configure(); + void ConfigureAccel(); + void ConfigureSampleRate(int sample_rate = 0); + void ConfigureFIFOWatermark(uint8_t samples); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void UpdateTemperature(); + + PX4Accelerometer _px4_accel; + + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_accel: transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")}; + perf_counter_t _drdy_interval_perf{nullptr}; + + uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{10}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::ACC_PWR_CONF, 0, ACC_PWR_CONF_BIT::acc_pwr_save }, + { Register::ACC_PWR_CTRL, ACC_PWR_CTRL_BIT::acc_enable, 0 }, + { Register::ACC_CONF, ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 }, + { Register::ACC_RANGE, ACC_RANGE_BIT::acc_range_24g, 0 }, + { Register::FIFO_WTM_0, 0, 0 }, + { Register::FIFO_WTM_1, 0, 0 }, + { Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 }, + { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 }, + { Register::INT1_IO_CONF, INT1_IO_CONF_BIT::int1_out, 0 }, + { Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0}, + }; +}; + +} // namespace Bosch::BMI088::Accelerometer diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp new file mode 100644 index 00000000000..8240a808fa2 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -0,0 +1,462 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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_Gyroscope.hpp" + +#include + +using namespace time_literals; + +namespace Bosch::BMI088::Gyroscope +{ + +BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, + int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : + BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio), + _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) +{ + if (drdy_gpio != 0) { + _drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_gyro: DRDY interval"); + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +BMI088_Gyroscope::~BMI088_Gyroscope() +{ + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_interval_perf); +} + +void BMI088_Gyroscope::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI088_Gyroscope::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_interval_perf); + + _px4_gyro.print_status(); +} + +int BMI088_Gyroscope::probe() +{ + const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID); + + if (chipid != ID) { + DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid); + return PX4_ERROR; + } + + return PX4_OK; +} + +void BMI088_Gyroscope::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor. + // Following a delay of 30 ms, all configuration settings are overwritten with their reset value. + RegisterWrite(Register::GYRO_SOFTRESET, 0xB6); + _reset_timestamp = now; + _consecutive_failures = 0; + _total_failures = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(30_ms); + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) { + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(1_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(10_ms); + } + + break; + + case STATE::FIFO_READ: { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_fifo_read_samples was set + if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) { + perf_count_interval(_drdy_interval_perf, now); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + // always check current FIFO status/count + bool success = false; + const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS); + + if (FIFO_STATUS & FIFO_STATUS_BIT::Fifo_overrun) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else { + const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::Fifo_frame_counter; + + if (fifo_frame_counter > FIFO_MAX_SAMPLES) { + // not necessarily an actual FIFO overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (fifo_frame_counter == 0) { + perf_count(_fifo_empty_perf); + + } else if (fifo_frame_counter >= 1) { + if (FIFORead(now, fifo_frame_counter)) { + success = true; + _consecutive_failures = 0; + } + } + } + + if (!success) { + _consecutive_failures++; + _total_failures++; + + // full reset if things are failing consistently + if (_consecutive_failures > 100 || _total_failures > 1000) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +void BMI088_Gyroscope::ConfigureGyro() +{ + const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0); + + switch (GYRO_RANGE) { + case gyro_range_2000_dps: + _px4_gyro.set_scale(math::radians(1.f / 16.384f)); + _px4_gyro.set_range(math::radians(2000.f)); + break; + + case gyro_range_1000_dps: + _px4_gyro.set_scale(math::radians(1.f / 32.768f)); + _px4_gyro.set_range(math::radians(1000.f)); + break; + + case gyro_range_500_dps: + _px4_gyro.set_scale(math::radians(1.f / 65.536f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case gyro_range_250_dps: + _px4_gyro.set_scale(math::radians(1.f / 131.072f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case gyro_range_125_dps: + _px4_gyro.set_scale(math::radians(1.f / 262.144f)); + _px4_gyro.set_range(math::radians(125.f)); + break; + } +} + +void BMI088_Gyroscope::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 1000; // default to 1000 Hz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + +void BMI088_Gyroscope::ConfigureFIFOWatermark(uint8_t samples) +{ + // FIFO watermark threshold + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_CONFIG_0) { + r.set_bits = samples; + r.clear_bits = ~r.set_bits; + } + } +} + +bool BMI088_Gyroscope::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + ConfigureGyro(); + + return success; +} + +int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void BMI088_Gyroscope::DataReady() +{ + uint8_t expected = 0; + + if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + ScheduleNow(); + } +} + +bool BMI088_Gyroscope::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool BMI088_Gyroscope::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI088_Gyroscope::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t BMI088_Gyroscope::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] {(uint8_t)reg, value}; + transfer(cmd, cmd, sizeof(cmd)); +} + +void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + + perf_begin(_transfer_perf); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); + return false; + } + + perf_end(_transfer_perf); + + PX4Gyroscope::FIFOSample gyro; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = buffer.f[i]; + + const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB); + const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB); + const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro_x; + gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + } + + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + _px4_gyro.updateFIFO(gyro); + + return true; +} + +void BMI088_Gyroscope::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer. + RegisterWrite(Register::FIFO_CONFIG_0, 0); + + // FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1 + RegisterWrite(Register::FIFO_CONFIG_1, 0); + + // reset while FIFO is disabled + _drdy_fifo_read_samples.store(0); + + // FIFO_CONFIG_0: restore FIFO watermark + // FIFO_CONFIG_1: re-enable FIFO + for (const auto &r : _register_cfg) { + if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) { + RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); + } + } +} + +} // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp new file mode 100644 index 00000000000..b3455f868ad --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "Bosch_BMI088_Gyroscope_Registers.hpp" + +namespace Bosch::BMI088::Gyroscope +{ + +class BMI088_Gyroscope : public BMI088 +{ +public: + BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); + ~BMI088_Gyroscope() override; + + void RunImpl() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr uint32_t GYRO_RATE{2000}; // 2000 Hz gyro + static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_RATE}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::FIFO_DATA) | DIR_READ}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Configure(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate = 0); + void ConfigureFIFOWatermark(uint8_t samples); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + PX4Gyroscope _px4_gyro; + + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_gyro: transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")}; + perf_counter_t _drdy_interval_perf{nullptr}; + + uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{8}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::GYRO_RANGE, GYRO_RANGE_BIT::gyro_range_2000_dps, 0 }, + { Register::GYRO_BANDWIDTH, 0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz }, + { Register::GYRO_INT_CTRL, GYRO_INT_CTRL_BIT::fifo_en, 0 }, + { Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl }, + { Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 }, + { Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 }, + { Register::FIFO_CONFIG_0, 0, 0 }, // fifo_water_mark_level_trigger_retain<6:0> + { Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::FIFO_MODE, 0 }, + }; +}; + +} // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp new file mode 100644 index 00000000000..091c9b78944 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 + +namespace Bosch::BMI088::Accelerometer +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t ID = 0x1E; + +enum class Register : uint8_t { + ACC_CHIP_ID = 0x00, + + TEMP_MSB = 0x22, + TEMP_LSB = 0x23, + + FIFO_LENGTH_0 = 0x24, + FIFO_LENGTH_1 = 0x25, + + FIFO_DATA = 0x26, + + ACC_CONF = 0x40, + ACC_RANGE = 0x41, + + FIFO_WTM_0 = 0x46, + FIFO_WTM_1 = 0x47, + FIFO_CONFIG_0 = 0x48, + FIFO_CONFIG_1 = 0x49, + + INT1_IO_CONF = 0x53, + + INT1_INT2_MAP_DATA = 0x58, + + ACC_PWR_CONF = 0x7C, + ACC_PWR_CTRL = 0x7D, + ACC_SOFTRESET = 0x7E, +}; + +// ACC_CONF +enum ACC_CONF_BIT : uint8_t { + // [7:4] acc_bwp + acc_bwp_Normal = Bit7 | Bit5, // Filter setting normal + + // [3:0] acc_odr + acc_odr_1600 = Bit3 | Bit2, // ODR 1600 Hz +}; + +// ACC_RANGE +enum ACC_RANGE_BIT : uint8_t { + acc_range_3g = 0, // ±3g + acc_range_6g = Bit0, // ±6g + acc_range_12g = Bit1, // ±12g + acc_range_24g = Bit1 | Bit0, // ±24g +}; + +// FIFO_CONFIG_0 +enum FIFO_CONFIG_0_BIT : uint8_t { + BIT1_ALWAYS = Bit1, // This bit must always be ‘1’. + FIFO_mode = Bit0, +}; + +// FIFO_CONFIG_1 +enum FIFO_CONFIG_1_BIT : uint8_t { + Acc_en = Bit6, + BIT4_ALWAYS = Bit4, // This bit must always be ‘1’. +}; + +// INT1_IO_CONF +enum INT1_IO_CONF_BIT : uint8_t { + int1_in = Bit4, + int1_out = Bit3, + + int1_lvl = Bit1, +}; + +// INT1_INT2_MAP_DATA +enum INT1_INT2_MAP_DATA_BIT : uint8_t { + int2_drdy = Bit6, + int2_fwm = Bit5, + int2_ffull = Bit4, + + int1_drdy = Bit2, + int1_fwm = Bit1, + int1_ffull = Bit0, +}; + +// ACC_PWR_CONF +enum ACC_PWR_CONF_BIT : uint8_t { + acc_pwr_save = 0x03 +}; + +// ACC_PWR_CTRL +enum ACC_PWR_CTRL_BIT : uint8_t { + acc_enable = 0x04 +}; + +namespace FIFO +{ +static constexpr size_t SIZE = 1024; + +// 1. Acceleration sensor data frame - Frame length: 7 bytes (1 byte header + 6 bytes payload) +// Payload: the next bytes contain the sensor data in the same order as defined in the register map (addresses 0x12 – 0x17). +// 2. Skip Frame - Frame length: 2 bytes (1 byte header + 1 byte payload) +// Payload: one byte containing the number of skipped frames. When more than 0xFF frames have been skipped, 0xFF is returned. +// 3. Sensortime Frame - Frame length: 4 bytes (1 byte header + 3 bytes payload) +// Payload: Sensortime (content of registers 0x18 – 0x1A), taken when the last byte of the last frame is read. + +struct DATA { + uint8_t Header; + uint8_t ACC_X_LSB; + uint8_t ACC_X_MSB; + uint8_t ACC_Y_LSB; + uint8_t ACC_Y_MSB; + uint8_t ACC_Z_LSB; + uint8_t ACC_Z_MSB; +}; +static_assert(sizeof(DATA) == 7); + +enum header : uint8_t { + sensor_data_frame = 0b10000100, + skip_frame = 0b01000000, + sensor_time_frame = 0b01000100, + FIFO_input_config_frame = 0b01001000, + sample_drop_frame = 0b01010000, +}; + +} // namespace FIFO +} // namespace Bosch::BMI088::Accelerometer diff --git a/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp new file mode 100644 index 00000000000..42d9df8f408 --- /dev/null +++ b/src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + +namespace Bosch::BMI088::Gyroscope +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t ID = 0x0F; + +enum class Register : uint8_t { + GYRO_CHIP_ID = 0x00, + + FIFO_STATUS = 0x0E, + GYRO_RANGE = 0x0F, + GYRO_BANDWIDTH = 0x10, + + GYRO_SOFTRESET = 0x14, + GYRO_INT_CTRL = 0x15, + INT3_INT4_IO_CONF = 0x16, + INT3_INT4_IO_MAP = 0x18, + + FIFO_WM_ENABLE = 0x1E, + + FIFO_CONFIG_0 = 0x3D, + FIFO_CONFIG_1 = 0x3E, + FIFO_DATA = 0x3F, +}; + +// FIFO_STATUS +enum FIFO_STATUS_BIT : uint8_t { + Fifo_overrun = Bit7, + Fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + +// GYRO_RANGE +enum GYRO_RANGE_BIT : uint8_t { + gyro_range_2000_dps = 0x00, // ±2000 + gyro_range_1000_dps = 0x01, // ±1000 + gyro_range_500_dps = 0x02, // ±500 + gyro_range_250_dps = 0x04, // ±250 + gyro_range_125_dps = 0x05, // ±125 +}; + +// GYRO_BANDWIDTH +enum GYRO_BANDWIDTH_BIT : uint8_t { + gyro_bw_532_Hz = Bit2 | Bit1 | Bit0 +}; + +// GYRO_INT_CTRL +enum GYRO_INT_CTRL_BIT : uint8_t { + data_en = Bit7, + fifo_en = Bit6, +}; + +// INT3_INT4_IO_CONF +enum INT3_INT4_IO_CONF_BIT : uint8_t { + Int3_od = Bit1, // ‘0’ Push-pull + Int3_lvl = Bit0, // ‘0’ Active low +}; + +// INT3_INT4_IO_MAP +enum INT3_INT4_IO_MAP_BIT : uint8_t { + Int4_data = Bit7, + Int4_fifo = Bit5, + Int3_fifo = Bit2, + Int3_data = Bit0, +}; + +// FIFO_WM_ENABLE +enum FIFO_WM_ENABLE_BIT : uint8_t { + fifo_wm_enable = Bit7 | Bit3, + fifo_wm_disable = Bit3, +}; + +// FIFO_CONFIG_1 +enum FIFO_CONFIG_1_BIT : uint8_t { + FIFO_MODE = Bit6, +}; + + +namespace FIFO +{ +struct DATA { + uint8_t RATE_X_LSB; + uint8_t RATE_X_MSB; + uint8_t RATE_Y_LSB; + uint8_t RATE_Y_MSB; + uint8_t RATE_Z_LSB; + uint8_t RATE_Z_MSB; +}; +static_assert(sizeof(DATA) == 6); + +// 100 frames of data in FIFO mode +static constexpr size_t SIZE = sizeof(DATA) * 100; + +} // namespace FIFO +} // namespace Bosch::BMI088::Gyroscope diff --git a/src/drivers/imu/bmi088/CMakeLists.txt b/src/drivers/imu/bosch/bmi088/CMakeLists.txt similarity index 81% rename from src/drivers/imu/bmi088/CMakeLists.txt rename to src/drivers/imu/bosch/bmi088/CMakeLists.txt index 35a92c3cfe4..8326953f07e 100644 --- a/src/drivers/imu/bmi088/CMakeLists.txt +++ b/src/drivers/imu/bosch/bmi088/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2020 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 @@ -30,13 +30,25 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__bmi88 + MODULE drivers__imu__bosch__bmi088 MAIN bmi088 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS - BMI088_accel.cpp - BMI088_gyro.cpp + Bosch_BMI088_Accelerometer_Registers.hpp + Bosch_BMI088_Gyroscope_Registers.hpp + + BMI088.cpp + BMI088.hpp + BMI088_Accelerometer.cpp + BMI088_Accelerometer.hpp + BMI088_Gyroscope.cpp + BMI088_Gyroscope.hpp + bmi088_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue ) diff --git a/src/drivers/imu/bmi088/bmi088_main.cpp b/src/drivers/imu/bosch/bmi088/bmi088_main.cpp similarity index 52% rename from src/drivers/imu/bmi088/bmi088_main.cpp rename to src/drivers/imu/bosch/bmi088/bmi088_main.cpp index d018f3aa62d..e6949e4cf64 100644 --- a/src/drivers/imu/bmi088/bmi088_main.cpp +++ b/src/drivers/imu/bosch/bmi088/bmi088_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2020 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 @@ -31,15 +31,12 @@ * ****************************************************************************/ -#include "BMI088_accel.hpp" -#include "BMI088_gyro.hpp" -#include +#include #include -extern "C" __EXPORT int bmi088_main(int argc, char *argv[]); +#include "BMI088.hpp" -void -BMI088::print_usage() +void BMI088::print_usage() { PRINT_MODULE_USAGE_NAME("bmi088", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); @@ -48,107 +45,16 @@ BMI088::print_usage() PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - - PRINT_MODULE_USAGE_COMMAND("regdump"); - PRINT_MODULE_USAGE_COMMAND("testerror"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +extern "C" int bmi088_main(int argc, char *argv[]) { - BMI088 *instance = nullptr; - - if (cli.type == DRV_ACC_DEVTYPE_BMI088) { - instance = new BMI088_accel(iterator.configuredBusOption(), iterator.bus(), BMI088_DEVICE_PATH_ACCEL, iterator.devid(), - cli.rotation, cli.bus_frequency, cli.spi_mode); - - } else if (cli.type == DRV_GYR_DEVTYPE_BMI088) { - instance = new BMI088_gyro(iterator.configuredBusOption(), iterator.bus(), BMI088_DEVICE_PATH_GYRO, iterator.devid(), - cli.rotation, cli.bus_frequency, cli.spi_mode); - } - - if (instance == nullptr) { - return nullptr; - } - - if (OK != instance->init()) { - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - delete instance; - return nullptr; - } - - instance->start(); - - return instance; -} - -void -BMI088::custom_method(const BusCLIArguments &cli) -{ - switch (cli.custom1) { - case 0: print_registers(); - break; - - case 1: test_error(); - break; - } -} - -BMI088::BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, uint8_t type, - uint32_t device, - enum spi_mode_e mode, - uint32_t frequency, enum Rotation rotation): - SPI(type, MODULE_NAME, bus, device, mode, frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, type), - _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]; -} - -int -BMI088::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - return transfer(cmd, nullptr, sizeof(cmd)); -} - -int -bmi088_main(int argc, char *argv[]) -{ - using ThisDriver = BMI088; int ch; + using ThisDriver = BMI088; BusCLIArguments cli{false, true}; cli.type = 0; - cli.default_spi_frequency = BMI088_BUS_SPEED; + cli.default_spi_frequency = 10000000; while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) { switch (ch) { @@ -187,16 +93,6 @@ bmi088_main(int argc, char *argv[]) return ThisDriver::module_status(iterator); } - if (!strcmp(verb, "regdump")) { - cli.custom1 = 0; - return ThisDriver::module_custom_method(cli, iterator); - } - - if (!strcmp(verb, "testerror")) { - cli.custom1 = 1; - return ThisDriver::module_custom_method(cli, iterator); - } - ThisDriver::print_usage(); return -1; } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 1b13d713cc1..481d8253afd 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -97,7 +97,7 @@ private: float _range{16 * CONSTANTS_ONE_G}; float _scale{1.f}; - float _temperature{0.f}; + float _temperature{NAN}; float _clip_limit{_range / _scale}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 2b94919c644..41693de8ed7 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -95,7 +95,7 @@ private: float _range{math::radians(2000.f)}; float _scale{1.f}; - float _temperature{0.f}; + float _temperature{NAN}; uint32_t _error_count{0};