forked from Archive/PX4-Autopilot
drivers/imu/bosch: new BMI085 driver
- very similar to the BMI088 with minor differences in accel range
This commit is contained in:
Daniel Agar
parent
077547f31e
commit
c71cc5b815
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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}),
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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 @@
|
|||
menuconfig DRIVERS_IMU_BOSCH_BMI085
|
||||
bool "bosch bmi085"
|
||||
default n
|
||||
---help---
|
||||
Enable support for bosch bmi085
|
||||
|
|
@ -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