diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index 4163c6c808..e6f4feb45b 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -13,6 +13,7 @@ CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI085=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y @@ -38,6 +39,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -46,6 +48,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -61,7 +64,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y @@ -70,10 +72,9 @@ CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y diff --git a/boards/mro/pixracerpro/init/rc.board_sensors b/boards/mro/pixracerpro/init/rc.board_sensors index 111e3eb044..a73708ede7 100644 --- a/boards/mro/pixracerpro/init/rc.board_sensors +++ b/boards/mro/pixracerpro/init/rc.board_sensors @@ -8,8 +8,14 @@ board_adc start icm20602 -s -b 1 -R 8 start # Internal SPI bus BMI088 accel & gyro -bmi088 -A -s -b 5 -R 8 start -bmi088 -G -s -b 5 -R 8 start +if bmi088 -A -s -b 5 -R 8 start +then + bmi088 -G -s -b 5 -R 8 start +else + # otherwise try BMI085 + bmi085 -A -s -b 5 -R 8 start + bmi085 -G -s -b 5 -R 8 start +fi # Internal ICM-20948 (with magnetometer) icm20948 -s -b 1 -R 8 -M start diff --git a/boards/mro/pixracerpro/src/spi.cpp b/boards/mro/pixracerpro/src/spi.cpp index af19682fe9..51ba3a5637 100644 --- a/boards/mro/pixracerpro/src/spi.cpp +++ b/boards/mro/pixracerpro/src/spi.cpp @@ -47,6 +47,8 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI5, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI085, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI085, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), }), initSPIBusExternal(SPI::Bus::SPI6, { initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}), diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 47e9c352b4..0edd9a169b 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -137,7 +137,8 @@ #define DRV_PWM_DEVTYPE_PCA9685 0x69 #define DRV_ACC_DEVTYPE_BMI088 0x6a #define DRV_OSD_DEVTYPE_ATXXXX 0x6b - +#define DRV_ACC_DEVTYPE_BMI085 0x6C +#define DRV_GYR_DEVTYPE_BMI085 0x6D #define DRV_DIST_DEVTYPE_LL40LS 0x70 #define DRV_DIST_DEVTYPE_MAPPYDOT 0x71 diff --git a/src/drivers/imu/bosch/bmi085/BMI085.cpp b/src/drivers/imu/bosch/bmi085/BMI085.cpp new file mode 100644 index 0000000000..4f168017af --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/BMI085.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "BMI085.hpp" + +#include "BMI085_Accelerometer.hpp" +#include "BMI085_Gyroscope.hpp" + +I2CSPIDriverBase *BMI085::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + BMI085 *instance = nullptr; + + if (config.devid_driver_index == DRV_ACC_DEVTYPE_BMI085) { + instance = new Bosch::BMI085::Accelerometer::BMI085_Accelerometer(config); + + } else if (config.devid_driver_index == DRV_GYR_DEVTYPE_BMI085) { + instance = new Bosch::BMI085::Gyroscope::BMI085_Gyroscope(config); + } + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +BMI085::BMI085(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio) +{ +} + +int BMI085::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMI085::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} diff --git a/src/drivers/imu/bosch/bmi085/BMI085.hpp b/src/drivers/imu/bosch/bmi085/BMI085.hpp new file mode 100644 index 0000000000..81ba0c0b17 --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/BMI085.hpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } + +class BMI085 : public device::SPI, public I2CSPIDriver +{ +public: + BMI085(const I2CSPIDriverConfig &config); + + virtual ~BMI085() = default; + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + virtual void RunImpl() = 0; + + int init() override; + virtual void print_status() = 0; + +protected: + + bool Reset(); + + const spi_drdy_gpio_t _drdy_gpio; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval + +}; diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp new file mode 100644 index 0000000000..8016632f2f --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.cpp @@ -0,0 +1,604 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "BMI085_Accelerometer.hpp" + +#include // CONSTANTS_ONE_G + +using namespace time_literals; + +namespace Bosch::BMI085::Accelerometer +{ + +BMI085_Accelerometer::BMI085_Accelerometer(const I2CSPIDriverConfig &config) : + BMI085(config), + _px4_accel(get_device_id(), config.rotation) +{ + if (config.drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed"); + } + + ConfigureSampleRate(_px4_accel.get_max_rate_hz()); +} + +BMI085_Accelerometer::~BMI085_Accelerometer() +{ + 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_missed_perf); +} + +void BMI085_Accelerometer::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI085_Accelerometer::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + 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_missed_perf); +} + +int BMI085_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 != acc_chip_id) { + DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID); + return PX4_ERROR; + } + + return PX4_OK; +} + +void BMI085_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; + _failure_count = 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) == acc_chip_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) > 1000_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(100_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(100_ms); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + uint8_t samples = 0; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_samples; + + } else { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + if (samples == 0) { + // check current FIFO count + const uint16_t fifo_byte_counter = FIFOReadCount(); + + if (fifo_byte_counter >= FIFO::SIZE) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if ((fifo_byte_counter == 0) || (fifo_byte_counter == 0x8000)) { + // An empty FIFO corresponds to 0x8000 + perf_count(_fifo_empty_perf); + + } else { + samples = fifo_byte_counter / sizeof(FIFO::DATA); + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + samples = 0; + } + } + } + + bool success = false; + + if (samples >= 1) { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_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 BMI085_Accelerometer::ConfigureAccel() +{ + const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0); + + switch (ACC_RANGE) { + case acc_range_2g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f); + _px4_accel.set_range(2.f * CONSTANTS_ONE_G); + break; + + case acc_range_4g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f); + _px4_accel.set_range(4.f * CONSTANTS_ONE_G); + break; + + case acc_range_8g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f); + _px4_accel.set_range(8.f * CONSTANTS_ONE_G); + break; + + case acc_range_16g: + _px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1)) / 32768.f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + break; + } +} + +void BMI085_Accelerometer::ConfigureSampleRate(int sample_rate) +{ + // 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_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE); + + ConfigureFIFOWatermark(_fifo_samples); +} + +void BMI085_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 BMI085_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 BMI085_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void BMI085_Accelerometer::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool BMI085_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 BMI085_Accelerometer::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI085_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 BMI085_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 BMI085_Accelerometer::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void BMI085_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 BMI085_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] + + return combine(FIFO_LENGTH_1, FIFO_LENGTH_0); +} + +bool BMI085_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); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + 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; + } + + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT; + + // 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] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(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 BMI085_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_timestamp_sample.store(0); +} + +void BMI085_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) { + perf_count(_bad_transfer_perf); + 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); + + } else { + perf_count(_bad_transfer_perf); + } +} + +} // namespace Bosch::BMI085::Accelerometer diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp new file mode 100644 index 0000000000..b20eb2573b --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/BMI085_Accelerometer.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "BMI085.hpp" + +#include + +#include "Bosch_BMI085_Accelerometer_Registers.hpp" + +namespace Bosch::BMI085::Accelerometer +{ + +class BMI085_Accelerometer : public BMI085 +{ +public: + BMI085_Accelerometer(const I2CSPIDriverConfig &config); + ~BMI085_Accelerometer() override; + + void RunImpl() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr uint32_t RATE{1600}; // 1600 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; + + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::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 _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_missed_perf{nullptr}; + + uint8_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / 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_16g, 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::BMI085::Accelerometer diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp new file mode 100644 index 0000000000..6c9288430b --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.cpp @@ -0,0 +1,466 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "BMI085_Gyroscope.hpp" + +#include + +using namespace time_literals; + +namespace Bosch::BMI085::Gyroscope +{ + +BMI085_Gyroscope::BMI085_Gyroscope(const I2CSPIDriverConfig &config) : + BMI085(config), + _px4_gyro(get_device_id(), config.rotation) +{ + if (config.drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed"); + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +BMI085_Gyroscope::~BMI085_Gyroscope() +{ + 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_missed_perf); +} + +void BMI085_Gyroscope::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void BMI085_Gyroscope::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + 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_missed_perf); +} + +int BMI085_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 BMI085_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; + _failure_count = 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) > 1000_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(100_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(100_ms); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + + // 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 technically an 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) { + + uint8_t samples = fifo_frame_counter; + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_samples + 1) { + // sample timestamp set from data ready already corresponds to _fifo_samples + if (timestamp_sample == 0) { + timestamp_sample = now - static_cast(FIFO_SAMPLE_DT); + } + + samples--; + } + + if (FIFORead((timestamp_sample == 0) ? now : timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_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 BMI085_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 BMI085_Gyroscope::ConfigureSampleRate(int sample_rate) +{ + // 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_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / RATE); + + ConfigureFIFOWatermark(_fifo_samples); +} + +void BMI085_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 BMI085_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 BMI085_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void BMI085_Gyroscope::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool BMI085_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 BMI085_Gyroscope::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool BMI085_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 BMI085_Gyroscope::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void BMI085_Gyroscope::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void BMI085_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 BMI085_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); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + sensor_gyro_fifo_s 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 BMI085_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_timestamp_sample.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::BMI085::Gyroscope diff --git a/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp new file mode 100644 index 0000000000..75d4c03d83 --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/BMI085_Gyroscope.hpp @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "BMI085.hpp" + +#include + +#include "Bosch_BMI085_Gyroscope_Registers.hpp" + +namespace Bosch::BMI085::Gyroscope +{ + +class BMI085_Gyroscope : public BMI085 +{ +public: + BMI085_Gyroscope(const I2CSPIDriverConfig &config); + ~BMI085_Gyroscope() override; + + void RunImpl() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr uint32_t RATE{2000}; // 2000 Hz + static constexpr float FIFO_SAMPLE_DT{1e6f / RATE}; + + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::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 _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_missed_perf{nullptr}; + + uint8_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / 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::BMI085::Gyroscope diff --git a/src/drivers/imu/bosch/bmi085/Bosch_BMI085_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi085/Bosch_BMI085_Accelerometer_Registers.hpp new file mode 100644 index 0000000000..62818f4cf0 --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/Bosch_BMI085_Accelerometer_Registers.hpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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::BMI085::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 acc_chip_id = 0x1F; + +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, // 0x0a (0b1010) Normal + + // [3:0] acc_odr + acc_odr_1600 = Bit3 | Bit2, // 0x0C ODR 1600 Hz +}; + +// ACC_RANGE +enum ACC_RANGE_BIT : uint8_t { + acc_range_2g = 0, // ±2g + acc_range_4g = Bit0, // ±4g + acc_range_8g = Bit1, // ±8g + acc_range_16g = Bit1 | Bit0, // ±16g +}; + +// FIFO_CONFIG_0 +enum FIFO_CONFIG_0_BIT : uint8_t { + BIT1_ALWAYS = Bit1, // This bit must always be ‘1’. + FIFO_mode = Bit0, // FIFO mode +}; + +// 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::BMI085::Accelerometer diff --git a/src/drivers/imu/bosch/bmi085/Bosch_BMI085_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi085/Bosch_BMI085_Gyroscope_Registers.hpp new file mode 100644 index 0000000000..8715347efd --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/Bosch_BMI085_Gyroscope_Registers.hpp @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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::BMI085::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::BMI085::Gyroscope diff --git a/src/drivers/imu/bosch/bmi085/CMakeLists.txt b/src/drivers/imu/bosch/bmi085/CMakeLists.txt new file mode 100644 index 0000000000..f11c0dab13 --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE drivers__imu__bosch__bmi085 + MAIN bmi085 + COMPILE_FLAGS + SRCS + Bosch_BMI085_Accelerometer_Registers.hpp + Bosch_BMI085_Gyroscope_Registers.hpp + + BMI085.cpp + BMI085.hpp + BMI085_Accelerometer.cpp + BMI085_Accelerometer.hpp + BMI085_Gyroscope.cpp + BMI085_Gyroscope.hpp + + bmi085_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/bosch/bmi085/Kconfig b/src/drivers/imu/bosch/bmi085/Kconfig new file mode 100644 index 0000000000..0f4baf2818 --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_BOSCH_BMI085 + bool "bosch bmi085" + default n + ---help--- + Enable support for bosch bmi085 diff --git a/src/drivers/imu/bosch/bmi085/bmi085_main.cpp b/src/drivers/imu/bosch/bmi085/bmi085_main.cpp new file mode 100644 index 0000000000..5963b16fea --- /dev/null +++ b/src/drivers/imu/bosch/bmi085/bmi085_main.cpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 +#include + +#include "BMI085.hpp" + +void BMI085::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmi085", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_FLAG('A', "Accel", true); + 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_DEFAULT_COMMANDS(); +} + +extern "C" int bmi085_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMI085; + BusCLIArguments cli{false, true}; + uint16_t type = 0; + cli.default_spi_frequency = 10000000; + const char *name = MODULE_NAME; + + while ((ch = cli.getOpt(argc, argv, "AGR:")) != EOF) { + switch (ch) { + case 'A': + type = DRV_ACC_DEVTYPE_BMI085; + name = MODULE_NAME "_accel"; + break; + + case 'G': + type = DRV_GYR_DEVTYPE_BMI085; + name = MODULE_NAME "_gyro"; + break; + + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb || type == 0) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(name, cli, type); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +}