Daniel Agar
3 years ago
15 changed files with 1983 additions and 6 deletions
@ -0,0 +1,88 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,81 @@
@@ -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 <drivers/drv_hrt.h> |
||||
#include <lib/drivers/device/spi.h> |
||||
#include <lib/perf/perf_counter.h> |
||||
#include <px4_platform_common/i2c_spi_buses.h> |
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } |
||||
|
||||
class BMI085 : public device::SPI, public I2CSPIDriver<BMI085> |
||||
{ |
||||
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<hrt_abstime> _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
|
||||
|
||||
}; |
||||
@ -0,0 +1,604 @@
@@ -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 <geo/geo.h> // 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<int>(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<BMI085_Accelerometer *>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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
|
||||
@ -0,0 +1,132 @@
@@ -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 <lib/drivers/accelerometer/PX4Accelerometer.hpp> |
||||
|
||||
#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<uint8_t>(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<uint8_t>(_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
|
||||
@ -0,0 +1,466 @@
@@ -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 <px4_platform/board_dma_alloc.h> |
||||
|
||||
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<int>(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<BMI085_Gyroscope *>(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<uint8_t>(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
|
||||
@ -0,0 +1,124 @@
@@ -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 <lib/drivers/gyroscope/PX4Gyroscope.hpp> |
||||
|
||||
#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<uint8_t>(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<uint8_t>(_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
|
||||
@ -0,0 +1,171 @@
@@ -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
|
||||
@ -0,0 +1,141 @@
@@ -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
|
||||
@ -0,0 +1,54 @@
@@ -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 |
||||
) |
||||
@ -0,0 +1,5 @@
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_IMU_BOSCH_BMI085 |
||||
bool "bosch bmi085" |
||||
default n |
||||
---help--- |
||||
Enable support for bosch bmi085 |
||||
@ -0,0 +1,101 @@
@@ -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 <px4_platform_common/getopt.h> |
||||
#include <px4_platform_common/module.h> |
||||
|
||||
#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; |
||||
} |
||||
Loading…
Reference in new issue