fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-14 22:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

[NEW] - bmi088 i2c drivers and crazyflie 2.1 conf

Browse Source
This commit is contained in:
TheLegendaryJedi
2020-12-11 18:48:05 +00:00
committed by Lorenz Meier
parent d7812f83f1
commit 40f971c082
41 changed files with 6686 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp
+94
View File
@@ -0,0 +1,94 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI088.hpp"
#include "BMI088_Accelerometer.hpp"
#include "BMI088_Gyroscope.hpp"
I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
BMI088 *instance = nullptr;
if (cli.type == DRV_ACC_DEVTYPE_BMI088) {
instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(iterator.configuredBusOption(), iterator.bus(),
cli.i2c_address, cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
} else if (cli.type == DRV_GYR_DEVTYPE_BMI088) {
instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(iterator.configuredBusOption(), iterator.bus(),
cli.i2c_address, cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
}
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
if (OK != instance->init()) {
delete instance;
return nullptr;
}
return instance;
}
BMI088::BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device,
enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) :
I2C(devtype, name, bus, device, frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype),
_drdy_gpio(drdy_gpio)
{
}
int BMI088::init()
{
int ret = I2C::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("I2C::init failed (%i)", ret);
return ret;
}
int res = Reset() ? 0 : -1;
_state = STATE::SELFTEST;
return res;
}
bool BMI088::Reset()
{
_state = STATE::RESET;
ScheduleClear();
ScheduleNow();
return true;
}
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp
+92
View File
@@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.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 BMI088 : public device::I2C, public I2CSPIDriver<BMI088>
{
public:
BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode,
uint32_t frequency, spi_drdy_gpio_t drdy_gpio);
virtual ~BMI088() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual void 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};
int _overflow_data_size_count{0};
int _overflow_fifo_max_samples_count{0};
int _fifo_read_error_count{0};
int _empty_count{0};
int _total_failure_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
SELFTEST,
RESET,
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
STATE _state{STATE::SELFTEST};
uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
};
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
+962
View File
@@ -0,0 +1,962 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI088_Accelerometer.hpp"
#include <ecl/geo/geo.h> // CONSTANTS_ONE_G
using namespace time_literals;
namespace Bosch::BMI088::Accelerometer
{
BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
_px4_accel(get_device_id(), rotation)
{
if (drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_accel: DRDY missed");
}
ConfigureSampleRate(1600);
}
BMI088_Accelerometer::~BMI088_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 BMI088_Accelerometer::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void BMI088_Accelerometer::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.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);
}
uint8_t BMI088_Accelerometer::RegisterRead(Register reg)
{
uint8_t add = static_cast<uint8_t>(reg);
uint8_t cmd[2] = {add, 0};
transfer(&cmd[0], 1, &cmd[1], 1);
return cmd[1];
}
uint8_t BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value)
{
uint8_t add = static_cast<uint8_t>(reg);
uint8_t cmd[2] = { add, value};
return transfer(cmd, sizeof(cmd), nullptr, 0);
}
int BMI088_Accelerometer::probe()
{
const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
if (ACC_CHIP_ID != ID) {
DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
return PX4_ERROR;
}
PX4_WARN("Probe success, ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID);
return PX4_OK;
}
void BMI088_Accelerometer::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::SELFTEST:
//PX4_WARN("Selftest state");
//SelfTest();
_state = STATE::RESET;
ScheduleDelayed(10_ms);
break;
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) == 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: {
//FIFOReadTest(now);
NormalRead(now);
}
break;
}
}
void BMI088_Accelerometer::ConfigureAccel()
{
//PX4_WARN("ConfigureAccel");
const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0);
switch (ACC_RANGE) {
case acc_range_3g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(3.f);
break;
case acc_range_6g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(6.f);
break;
case acc_range_12g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(12.f);
break;
case acc_range_24g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(24.f);
break;
}
}
void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 800; // default to 800 Hz
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
PX4_WARN("_fifo_empty_interval_us %d", _fifo_empty_interval_us);
_fifo_samples = math::min((float)_fifo_empty_interval_us / (1e6f / RATE), (float)FIFO_MAX_SAMPLES);
PX4_WARN("_fifo_samples %d", _fifo_samples);
// recompute FIFO empty interval (us) with actual sample limit
_fifo_empty_interval_us = _fifo_samples * (1e6f / RATE);
PX4_WARN("_fifo_empty_interval_us %d", _fifo_empty_interval_us);
//PX4_WARN("_fifo_samples %d", _fifo_samples);
ConfigureFIFOWatermark(_fifo_samples);
}
void BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO_WTM: 13 bit FIFO watermark level value
// unit of the fifo watermark is one byte
const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_WTM_0) {
// fifo_water_mark[7:0]
r.set_bits = fifo_watermark_threshold & 0x00FF;
r.clear_bits = ~r.set_bits;
} else if (r.reg == Register::FIFO_WTM_1) {
// fifo_water_mark[12:8]
r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8;
r.clear_bits = ~r.set_bits;
}
}
}
bool BMI088_Accelerometer::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_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 &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
ConfigureAccel();
return success;
}
int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<BMI088_Accelerometer *>(arg)->DataReady();
return 0;
}
void BMI088_Accelerometer::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
}
bool BMI088_Accelerometer::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool BMI088_Accelerometer::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool BMI088_Accelerometer::RegisterCheck(const register_config_t &reg_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;
}
void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
}
uint16_t BMI088_Accelerometer::FIFOReadCount()
{
// FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte
uint8_t fifo_len_buf[2] {};
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], 1, &fifo_len_buf[0], 2) != PX4_OK) {
perf_count(_bad_transfer_perf);
return 0;
}
const uint8_t FIFO_LENGTH_0 = fifo_len_buf[0]; // fifo_byte_counter[7:0]
const uint8_t FIFO_LENGTH_1 = fifo_len_buf[1] & 0x3F; // fifo_byte_counter[13:8]
return combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
}
bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_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, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_count(_bad_transfer_perf);
return false;
}
//PX4_WARN("Accel transfer success");
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] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel.samples++;
fifo_buffer_index += 7; // move forward to next record
}
break;
case FIFO::header::skip_frame:
// Skip Frame
// Frame length: 2 bytes (1 byte header + 1 byte payload)
PX4_DEBUG("Skip Frame");
fifo_buffer_index += 2;
break;
case FIFO::header::sensor_time_frame:
// Sensortime Frame
// Frame length: 4 bytes (1 byte header + 3 bytes payload)
PX4_DEBUG("Sensortime Frame");
fifo_buffer_index += 4;
break;
case FIFO::header::FIFO_input_config_frame:
// FIFO input config Frame
// Frame length: 2 bytes (1 byte header + 1 byte payload)
PX4_DEBUG("FIFO input config Frame");
fifo_buffer_index += 2;
break;
case FIFO::header::sample_drop_frame:
// Sample drop Frame
// Frame length: 2 bytes (1 byte header + 1 byte payload)
PX4_DEBUG("Sample drop Frame");
fifo_buffer_index += 2;
break;
default:
fifo_buffer_index++;
break;
}
}
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
return true;
}
return false;
}
bool BMI088_Accelerometer::SimpleFIFORead(const hrt_abstime &timestamp_sample)
{
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT;
int fifo_fill_level = 0;
uint8_t data_o[2] = { 0, 0 };
uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_LENGTH_0)};
data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
transfer(&data_i[0], 1, &data_o[0], 2);
fifo_fill_level = data_o[0] + 256 * data_o[1];
//PX4_WARN("fifo_fill_level %d", fifo_fill_level);
//uint8_t custom_size = fifo_fill_level * 7;
uint8_t custom_size = fifo_fill_level;
uint8_t buffer_data[custom_size] = {0};
uint8_t buffer[1] = {0};
buffer[0] = static_cast<uint8_t>(Register::FIFO_DATA);
if(fifo_fill_level > 0){
transfer(&buffer[0], 1, &buffer_data[0], custom_size);
struct FIFO::bmi08x_sensor_data bmi08x_accel;
/* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */
for(int i = 1; i < custom_size;)
{
/* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */
if(buffer_data[i] == (0x84 & 0x8c))
{
UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]);
//PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i/6, (double)bmi08x_accel.x, (double)bmi08x_accel.y, (double)bmi08x_accel.z);
accel.x[accel.samples] = bmi08x_accel.x;
accel.y[accel.samples] = bmi08x_accel.y;
accel.z[accel.samples] = bmi08x_accel.z;
accel.samples++;
i += 7;
}
else{
i++;
}
}
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
return true;
}
}
return false;
}
void BMI088_Accelerometer::FIFOReset()
{
perf_count(_fifo_reset_perf);
// ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E).
RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
}
void BMI088_Accelerometer::UpdateTemperature()
{
// stored in an 11-bit value in 2’s complement format
uint8_t temperature_buf[4] {};
temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_MSB) | ACC_I2C_ADDR_PRIMARY;
// temperature_buf[1] dummy byte
if (transfer(&temperature_buf[0], 1, &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);
}
}
bool BMI088_Accelerometer::SelfTest() {
PX4_WARN("Running self-test with datasheet recomended steps(page 17)");
// Reset
PX4_WARN("Reseting the sensor");
if(RegisterWrite(Register::ACC_SOFTRESET, 0xB6) == PX4_OK){
PX4_WARN("Reset success");
}
usleep(100000);
PX4_WARN("Accel on");
if(RegisterWrite(Register::ACC_PWR_CTRL, 0x04) == PX4_OK){
PX4_WARN("Accel on success");
}
usleep(100000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
Configure();
usleep(1000000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
PX4_WARN("ACC_CHIP_ID: 0x%02x", ACC_CHIP_ID);
usleep(30000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
if(RegisterWrite(Register::ACC_PWR_CONF, 0) == PX4_OK){
PX4_WARN("Start sensor success");
PX4_WARN("ACC_PWR_CONF(0): 0x%02x", RegisterRead(Register::ACC_PWR_CONF));
}
usleep(2000000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
if(RegisterWrite(Register::ACC_RANGE, 0x03) == PX4_OK){
PX4_WARN("Range set success");
PX4_WARN("ACC_RANGE(0x03): 0x%02x", RegisterRead(Register::ACC_RANGE));
}
usleep(100000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
if(RegisterWrite(Register::ACC_CONF, 0xA7) == PX4_OK){
PX4_WARN("Conf set success");
PX4_WARN("ACC_CONF(0xA7): 0x%02x", RegisterRead(Register::ACC_CONF));
}
usleep(100000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
// Positive sel-test polarity
if(RegisterWrite(Register::ACC_SELF_TEST, 0x0D) == PX4_OK){
PX4_WARN("Self-test positive mode set success");
PX4_WARN("ACC_SELF_TEST(0x0D): 0x%02x", RegisterRead(Register::ACC_SELF_TEST));
}
usleep(100000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
float *accel_mss = ReadAccelDataFIFO();
PX4_WARN("Positive value");
PX4_WARN("X %f", (double)accel_mss[0]);
PX4_WARN("Y %f", (double)accel_mss[1]);
PX4_WARN("Z %f", (double)accel_mss[2]);
// Negative sel-test polarity
if(RegisterWrite(Register::ACC_SELF_TEST, 0x09) == PX4_OK){
PX4_WARN("Self-test negative mode set success");
PX4_WARN("ACC_SELF_TEST(0x09): 0x%02x", RegisterRead(Register::ACC_SELF_TEST));
}
usleep(600000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
float *accel_mss2 = ReadAccelDataFIFO();
PX4_WARN("Negative value");
PX4_WARN("X %f", (double)accel_mss2[0]);
PX4_WARN("Y %f", (double)accel_mss2[1]);
PX4_WARN("Z %f", (double)accel_mss2[2]);
// Calculate difference between positive and negative sef-test response
float diff_x = accel_mss[0] - accel_mss2[0];
float diff_y = accel_mss[1] - accel_mss2[1];
float diff_z = accel_mss[2] - accel_mss2[2];
PX4_WARN("Diff value");
PX4_WARN("diff_x %f", (double)diff_x);
PX4_WARN("diff_y %f", (double)diff_y);
PX4_WARN("diff_z %f", (double)diff_z);
if(diff_x >= 1000){
PX4_WARN("X Axis self-test success");
}
if(diff_y >= 1000){
PX4_WARN("Y Axis self-test success");
}
if(diff_z >= 500){
PX4_WARN("Z Axis self-test success");
}
// Disable self-test
RegisterWrite(Register::ACC_SELF_TEST, 0x00);
usleep(60000);
PX4_WARN("Sensor ErrReg: 0x%02x", CheckSensorErrReg());
// Reset
//PX4_WARN("Reseting the sensor again");
//RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
//usleep(100000);
return true;
}
float * BMI088_Accelerometer::ReadAccelData()
{
uint8_t cmd[1] = {0x12};
uint8_t buf[6] = {0, 0, 0, 0, 0, 0};
uint8_t* buffer = buf;
int16_t accel[3];
if(transfer(&cmd[0], 1, buffer, sizeof(buf)) == PX4_OK) {
PX4_WARN("ReadAccelData transfer success");
}
for(uint8_t i = 0; i < sizeof(buf); i++){
PX4_WARN("buf[%d]: %f", i, (double)buf[i]);
}
accel[0] = (buf[1] << 8) | buf[0];
accel[1] = (buf[3] << 8) | buf[2];
accel[2] = (buf[5] << 8) | buf[4];
float *accel_mss = new float[3];
accel_mss[0] = (float) accel[0] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f;
accel_mss[1] = (float) accel[1] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f;
accel_mss[2] = (float) accel[2] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f;
return accel_mss;
}
float * BMI088_Accelerometer::ReadAccelDataFIFO()
{
float *accel_mg = new float[3];
struct FIFO::bmi08x_sensor_data bmi08x_accel;
uint8_t buffer[50] = {0};
PX4_WARN("FIFO mode is stop-at-full");
/* Desired FIFO mode is stop-at-full: set bit #0 to 1 in 0x48. Bit #1 must always be one! */
buffer[0] = 0x00 | 0x02;
RegisterWrite(Register::FIFO_CONFIG_0, buffer[0]);
PX4_WARN("FIFO_CONFIG_0(0x%02x): 0x%02x",buffer[0], RegisterRead(Register::FIFO_CONFIG_0));
PX4_WARN("Downsampling factor 2**4 = 16");
/* Downsampling factor 2**4 = 16: write 4 into bit #4-6 of reg. 0x45. Bit #7 must always be one! */
buffer[0] = 0x10 | 0x80;
RegisterWrite(Register::FIFO_DOWN_SAMPLING, buffer[0]);
PX4_WARN("FIFO_DOWN_SAMPLING(0x%02x): 0x%02x",buffer[0], RegisterRead(Register::FIFO_DOWN_SAMPLING));
/* Set water mark to 42 bytes (aka 6 frames, each 7 bytes: 1 byte header + 6 bytes accel data) */
// uint16_t wml = 42;
// buffer[0] = (uint8_t) wml & 0xff;
// buffer[1] = (uint8_t) (wml >> 8) & 0xff;
// uint8_t add = static_cast<uint8_t>(Register::FIFO_WTM_0);
// uint8_t cmd[3] = { add, buffer[0], buffer[1]};
// transfer(cmd, sizeof(cmd), nullptr, 0);
// PX4_WARN("FIFO_WTM_0(0x%02x): 0x%02x",cmd[0], RegisterRead(Register::FIFO_WTM_0));
/* Enable the actual FIFO functionality: write 0x50 to 0x49. Bit #4 must always be one! */
buffer[0] = 0x10 | 0x40;
RegisterWrite(Register::FIFO_CONFIG_1, buffer[0]);
PX4_WARN("FIFO_CONFIG_1(0x%02x): 0x%02x",buffer[0], RegisterRead(Register::FIFO_CONFIG_1));
usleep(1000000);
int fifo_fill_level = 0;
uint8_t data_o[2] = { 0, 0 };
uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_LENGTH_0)};
data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
transfer(&data_i[0], 1, &data_o[0], 2);
fifo_fill_level = data_o[0] + 256 * data_o[1];
PX4_WARN("fifo_fill_level %d", fifo_fill_level);
// while(fifo_fill_level < wml)
// {
// transfer(&data_i[0], 1, &data_o[0], 2);
// fifo_fill_level = data_o[0] + 256 * data_o[1];
// PX4_WARN("fifo_fill_level %d", fifo_fill_level);
// }
uint8_t custom_size = 42;
uint8_t buffer_data[custom_size] = {0};
buffer[0] = static_cast<uint8_t>(Register::FIFO_DATA);
bmi08x_accel.x=10;
PX4_WARN("bmi08x_accel %d", bmi08x_accel.x);
transfer(&buffer[0], 1, &buffer_data[0], custom_size);
/* This is a super-simple FIFO parsing loop, hoping it will only find valid accel data packets */
for(int i = 1; i < custom_size;)
{
/* Header of acceleration sensor data frame: 100001xxb, where x is INT1/INT2 tag, ignored here */
if(buffer_data[i] == (0x84 & 0x8c))
{
UnpackSensorData(&bmi08x_accel, &buffer_data[i + 1]);
PX4_WARN("Frame: %03d ax:%f ay:%f az:%f", i/6, (double)bmi08x_accel.x, (double)bmi08x_accel.y, (double)bmi08x_accel.z);
accel_mg[0] = bmi08x_accel.x;
accel_mg[1] = bmi08x_accel.y;
accel_mg[2] = bmi08x_accel.z;
float* data_in_mg = SensorDataTomg(accel_mg);
PX4_WARN("Frame mg: %03d ax:%f ay:%f az:%f", i/6, (double)data_in_mg[0], (double)data_in_mg[1], (double)data_in_mg[2]);
i += 7;
}
else{
i++;
}
}
return accel_mg;
}
uint8_t BMI088_Accelerometer::CheckSensorErrReg(){
return RegisterRead(Register::ACC_ERR_REG);
}
void BMI088_Accelerometer::UnpackSensorData(struct FIFO::bmi08x_sensor_data *sens_data, uint8_t *buffer)
{
uint16_t data_lsb;
uint16_t data_msb;
uint16_t start_idx = 0;
/* Gyro raw x data */
data_lsb = buffer[start_idx++];
data_msb = buffer[start_idx++];
sens_data->x = (int16_t)((data_msb << 8) | data_lsb);
/* Gyro raw y data */
data_lsb = buffer[start_idx++];
data_msb = buffer[start_idx++];
sens_data->y = (int16_t)((data_msb << 8) | data_lsb);
/* Gyro raw z data */
data_lsb = buffer[start_idx++];
data_msb = buffer[start_idx++];
sens_data->z = (int16_t)((data_msb << 8) | data_lsb);
}
float* BMI088_Accelerometer::SensorDataTomg(float* data)
{
data[0] = (float) data[0] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f;
data[1] = (float) data[1] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f;
data[2] = (float) data[2] / 32768.0f * 1000.0f * powf(2.0f, 24.0f+1.0f) * 1.50f;
return data;
}
bool BMI088_Accelerometer::NormalRead(const hrt_abstime &timestamp_sample) {
const int16_t tX[3] = {1, 0, 0};
const int16_t tY[3] = {0, -1, 0};
const int16_t tZ[3] = {0, 0, -1};
float x = 0;
float y = 0;
float z = 0;
uint8_t buffer[6] = {0};
uint8_t cmd[1] = {static_cast<uint8_t>(Register::ACC_READ)};
transfer(&cmd[0], 1, &buffer[0], 6);
uint8_t RATE_X_LSB = buffer[0];
uint8_t RATE_X_MSB = buffer[1];
uint8_t RATE_Y_LSB = buffer[2];
uint8_t RATE_Y_MSB = buffer[3];
uint8_t RATE_Z_LSB = buffer[4];
uint8_t RATE_Z_MSB = buffer[5];
const int16_t accel_x = combine(RATE_X_MSB, RATE_X_LSB);
const int16_t accel_y = combine(RATE_Y_MSB, RATE_Y_LSB);
const int16_t accel_z = combine(RATE_Z_MSB, 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)
x = accel_x * tX[0] + accel_y * tX[1] + accel_z * tX[2];
y = accel_x * tY[0] + accel_y * tY[1] + accel_z * tY[2];
z = accel_x * tZ[0] + accel_y * tZ[1] + accel_z * tZ[2];
//PX4_WARN("x: %f | y: %f | z: %f", (double)x, (double)y ,(double)z);
_px4_accel.update(timestamp_sample, x, y, z);
return true;
}
bool BMI088_Accelerometer::FIFOReadTest(const hrt_abstime &timestamp_sample){
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT;
int fifo_fill_level = 0;
uint8_t data_o[2] = { 0, 0 };
uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_LENGTH_0)};
data_i[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0);
transfer(&data_i[0], 1, &data_o[0], 2);
fifo_fill_level = data_o[0] + (data_o[1]<<8);
if (fifo_fill_level & 0x8000) {
//PX4_WARN("fifo_fill_level & 0x8000");
// empty
return false;
}
int n_frames_to_read = 5;
// don't read more than 8 frames at a time
if (fifo_fill_level > n_frames_to_read*7) {
fifo_fill_level = n_frames_to_read*7;
}
if (fifo_fill_level == 0) {
//PX4_WARN("fifo_fill_level == 0");
return false;
}
uint8_t data[fifo_fill_level];
data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
if(transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK) {
//PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK");
return false;
}
const uint8_t *p = &data[0];
while (fifo_fill_level >= 7) {
/*
the fifo frames are variable length, with the frame type in the first byte
*/
uint8_t frame_len = 2;
switch (p[0] & 0xFC) {
case 0x84: {
// accel frame
frame_len = 7;
const uint8_t *d = p+1;
int16_t xyz[3] {
int16_t(uint16_t(d[0] | (d[1]<<8))),
int16_t(uint16_t(d[2] | (d[3]<<8))),
int16_t(uint16_t(d[4] | (d[5]<<8)))};
const int16_t tX[3] = {1, 0, 0};
const int16_t tY[3] = {0, -1, 0};
const int16_t tZ[3] = {0, 0, -1};
float x = 0;
float y = 0;
float z = 0;
x = xyz[0] * tX[0] + xyz[1] * tX[1] + xyz[2] * tX[2];
y = xyz[0] * tY[0] + xyz[1] * tY[1] + xyz[2] * tY[2];
z = xyz[0] * tZ[0] + xyz[1] * tZ[1] + xyz[2] * tZ[2];
accel.x[accel.samples] = x;
accel.y[accel.samples] = y;
accel.z[accel.samples] = z;
accel.samples++;
break;
}
case 0x40:
// skip frame
frame_len = 2;
break;
case 0x44:
// sensortime frame
frame_len = 4;
break;
case 0x48:
// fifo config frame
frame_len = 2;
break;
case 0x50:
// sample drop frame
frame_len = 2;
break;
}
p += frame_len;
fifo_fill_level -= frame_len;
}
_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_WARN("accel.samples: %d", accel.samples);
_px4_accel.updateFIFO(accel);
return true;
}
return true;
}
} // namespace Bosch::BMI088::Accelerometer
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp
+147
View File
@@ -0,0 +1,147 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "BMI088.hpp"
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include "Bosch_BMI088_Accelerometer_Registers.hpp"
namespace Bosch::BMI088::Accelerometer
{
class BMI088_Accelerometer : public BMI088
{
public:
BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
~BMI088_Accelerometer() override;
void RunImpl() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
// static constexpr uint32_t RATE{1600}; // 1600 Hz
static constexpr uint32_t RATE{1600}; // 1600 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_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)};
uint8_t dummy{0};
uint8_t FIFO_LENGTH_0{0};
uint8_t FIFO_LENGTH_1{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// Transfer data without length
struct FIFOTransferBufferWithoutLength {
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 &reg_cfg);
uint8_t RegisterRead(Register reg);
uint8_t RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
void FIFOReset();
void UpdateTemperature();
void UnpackSensorData(struct FIFO::bmi08x_sensor_data *sens_data, uint8_t *buffer);
bool SelfTest();
float* ReadAccelData();
float* ReadAccelDataFIFO();
float* SensorDataTomg(float* data);
uint8_t CheckSensorErrReg();
bool SimpleFIFORead(const hrt_abstime &timestamp_sample);
bool NormalRead(const hrt_abstime &timestamp_sample);
bool FIFOReadTest(const hrt_abstime &timestamp_sample);
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_24g, 0 },
{ Register::FIFO_WTM_0, 0, 0 },
{ Register::FIFO_WTM_1, 0, 0 },
{ Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 },
{ Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 },
{ Register::INT1_IO_CONF, INT1_IO_CONF_BIT::int1_out, 0 },
{ Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0},
};
};
} // namespace Bosch::BMI088::Accelerometer
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
+509
View File
@@ -0,0 +1,509 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI088_Gyroscope.hpp"
#include <px4_platform/board_dma_alloc.h>
using namespace time_literals;
namespace Bosch::BMI088::Gyroscope
{
BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
_px4_gyro(get_device_id(), rotation)
{
if (drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME"_gyro: DRDY missed");
}
ConfigureSampleRate(2000);
}
BMI088_Gyroscope::~BMI088_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 BMI088_Gyroscope::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void BMI088_Gyroscope::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.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 BMI088_Gyroscope::probe()
{
const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID);
if (chipid != ID) {
DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid);
return PX4_ERROR;
}
return PX4_OK;
}
void BMI088_Gyroscope::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::SELFTEST:
//SelfTest();
_state = STATE::RESET;
ScheduleDelayed(1_ms);
break;
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: {
//FIFOReadTest(now);
NormalRead(now);
}
break;
}
}
void BMI088_Gyroscope::ConfigureGyro()
{
const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
switch (GYRO_RANGE) {
case gyro_range_2000_dps:
_px4_gyro.set_scale(math::radians(1.f / 16.384f));
_px4_gyro.set_range(math::radians(2000.f));
break;
case gyro_range_1000_dps:
_px4_gyro.set_scale(math::radians(1.f / 32.768f));
_px4_gyro.set_range(math::radians(1000.f));
break;
case gyro_range_500_dps:
_px4_gyro.set_scale(math::radians(1.f / 65.536f));
_px4_gyro.set_range(math::radians(500.f));
break;
case gyro_range_250_dps:
_px4_gyro.set_scale(math::radians(1.f / 131.072f));
_px4_gyro.set_range(math::radians(250.f));
break;
case gyro_range_125_dps:
_px4_gyro.set_scale(math::radians(1.f / 262.144f));
_px4_gyro.set_range(math::radians(125.f));
break;
}
}
void BMI088_Gyroscope::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 2000; // default to 1000 Hz
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_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 BMI088_Gyroscope::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO watermark threshold
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_CONFIG_0) {
r.set_bits = samples;
r.clear_bits = ~r.set_bits;
}
}
}
bool BMI088_Gyroscope::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_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 &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
ConfigureGyro();
return success;
}
int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<BMI088_Gyroscope *>(arg)->DataReady();
return 0;
}
void BMI088_Gyroscope::DataReady()
{
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_samples)) {
ScheduleNow();
}
}
bool BMI088_Gyroscope::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool BMI088_Gyroscope::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool BMI088_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
return success;
}
uint8_t BMI088_Gyroscope::RegisterRead(Register reg)
{
uint8_t add = static_cast<uint8_t>(reg);
uint8_t cmd[2] = {add, 0};
transfer(&cmd[0], 1, &cmd[1], 1);
return cmd[1];
}
void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value)
{
uint8_t add = static_cast<uint8_t>(reg);
uint8_t cmd[2] = {add, value};
transfer(cmd, sizeof(cmd), nullptr, 0);
}
void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
}
bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
//PX4_WARN("Estimated transfer size: %d", transfer_size);
if (transfer((uint8_t *)&buffer, 1, (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 BMI088_Gyroscope::FIFOReset()
{
perf_count(_fifo_reset_perf);
// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
RegisterWrite(Register::FIFO_CONFIG_0, 0);
// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO
for (const auto &r : _register_cfg) {
if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
}
}
}
bool BMI088_Gyroscope::SelfTest() {
//Datasheet page 17 self test
//Set bit0 to enable built in self test
RegisterWrite(Register::SELF_TEST, 0x01);
usleep(10000);
uint8_t res = 0;
uint8_t test_res = false;
while(true){
res = RegisterRead(Register::SELF_TEST);
if((res & 0x02) == 0x02){
if((res & 0x04) == 0x00) {
PX4_WARN("Gyro Self-test success");
test_res = true;
} else {
PX4_WARN("Gyro Self-test error");
}
break;
}
}
RegisterWrite(Register::SELF_TEST, 0x00);
return test_res;
}
bool BMI088_Gyroscope::NormalRead(const hrt_abstime &timestamp_sample) {
float x = 0;
float y = 0;
float z = 0;
uint8_t buffer[6] = {0};
uint8_t cmd[1] = {static_cast<uint8_t>(Register::READ_GYRO)};
transfer(&cmd[0], 1, &buffer[0], 6);
uint8_t RATE_X_LSB = buffer[0];
uint8_t RATE_X_MSB = buffer[1];
uint8_t RATE_Y_LSB = buffer[2];
uint8_t RATE_Y_MSB = buffer[3];
uint8_t RATE_Z_LSB = buffer[4];
uint8_t RATE_Z_MSB = buffer[5];
const int16_t gyro_x = combine(RATE_X_MSB, RATE_X_LSB);
const int16_t gyro_y = combine(RATE_Y_MSB, RATE_Y_LSB);
const int16_t gyro_z = combine(RATE_Z_MSB, 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)
x = gyro_x;
y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
_px4_gyro.update(timestamp_sample, x, y, z);
return true;
}
bool BMI088_Gyroscope::FIFOReadTest(const hrt_abstime &timestamp_sample)
{
uint8_t n_frames;
sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample;
gyro.samples = 0;
gyro.dt = FIFO_SAMPLE_DT;
uint8_t data_i[1] = {static_cast<uint8_t>(Register::FIFO_STATUS)};
transfer(&data_i[0], 1, &n_frames, 1);
n_frames &= 0x7F;
int n_frames_to_read = 5;
// don't read more than 8 frames at a time
if (n_frames > n_frames_to_read) {
n_frames = n_frames_to_read;
}
if (n_frames == 0) {
return false;
}
uint8_t data[6*n_frames];
data[0] = static_cast<uint8_t>(Register::FIFO_DATA);
if(transfer(&data[0], 1, &data[0], 6*n_frames) != PX4_OK) {
//PX4_WARN("transfer(&data[0], 1, &data[0], fifo_fill_level) != PX4_OK");
return false;
}
for (uint8_t i = 0; i < n_frames; i++) {
const uint8_t *d = &data[i*6];
int16_t xyz[3] {
int16_t(uint16_t(d[0] | d[1]<<8)),
int16_t(uint16_t(d[2] | d[3]<<8)),
int16_t(uint16_t(d[4] | d[5]<<8)) };
gyro.x[i] = xyz[0];
gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1];
gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2];
gyro.samples++;
}
_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));
if (gyro.samples > 0) {
//PX4_WARN("accel.samples: %d", accel.samples);
_px4_gyro.updateFIFO(gyro);
return true;
}
return true;
}
} // namespace Bosch::BMI088::Gyroscope
src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp
+128
View File
@@ -0,0 +1,128 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "BMI088.hpp"
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include "Bosch_BMI088_Gyroscope_Registers.hpp"
namespace Bosch::BMI088::Gyroscope
{
class BMI088_Gyroscope : public BMI088
{
public:
BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
~BMI088_Gyroscope() override;
void RunImpl() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr uint32_t RATE{400}; // 2000 Hz
static constexpr float FIFO_SAMPLE_DT{1e6f / RATE};
static constexpr uint32_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 &reg_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 &timestamp_sample, uint8_t samples);
void FIFOReset();
bool SelfTest();
bool NormalRead(const hrt_abstime &timestamp_sample);
bool FIFOReadTest(const hrt_abstime &timestamp_sample);
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_2000_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_STREAM, 0 },
};
};
} // namespace Bosch::BMI088::Gyroscope
src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
+202
View File
@@ -0,0 +1,202 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
namespace Bosch::BMI088::Accelerometer
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
static constexpr uint32_t I2C_400_SPEED = 400 * 1000; // 400kHz I2C interface
static constexpr uint32_t I2C_200_SPEED = 200 * 1000; // 200kHz I2C interface
static constexpr uint32_t I2C_100_SPEED = 100 * 1000; // 100kHz I2C interface
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t ID = 0x1E;
static constexpr uint8_t ACC_I2C_ADDR_PRIMARY = 0x18;
static constexpr uint8_t ACC_I2C_ADDR_SECONDARY = 0x19;
enum class Register : uint8_t {
ACC_CHIP_ID = 0x00,
ACC_ERR_REG = 0x02,
TEMP_MSB = 0x22,
TEMP_LSB = 0x23,
FIFO_LENGTH_0 = 0x24,
FIFO_LENGTH_1 = 0x25,
FIFO_DATA = 0x26,
ACC_CONF = 0x40,
ACC_RANGE = 0x41,
FIFO_DOWN_SAMPLING = 0x45,
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_SELF_TEST = 0x6D,
ACC_I2C_ADDR_PRIMARY_REG = 0x6D,
ACC_I2C_ADDR_SECONDARY_REG = 0x19,
ACC_READ = 0x12
};
// ACC_CONF
enum ACC_CONF_BIT : uint8_t {
// [7:4] acc_bwp
acc_bwp_Normal = Bit7 | Bit5, // Filter setting normal
// [7:4] acc_bwp
acc_bwp_osr_4 = Bit7, // OSR4
// [3:0] acc_odr
acc_odr_1600 = Bit3 | Bit2, // ODR 1600 Hz
// [3:0] acc_odr
acc_odr_12_5 = Bit2 | Bit0, // ODR 12.5 Hz
// [3:0] acc_odr
acc_odr_100 = Bit3, // ODR 100 Hz
};
// ACC_RANGE
enum ACC_RANGE_BIT : uint8_t {
acc_range_3g = 0, // ±3g
acc_range_6g = Bit0, // ±6g
acc_range_12g = Bit1, // ±12g
acc_range_24g = Bit1 | Bit0, // ±24g
};
// FIFO_CONFIG_0
enum FIFO_CONFIG_0_BIT : uint8_t {
BIT1_ALWAYS = Bit1, // This bit must always be ‘1’.
FIFO_mode = Bit0,
};
// FIFO_CONFIG_1
enum FIFO_CONFIG_1_BIT : uint8_t {
Acc_en = Bit6,
BIT4_ALWAYS = Bit4, // This bit must always be ‘1’.
};
// INT1_IO_CONF
enum INT1_IO_CONF_BIT : uint8_t {
int1_in = Bit4,
int1_out = Bit3,
int1_lvl = Bit1,
};
// INT1_INT2_MAP_DATA
enum INT1_INT2_MAP_DATA_BIT : uint8_t {
int2_drdy = Bit6,
int2_fwm = Bit5,
int2_ffull = Bit4,
int1_drdy = Bit2,
int1_fwm = Bit1,
int1_ffull = Bit0,
};
// ACC_PWR_CONF
enum ACC_PWR_CONF_BIT : uint8_t {
acc_pwr_save = 0x03
};
// ACC_PWR_CTRL
enum ACC_PWR_CTRL_BIT : uint8_t {
acc_enable = 0x04
};
namespace FIFO
{
// 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 constexpr size_t SIZE = 1024;
//static constexpr size_t SIZE = sizeof(DATA) * 10;
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,
};
struct bmi08x_sensor_data
{
/*! X-axis sensor data */
int16_t x;
/*! Y-axis sensor data */
int16_t y;
/*! Z-axis sensor data */
int16_t z;
};
} // namespace FIFO
} // namespace Bosch::BMI088::Accelerometer
src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
+154
View File
@@ -0,0 +1,154 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
namespace Bosch::BMI088::Gyroscope
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
static constexpr uint32_t I2C_400_SPEED = 400 * 1000; // 400kHz I2C interface
static constexpr uint32_t I2C_200_SPEED = 200 * 1000; // 200kHz I2C interface
static constexpr uint32_t I2C_100_SPEED = 100 * 1000; // 100kHz I2C interface
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t ID = 0x0F;
static constexpr uint8_t GYRO_I2C_ADDR_PRIMARY = 0x68;
static constexpr uint8_t GYRO_I2C_ADDR_SECONDARY = 0x69;
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,
SELF_TEST = 0x3C,
READ_GYRO = 0x02,
};
// 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_100_Hz = Bit2 | Bit1 | Bit0,
gyro_bw_200_Hz = Bit4,
gyro_bw_2000_Hz = 0x00,
};
// 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,
FIFO_MODE_STREAM = Bit7,
};
namespace FIFO
{
struct DATA {
uint8_t RATE_X_LSB;
uint8_t RATE_X_MSB;
uint8_t RATE_Y_LSB;
uint8_t RATE_Y_MSB;
uint8_t RATE_Z_LSB;
uint8_t RATE_Z_MSB;
};
static_assert(sizeof(DATA) == 6);
// 100 frames of data in FIFO mode
static constexpr size_t SIZE = sizeof(DATA) * 100;
} // namespace FIFO
} // namespace Bosch::BMI088::Gyroscope
src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt
+54
View File
@@ -0,0 +1,54 @@
############################################################################
#
# Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# 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__bmi088_i2c
MAIN bmi088_i2c
COMPILE_FLAGS
SRCS
Bosch_BMI088_Accelerometer_Registers.hpp
Bosch_BMI088_Gyroscope_Registers.hpp
BMI088.cpp
BMI088.hpp
BMI088_Accelerometer.cpp
BMI088_Accelerometer.hpp
BMI088_Gyroscope.cpp
BMI088_Gyroscope.hpp
bmi088_i2c_main.cpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
px4_work_queue
)
src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp
+103
View File
@@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include "BMI088.hpp"
void BMI088::print_usage()
{
PRINT_MODULE_USAGE_NAME("bmi088_i2c", "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(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
extern "C" int bmi088_i2c_main(int argc, char *argv[])
{
int ch;
using ThisDriver = BMI088;
BusCLIArguments cli{true, true};
cli.i2c_address = 0x18;
cli.default_i2c_frequency = 400 * 1000;
cli.default_spi_frequency = 400 * 1000;
while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) {
switch (ch) {
case 'A':
cli.type = DRV_ACC_DEVTYPE_BMI088;
cli.i2c_address = 0x18;
break;
case 'G':
cli.type = DRV_GYR_DEVTYPE_BMI088;
cli.i2c_address = 0x69;
break;
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
}
}
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, 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);
}
PX4_WARN("print_usage1");
ThisDriver::print_usage();
return -1;
}