yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

new Bosch BMI055 IMU driver using FIFOs and DRDY

Browse Source 
- accel & gyro FIFOs enabled
 - FIFO watermark on data ready interrupt
 - sensor side filtering completely disabled
 - gyro now respects `IMU_GYRO_RATEMAX` (up to 2 kHz)
 - saves a few % cpu (at default rate)
This commit is contained in:
Daniel Agar
2020-05-29 16:58:02 -04:00
parent daf75e40eb
commit e819f99064
28 changed files with 1656 additions and 1569 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/fmurt1062-v1/default.cmake
View File

@@ -29,7 +29,7 @@ px4_add_board(
#imu/adis16477
#imu/adis16497
#imu # all available imu drivers
imu/bmi055
imu/bosch/bmi055
imu/mpu6000
irlock
lights/blinkm

4
boards/nxp/fmurt1062-v1/init/rc.board_sensors
View File

@@ -24,8 +24,8 @@ icm20602 -R 2 -s start
icm20689 -R 2 -s start
# Internal SPI bus BMI055 accel/gyro
bmi055 -A -R 10 -s start
bmi055 -G -R 10 -s start
bmi055 -A -R 2 -s start
bmi055 -G -R 2 -s start
# internal compass
ist8310 -I start

2
boards/px4/fmu-v5/critmonitor.cmake
View File

@@ -30,7 +30,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/default.cmake
View File

@@ -30,7 +30,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/fixedwing.cmake
View File

@@ -27,7 +27,7 @@ px4_add_board(
imu/adis16477
imu/adis16497
#imu # all available imu drivers
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

4
boards/px4/fmu-v5/init/rc.board_sensors
View File

@@ -12,8 +12,8 @@ icm20602 -s -R 2 start
icm20689 -s -R 2 start
# Internal SPI bus BMI055 accel/gyro
bmi055 -A -R 10 -s start
bmi055 -G -R 10 -s start
bmi055 -A -R 2 -s start
bmi055 -G -R 2 -s start
# internal compass
ist8310 -I start

2
boards/px4/fmu-v5/irqmonitor.cmake
View File

@@ -30,7 +30,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/multicopter.cmake
View File

@@ -28,7 +28,7 @@ px4_add_board(
imu/adis16477
imu/adis16497
#imu # all available imu drivers
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/optimized.cmake
View File

@@ -30,7 +30,7 @@ px4_add_board(
dshot
gps
#imu # all available imu drivers
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/rover.cmake
View File

@@ -25,7 +25,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/rtps.cmake
View File

@@ -30,7 +30,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi055
imu/bosch/bmi055
imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
boards/px4/fmu-v5/stackcheck.cmake
View File

@@ -31,7 +31,7 @@ px4_add_board(
#imu/adis16448
#imu/adis16477
#imu/adis16497
#imu/bmi055
#imu/bosch/bmi055
#imu/invensense/icm20602
imu/invensense/icm20689
#imu/mpu6000 # legacy icm20602/icm20689 driver

2
platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/micro_hal.h
View File

@@ -103,7 +103,7 @@ __BEGIN_DECLS
/* imxrt_gpiosetevent is not implemented and will need to be added */
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) imxrt_gpiosetevent(pinset,r,f,e,fp,a)
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) (-1)
#define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_SCHMITT_TRIGGER | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ))
#define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST))

449
src/drivers/imu/bmi055/BMI055_accel.cpp
View File

@@ -1,449 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI055_accel.hpp"
using namespace time_literals;
/*
list of registers that will be checked in check_registers(). Note
that ADDR_WHO_AM_I must be first in the list.
*/
const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS] = { BMI055_ACC_CHIP_ID,
BMI055_ACC_BW,
BMI055_ACC_RANGE,
BMI055_ACC_INT_EN_1,
BMI055_ACC_INT_MAP_1,
};
BMI055_accel::BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device,
enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
BMI055(DRV_ACC_DEVTYPE_BMI055, "bmi055_accel", path_accel, bus_option, bus, device, spi_mode, bus_frequency, rotation),
_px4_accel(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")),
_got_duplicate(false)
{
}
BMI055_accel::~BMI055_accel()
{
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
BMI055_accel::init()
{
/* do SPI init (and probe) first */
int ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
return reset();
}
int BMI055_accel::reset()
{
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET); // Soft-reset
up_udelay(5000);
write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_500); //Write accel bandwidth (DLPF)
write_checked_reg(BMI055_ACC_RANGE, BMI055_ACCEL_RANGE_2_G);//Write range
write_checked_reg(BMI055_ACC_INT_EN_1, BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range
//Enable Accelerometer in normal mode
write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL);
up_udelay(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
return OK;
}
int
BMI055_accel::probe()
{
/* look for device ID */
_whoami = read_reg(BMI055_ACC_CHIP_ID);
// verify product revision
switch (_whoami) {
case BMI055_ACC_WHO_AM_I:
memset(_checked_values, 0, sizeof(_checked_values));
memset(_checked_bad, 0, sizeof(_checked_bad));
_checked_values[0] = _whoami;
_checked_bad[0] = _whoami;
return OK;
}
DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
return -EIO;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
BMI055_accel::test_error()
{
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);
::printf("error triggered\n");
print_registers();
}
void
BMI055_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_checked_reg(reg, val);
}
void
BMI055_accel::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
}
}
}
int
BMI055_accel::set_accel_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_ACCEL_RANGE_2_G | BMI055_ACCEL_RANGE_16_G;
float lsb_per_g;
if (max_g == 0) {
max_g = 16;
}
if (max_g <= 2) {
//max_accel_g = 2;
setbits |= BMI055_ACCEL_RANGE_2_G;
lsb_per_g = 1024;
} else if (max_g <= 4) {
//max_accel_g = 4;
setbits |= BMI055_ACCEL_RANGE_4_G;
lsb_per_g = 512;
} else if (max_g <= 8) {
//max_accel_g = 8;
setbits |= BMI055_ACCEL_RANGE_8_G;
lsb_per_g = 256;
} else if (max_g <= 16) {
//max_accel_g = 16;
setbits |= BMI055_ACCEL_RANGE_16_G;
lsb_per_g = 128;
} else {
return -EINVAL;
}
_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
modify_reg(BMI055_ACC_RANGE, clearbits, setbits);
return OK;
}
void
BMI055_accel::start()
{
/* start polling at the specified rate */
ScheduleOnInterval((1_s / BMI055_ACCEL_DEFAULT_RATE) / 2, 1000);
}
void
BMI055_accel::check_registers(void)
{
uint8_t v;
if ((v = read_reg(_checked_registers[_checked_next])) !=
_checked_values[_checked_next]) {
_checked_bad[_checked_next] = v;
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % BMI055_ACCEL_NUM_CHECKED_REGISTERS;
}
void
BMI055_accel::RunImpl()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int8_t temp;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI055 in one pass.
*/
uint8_t index = 0;
uint8_t accel_data[8] {};
accel_data[index] = BMI055_ACC_X_L | DIR_READ;
const hrt_abstime timestamp_sample = hrt_absolute_time();
if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) {
return;
}
check_registers();
/* Extracting accel data from the read data */
index = 1;
uint16_t lsb, msb, msblsb;
lsb = (uint16_t)accel_data[index++];
uint8_t status_x = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_x = ((int16_t)msblsb >> 4); /* Data in X axis */
lsb = (uint16_t)accel_data[index++];
uint8_t status_y = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_y = ((int16_t)msblsb >> 4); /* Data in Y axis */
lsb = (uint16_t)accel_data[index++];
uint8_t status_z = (lsb & BMI055_NEW_DATA_MASK);
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_z = ((int16_t)msblsb >> 4); /* Data in Z axis */
// Byte
report.temp = accel_data[index];
// Checking the status of new data
if ((!status_x) || (!status_y) || (!status_z)) {
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
_got_duplicate = false;
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi055 accel does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again, but don't return any data yet
_register_wait--;
return;
}
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
_px4_accel.set_error_count(error_count);
/*
* Temperature is reported as Eight-bit 2’s complement sensor temperature value
* with 0.5 °C/LSB sensitivity and an offset of 23.0 °C
*/
_px4_accel.set_temperature((report.temp * 0.5f) + 23.0f);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
*/
_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
/* stop measuring */
perf_end(_sample_perf);
}
void
BMI055_accel::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("Type: Accel");
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
if (v != _checked_bad[i]) {
::printf("reg %02x:%02x was bad %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_bad[i]);
}
}
_px4_accel.print_status();
}
void
BMI055_accel::print_registers()
{
uint8_t index = 0;
printf("BMI055 accel registers\n");
uint8_t reg = _checked_registers[index++];
uint8_t v = read_reg(reg);
printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
}

227
src/drivers/imu/bmi055/BMI055_accel.hpp
View File

@@ -1,227 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include "BMI055.hpp"
#define BMI055_DEVICE_PATH_ACCEL "/dev/bmi055_accel"
#define BMI055_DEVICE_PATH_ACCEL_EXT "/dev/bmi055_accel_ext"
// BMI055 Accel registers
#define BMI055_ACC_CHIP_ID 0x00
#define BMI055_ACC_X_L 0x02
#define BMI055_ACC_X_H 0x03
#define BMI055_ACC_Y_L 0x04
#define BMI055_ACC_Y_H 0x05
#define BMI055_ACC_Z_L 0x06
#define BMI055_ACC_Z_H 0x07
#define BMI055_ACC_TEMP 0x08
#define BMI055_ACC_INT_STATUS_0 0x09
#define BMI055_ACC_INT_STATUS_1 0x0A
#define BMI055_ACC_INT_STATUS_2 0x0B
#define BMI055_ACC_INT_STATUS_3 0x0C
#define BMI055_ACC_FIFO_STATUS 0x0E
#define BMI055_ACC_RANGE 0x0F
#define BMI055_ACC_BW 0x10
#define BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACC_PMU_LOW_POWER 0x12
#define BMI055_ACC_DATA_CTRL 0x13
#define BMI055_ACC_SOFTRESET 0x14
#define BMI055_ACC_INT_EN_0 0x16
#define BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_INT_EN_2 0x18
#define BMI055_ACC_INT_MAP_0 0x19
#define BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_INT_MAP_2 0x1B
#define BMI055_ACC_INT_SRC 0x1E
#define BMI055_ACC_INT_OUT_CTRL 0x20
#define BMI055_ACC_INT_LATCH 0x21
#define BMI055_ACC_INT_LH_0 0x22
#define BMI055_ACC_INT_LH_1 0x23
#define BMI055_ACC_INT_LH_2 0x24
#define BMI055_ACC_INT_LH_3 0x25
#define BMI055_ACC_INT_LH_4 0x26
#define BMI055_ACC_INT_MOT_0 0x27
#define BMI055_ACC_INT_MOT_1 0x28
#define BMI055_ACC_INT_MOT_2 0x29
#define BMI055_ACC_INT_TAP_0 0x2A
#define BMI055_ACC_INT_TAP_1 0x2B
#define BMI055_ACC_INT_ORIE_0 0x2C
#define BMI055_ACC_INT_ORIE_1 0x2D
#define BMI055_ACC_INT_FLAT_0 0x2E
#define BMI055_ACC_INT_FLAT_1 0x2F
#define BMI055_ACC_FIFO_CONFIG_0 0x30
#define BMI055_ACC_SELF_TEST 0x32
#define BMI055_ACC_EEPROM_CTRL 0x33
#define BMI055_ACC_SERIAL_CTRL 0x34
#define BMI055_ACC_OFFSET_CTRL 0x36
#define BMI055_ACC_OFC_SETTING 0x37
#define BMI055_ACC_OFFSET_X 0x38
#define BMI055_ACC_OFFSET_Y 0x39
#define BMI055_ACC_OFFSET_Z 0x3A
#define BMI055_ACC_TRIM_GPO 0x3B
#define BMI055_ACC_TRIM_GP1 0x3C
#define BMI055_ACC_FIFO_CONFIG_1 0x3E
#define BMI055_ACC_FIFO_DATA 0x3F
// BMI055 Accelerometer Chip-Id
#define BMI055_ACC_WHO_AM_I 0xFA
// DLPF filter bandwidth settings
#define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_62_5 (1<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_BW_125 (1<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_250 (1<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_500 (1<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_1000 (1<<3) | (1<<2) | (1<<1) | (1<<0)
//BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACCEL_NORMAL (0<<7) | (0<<6) | (0<<5)
#define BMI055_ACCEL_DEEP_SUSPEND (0<<7) | (0<<6) | (1<<5)
#define BMI055_ACCEL_LOW_POWER (0<<7) | (1<<6) | (0<<5)
#define BMI055_ACCEL_SUSPEND (1<<7) | (0<<6) | (0<<5)
//BMI055_ACC_RANGE 0x0F
#define BMI055_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0)
//BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_DRDY_INT_EN (1<<4)
//BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_DRDY_INT1 (1<<0)
// Default and Max values
#define BMI055_ACCEL_DEFAULT_RANGE_G 16
#define BMI055_ACCEL_DEFAULT_RATE 1000
#define BMI055_ACCEL_MAX_RATE 1000
#define BMI055_ACCEL_MAX_PUBLISH_RATE 280
#define BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
/* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */
#define BMI055_NEW_DATA_MASK 0x01
class BMI055_accel : public BMI055
{
public:
BMI055_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode);
virtual ~BMI055_accel();
int init() override;
/// Start automatic measurement.
void start() override;
void print_status() override;
void print_registers() override;
/// deliberately cause a sensor error
void test_error() override;
void RunImpl() override;
protected:
int probe() override;
private:
PX4Accelerometer _px4_accel;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _duplicates;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_ACCEL_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
bool _got_duplicate;
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Modify a register in the BMI055_accel
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_accel, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_accel measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_accel_range(unsigned max_g);
/*
* check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_accel(const BMI055_accel &);
BMI055_accel operator=(const BMI055_accel &);
};

484
src/drivers/imu/bmi055/BMI055_gyro.cpp
View File

@@ -1,484 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI055_gyro.hpp"
using namespace time_literals;
/*
list of registers that will be checked in check_registers(). Note
that ADDR_WHO_AM_I must be first in the list.
*/
const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] = { BMI055_GYR_CHIP_ID,
BMI055_GYR_LPM1,
BMI055_GYR_BW,
BMI055_GYR_RANGE,
BMI055_GYR_INT_EN_0,
BMI055_GYR_INT_EN_1,
BMI055_GYR_INT_MAP_1
};
BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_gyro, uint32_t device,
enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
BMI055(DRV_GYR_DEVTYPE_BMI055, "bmi055_gyro", path_gyro, bus_option, bus, device, spi_mode, bus_frequency, rotation),
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi055_gyro_duplicates"))
{
}
BMI055_gyro::~BMI055_gyro()
{
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
BMI055_gyro::init()
{
/* do SPI init (and probe) first */
int ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
return ret;
}
return reset();
}
int BMI055_gyro::reset()
{
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset
usleep(5000);
write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate())
write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range
write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range
gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth
//Enable Gyroscope in normal mode
write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL);
up_udelay(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
return OK;
}
int
BMI055_gyro::probe()
{
/* look for device ID */
_whoami = read_reg(BMI055_GYR_CHIP_ID);
// verify product revision
switch (_whoami) {
case BMI055_GYR_WHO_AM_I:
memset(_checked_values, 0, sizeof(_checked_values));
memset(_checked_bad, 0, sizeof(_checked_bad));
_checked_values[0] = _whoami;
_checked_bad[0] = _whoami;
return OK;
}
DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
return -EIO;
}
int
BMI055_gyro::gyro_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_GYRO_BW_MASK;
if (frequency <= 100) {
setbits |= BMI055_GYRO_RATE_100; /* 32 Hz cutoff */
//_gyro_sample_rate = 100;
} else if (frequency <= 250) {
setbits |= BMI055_GYRO_RATE_400; /* 47 Hz cutoff */
//_gyro_sample_rate = 400;
} else if (frequency <= 1000) {
setbits |= BMI055_GYRO_RATE_1000; /* 116 Hz cutoff */
//_gyro_sample_rate = 1000;
} else if (frequency > 1000) {
setbits |= BMI055_GYRO_RATE_2000; /* 230 Hz cutoff */
//_gyro_sample_rate = 2000;
} else {
return -EINVAL;
}
modify_reg(BMI055_GYR_BW, clearbits, setbits);
return OK;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
BMI055_gyro::test_error()
{
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);
::printf("error triggered\n");
print_registers();
}
void
BMI055_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_checked_reg(reg, val);
}
void
BMI055_gyro::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
}
}
}
int
BMI055_gyro::set_gyro_range(unsigned max_dps)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_GYRO_RANGE_125_DPS | BMI055_GYRO_RANGE_250_DPS;
float lsb_per_dps;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 125) {
//max_gyro_dps = 125;
lsb_per_dps = 262.4;
setbits |= BMI055_GYRO_RANGE_125_DPS;
} else if (max_dps <= 250) {
//max_gyro_dps = 250;
lsb_per_dps = 131.2;
setbits |= BMI055_GYRO_RANGE_250_DPS;
} else if (max_dps <= 500) {
//max_gyro_dps = 500;
lsb_per_dps = 65.6;
setbits |= BMI055_GYRO_RANGE_500_DPS;
} else if (max_dps <= 1000) {
//max_gyro_dps = 1000;
lsb_per_dps = 32.8;
setbits |= BMI055_GYRO_RANGE_1000_DPS;
} else if (max_dps <= 2000) {
//max_gyro_dps = 2000;
lsb_per_dps = 16.4;
setbits |= BMI055_GYRO_RANGE_2000_DPS;
} else {
return -EINVAL;
}
_px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
modify_reg(BMI055_GYR_RANGE, clearbits, setbits);
return OK;
}
void
BMI055_gyro::start()
{
/* start polling at the specified rate */
ScheduleOnInterval((1_s / BMI055_GYRO_DEFAULT_RATE) / 2, 1000);
}
void
BMI055_gyro::check_registers(void)
{
uint8_t v;
if ((v = read_reg(_checked_registers[_checked_next])) !=
_checked_values[_checked_next]) {
_checked_bad[_checked_next] = v;
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % BMI055_GYRO_NUM_CHECKED_REGISTERS;
}
void
BMI055_gyro::RunImpl()
{
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
}
struct BMI_GyroReport bmi_gyroreport;
struct Report {
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report;
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMI055 gyro in one pass.
*/
bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ;
const hrt_abstime timestamp_sample = hrt_absolute_time();
if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
return;
}
check_registers();
int8_t temp = read_reg(BMI055_ACC_TEMP);
report.temp = temp;
report.gyro_x = bmi_gyroreport.gyro_x;
report.gyro_y = bmi_gyroreport.gyro_y;
report.gyro_z = bmi_gyroreport.gyro_z;
if (report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the bmi055 does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
if (_register_wait != 0) {
// we are waiting for some good transfers before using
// the sensor again, but don't return any data yet
_register_wait--;
return;
}
// don't publish duplicated reads
if ((report.gyro_x == _gyro_prev[0]) && (report.gyro_y == _gyro_prev[1]) && (report.gyro_z == _gyro_prev[2])) {
perf_end(_sample_perf);
perf_count(_duplicates);
return;
} else {
_gyro_prev[0] = report.gyro_x;
_gyro_prev[1] = report.gyro_y;
_gyro_prev[2] = report.gyro_z;
}
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
_px4_gyro.set_error_count(error_count);
/*
* Temperature is reported as Eight-bit 2’s complement sensor temperature value
* with 0.5 °C/LSB sensitivity and an offset of 23.0 °C
*/
_px4_gyro.set_temperature((report.temp * 0.5f) + 23.0f);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
/* stop measuring */
perf_end(_sample_perf);
}
void
BMI055_gyro::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("Type: Gyro");
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
if (v != _checked_bad[i]) {
::printf("reg %02x:%02x was bad %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_bad[i]);
}
}
_px4_gyro.print_status();
}
void
BMI055_gyro::print_registers()
{
uint8_t index = 0;
printf("BMI055 gyro registers\n");
uint8_t reg = _checked_registers[index++];
uint8_t v = read_reg(reg);
printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
}

238
src/drivers/imu/bmi055/BMI055_gyro.hpp
View File

@@ -1,238 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include "BMI055.hpp"
#define BMI055_DEVICE_PATH_GYRO "/dev/bmi055_gyro"
#define BMI055_DEVICE_PATH_GYRO_EXT "/dev/bmi055_gyro_ext"
// BMI055 Gyro registers
#define BMI055_GYR_CHIP_ID 0x00
#define BMI055_GYR_X_L 0x02
#define BMI055_GYR_X_H 0x03
#define BMI055_GYR_Y_L 0x04
#define BMI055_GYR_Y_H 0x05
#define BMI055_GYR_Z_L 0x06
#define BMI055_GYR_Z_H 0x07
#define BMI055_GYR_INT_STATUS_0 0x09
#define BMI055_GYR_INT_STATUS_1 0x0A
#define BMI055_GYR_INT_STATUS_2 0x0B
#define BMI055_GYR_INT_STATUS_3 0x0C
#define BMI055_GYR_FIFO_STATUS 0x0E
#define BMI055_GYR_RANGE 0x0F
#define BMI055_GYR_BW 0x10
#define BMI055_GYR_LPM1 0x11
#define BMI055_GYR_LPM2 0x12
#define BMI055_GYR_RATE_HBW 0x13
#define BMI055_GYR_SOFTRESET 0x14
#define BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_INT_EN_1 0x16
#define BMI055_GYR_INT_MAP_0 0x17
#define BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_INT_MAP_2 0x19
#define BMI055_GYRO_0_REG 0x1A
#define BMI055_GYRO_1_REG 0x1B
#define BMI055_GYRO_2_REG 0x1C
#define BMI055_GYRO_3_REG 0x1E
#define BMI055_GYR_INT_LATCH 0x21
#define BMI055_GYR_INT_LH_0 0x22
#define BMI055_GYR_INT_LH_1 0x23
#define BMI055_GYR_INT_LH_2 0x24
#define BMI055_GYR_INT_LH_3 0x25
#define BMI055_GYR_INT_LH_4 0x26
#define BMI055_GYR_INT_LH_5 0x27
#define BMI055_GYR_SOC 0x31
#define BMI055_GYR_A_FOC 0x32
#define BMI055_GYR_TRIM_NVM_CTRL 0x33
#define BMI055_BGW_SPI3_WDT 0x34
#define BMI055_GYR_OFFSET_COMP 0x36
#define BMI055_GYR_OFFSET_COMP_X 0x37
#define BMI055_GYR_OFFSET_COMP_Y 0x38
#define BMI055_GYR_OFFSET_COMP_Z 0x39
#define BMI055_GYR_TRIM_GPO 0x3A
#define BMI055_GYR_TRIM_GP1 0x3B
#define BMI055_GYR_SELF_TEST 0x3C
#define BMI055_GYR_FIFO_CONFIG_0 0x3D
#define BMI055_GYR_FIFO_CONFIG_1 0x3E
#define BMI055_GYR_FIFO_DATA 0x3F
// BMI055 Gyroscope Chip-Id
#define BMI055_GYR_WHO_AM_I 0x0F
// ODR & DLPF filter bandwidth settings (they are coupled)
#define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0)
//BMI055_GYR_LPM1 0x11
#define BMI055_GYRO_NORMAL (0<<7) | (0<<5)
#define BMI055_GYRO_DEEP_SUSPEND (0<<7) | (1<<5)
#define BMI055_GYRO_SUSPEND (1<<7) | (0<<5)
//BMI055_GYR_RANGE 0x0F
#define BMI055_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
#define BMI055_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
#define BMI055_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0)
//BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_DRDY_INT_EN (1<<7)
//BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_DRDY_INT1 (1<<0)
// Default and Max values
#define BMI055_GYRO_DEFAULT_RANGE_DPS 2000
#define BMI055_GYRO_DEFAULT_RATE 1000
#define BMI055_GYRO_MAX_RATE 1000
#define BMI055_GYRO_MAX_PUBLISH_RATE 280
#define BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50
/* Mask definitions for Gyro bandwidth */
#define BMI055_GYRO_BW_MASK 0x0F
#define BMI055_ACC_TEMP 0x08
class BMI055_gyro : public BMI055
{
public:
BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode);
virtual ~BMI055_gyro();
int init() override;
/// Start automatic measurement.
void start() override;
void print_status() override;
void print_registers() override;
/// deliberately cause a sensor error
void test_error() override;
void RunImpl() override;
protected:
int probe() override;
private:
PX4Gyroscope _px4_gyro;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _duplicates;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_GYRO_NUM_CHECKED_REGISTERS 7
static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS];
int16_t _gyro_prev[3] {};
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Modify a register in the BMI055_gyro
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_gyro, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_gyro measurement range.
*
* @param max_dps The maximum DPS value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_gyro_range(unsigned max_dps);
/*
* set gyro sample rate
*/
int gyro_set_sample_rate(float desired_sample_rate_hz);
/*
* check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_gyro(const BMI055_gyro &);
BMI055_gyro operator=(const BMI055_gyro &);
#pragma pack(push, 1)
/**
* Report conversation within the BMI055_gyro, including command byte and
* interrupt status.
*/
struct BMI_GyroReport {
uint8_t cmd;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
};
#pragma pack(pop)
};

92
src/drivers/imu/bosch/bmi055/BMI055.cpp Normal file
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.
*
****************************************************************************/
#include "BMI055.hpp"
#include "BMI055_Accelerometer.hpp"
#include "BMI055_Gyroscope.hpp"
I2CSPIDriverBase *BMI055::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
BMI055 *instance = nullptr;
if (cli.type == DRV_ACC_DEVTYPE_BMI055) {
instance = new Bosch::BMI055::Accelerometer::BMI055_Accelerometer(iterator.configuredBusOption(), iterator.bus(),
iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
} else if (cli.type == DRV_GYR_DEVTYPE_BMI055) {
instance = new Bosch::BMI055::Gyroscope::BMI055_Gyroscope(iterator.configuredBusOption(), iterator.bus(),
iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
}
if (!instance) {
PX4_ERR("alloc failed");
return nullptr;
}
if (OK != instance->init()) {
delete instance;
return nullptr;
}
return instance;
}
BMI055::BMI055(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device,
enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) :
SPI(devtype, name, bus, device, mode, frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype),
_drdy_gpio(drdy_gpio)
{
}
int BMI055::init()
{
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
return Reset() ? 0 : -1;
}
bool BMI055::Reset()
{
_state = STATE::RESET;
ScheduleClear();
ScheduleNow();
return true;
}

69
src/drivers/imu/bmi055/BMI055.hpp → src/drivers/imu/bosch/bmi055/BMI055.hpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,67 +33,54 @@
#pragma once
#include <drivers/device/spi.h>
#include <ecl/geo/geo.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/spi.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
//Soft-reset command Value
#define BMI055_SOFT_RESET 0xB6
#define BMI055_BUS_SPEED 10*1000*1000
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
class BMI055 : public device::SPI, public I2CSPIDriver<BMI055>
{
public:
BMI055(uint8_t devtype, const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, uint32_t device,
enum spi_mode_e mode, uint32_t frequency, enum Rotation rotation);
BMI055(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 ~BMI055() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual void start() = 0;
virtual void RunImpl() = 0;
int init() override;
virtual void print_status() = 0;
protected:
virtual void print_registers() = 0;
virtual void test_error() = 0;
bool Reset();
void custom_method(const BusCLIArguments &cli) override;
const spi_drdy_gpio_t _drdy_gpio;
uint8_t _whoami; ///< whoami result
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
unsigned _consecutive_failures{0};
unsigned _total_failures{0};
uint8_t _register_wait;
uint64_t _reset_wait;
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
bool _data_ready_interrupt_enabled{false};
enum Rotation _rotation;
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
uint8_t _checked_next;
STATE _state{STATE::RESET};
/**
* Read a register from the BMI055
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg) override;
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the BMI055
*
* @param reg The register to write.
* @param value The new value to write.
* @return OK on success, negative errno otherwise.
*/
int write_reg(unsigned reg, uint8_t value) override;
uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
};

467
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp Normal file
View File

@@ -0,0 +1,467 @@
/****************************************************************************
*
* 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 "BMI055_Accelerometer.hpp"
#include <ecl/geo/geo.h> // CONSTANTS_ONE_G
using namespace time_literals;
namespace Bosch::BMI055::Accelerometer
{
BMI055_Accelerometer::BMI055_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) :
BMI055(DRV_ACC_DEVTYPE_BMI055, "BMI055_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_accel: DRDY interval");
}
ConfigureSampleRate(_px4_accel.get_max_rate_hz());
}
BMI055_Accelerometer::~BMI055_Accelerometer()
{
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_interval_perf);
}
void BMI055_Accelerometer::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void BMI055_Accelerometer::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf);
_px4_accel.print_status();
}
int BMI055_Accelerometer::probe()
{
const uint8_t BGW_CHIPID = RegisterRead(Register::BGW_CHIPID);
if (BGW_CHIPID != chip_id) {
DEVICE_DEBUG("unexpected BGW_CHIPID 0x%02x", BGW_CHIPID);
return PX4_ERROR;
}
return PX4_OK;
}
void BMI055_Accelerometer::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
RegisterWrite(Register::BGW_SOFTRESET, 0xB6);
_reset_timestamp = now;
_consecutive_failures = 0;
_total_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(25_ms);
break;
case STATE::WAIT_FOR_RESET:
if (RegisterRead(Register::BGW_CHIPID) == chip_id) {
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleDelayed(25_ms);
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
}
}
break;
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading from FIFO
_state = STATE::FIFO_READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
FIFOReset();
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(10_ms);
}
break;
case STATE::FIFO_READ: {
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_accel_samples) {
perf_count_interval(_drdy_interval_perf, now);
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
// always check current FIFO status/count
bool success = false;
const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
FIFOReset();
perf_count(_fifo_overflow_perf);
} else {
const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
// not necessarily an actual FIFO overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (fifo_frame_counter == 0) {
perf_count(_fifo_empty_perf);
} else if (fifo_frame_counter >= 1) {
if (FIFORead(now, fifo_frame_counter)) {
success = true;
_consecutive_failures = 0;
}
}
}
if (!success) {
_consecutive_failures++;
_total_failures++;
// full reset if things are failing consistently
if (_consecutive_failures > 100 || _total_failures > 1000) {
Reset();
return;
}
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
_checked_register = (_checked_register + 1) % size_register_cfg;
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
}
}
break;
}
}
void BMI055_Accelerometer::ConfigureAccel()
{
const uint8_t PMU_RANGE = RegisterRead(Register::PMU_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
switch (PMU_RANGE) {
case range_2g:
_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); // 1024 LSB/g, 0.98mg/LSB
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
break;
case range_4g:
_px4_accel.set_scale(CONSTANTS_ONE_G / 512.f); // 512 LSB/g, 1.95mg/LSB
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
break;
case range_8g:
_px4_accel.set_scale(CONSTANTS_ONE_G / 256.f); // 256 LSB/g, 3.91mg/LSB
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
break;
case range_16g:
_px4_accel.set_scale(CONSTANTS_ONE_G / 128.f); // 128 LSB/g, 7.81mg/LSB
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
break;
}
}
void BMI055_Accelerometer::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 1000; // default to 1000 Hz
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_accel_samples = math::min((float)_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
// recompute FIFO empty interval (us) with actual accel sample limit
_fifo_empty_interval_us = _fifo_accel_samples * (1e6f / ACCEL_RATE);
ConfigureFIFOWatermark(_fifo_accel_samples);
}
void BMI055_Accelerometer::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 BMI055_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 BMI055_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<BMI055_Accelerometer *>(arg)->DataReady();
return 0;
}
void BMI055_Accelerometer::DataReady()
{
uint8_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_accel_samples)) {
ScheduleNow();
}
}
bool BMI055_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 BMI055_Accelerometer::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool BMI055_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;
}
uint8_t BMI055_Accelerometer::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 BMI055_Accelerometer::RegisterWrite(Register reg, uint8_t value)
{
uint8_t cmd[2] {(uint8_t)reg, value};
transfer(cmd, cmd, sizeof(cmd));
}
void BMI055_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);
}
}
bool BMI055_Accelerometer::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);
perf_begin(_transfer_perf);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf);
return false;
}
perf_end(_transfer_perf);
PX4Accelerometer::FIFOSample accel;
accel.timestamp_sample = timestamp_sample;
accel.samples = samples;
accel.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
// acc_x_msb<11:4> + acc_x_lsb<3:0>
const int16_t accel_x = combine(fifo_sample.ACCD_X_MSB, fifo_sample.ACCD_X_LSB) >> 4;
const int16_t accel_y = combine(fifo_sample.ACCD_Y_MSB, fifo_sample.ACCD_Y_LSB) >> 4;
const int16_t accel_z = combine(fifo_sample.ACCD_Z_MSB, fifo_sample.ACCD_Z_LSB) >> 4;
// 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[i] = accel_x;
accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
}
_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));
_px4_accel.updateFIFO(accel);
return true;
}
void BMI055_Accelerometer::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);
}
}
}
void BMI055_Accelerometer::UpdateTemperature()
{
// The slope of the temperature sensor is 0.5K/LSB, its center temperature is 23°C [(ACC 0x08) temp = 0x00].
float temperature = RegisterRead(Register::ACCD_TEMP) * 0.5f + 23.f;
if (PX4_ISFINITE(temperature)) {
_px4_accel.set_temperature(temperature);
}
}
} // namespace Bosch::BMI055::Accelerometer

128
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.hpp Normal file
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 "BMI055.hpp"
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include "Bosch_BMI055_Accelerometer_Registers.hpp"
namespace Bosch::BMI055::Accelerometer
{
class BMI055_Accelerometer : public BMI055
{
public:
BMI055_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);
~BMI055_Accelerometer() override;
void RunImpl() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr uint32_t ACCEL_RATE{2000}; // 2000 Hz accel
static constexpr float FIFO_SAMPLE_DT{1e6f / ACCEL_RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Accelerometer::FIFOSample::x) / sizeof(PX4Accelerometer::FIFOSample::x[0]))};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<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 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);
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();
void UpdateTemperature();
PX4Accelerometer _px4_accel;
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_accel: transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: bad transfer")};
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_accel: FIFO reset")};
perf_counter_t _drdy_interval_perf{nullptr};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_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::PMU_RANGE, PMU_RANGE_BIT::range_16g, Bit1 | Bit0},
{ Register::ACCD_HBW, ACCD_HBW_BIT::data_high_bw, 0},
{ Register::INT_EN_1, INT_EN_1_BIT::int_fwm_en, 0},
{ Register::INT_MAP_1, INT_MAP_1_BIT::int1_fwm, 0},
{ Register::INT_OUT_CTRL, 0, INT_OUT_CTRL_BIT::int1_od | INT_OUT_CTRL_BIT::int1_lvl},
{ Register::INT_RST_LATCH, INT_RST_LATCH_BIT::temporary_250us, 0},
{ Register::FIFO_CONFIG_0, 0, 0 }, // fifo_water_mark_level_trigger_retain<5:0>
{ Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::fifo_mode, 0},
};
};
} // namespace Bosch::BMI055::Accelerometer

461
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp Normal file
View File

@@ -0,0 +1,461 @@
/****************************************************************************
*
* 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 "BMI055_Gyroscope.hpp"
#include <px4_platform/board_dma_alloc.h>
using namespace time_literals;
namespace Bosch::BMI055::Gyroscope
{
BMI055_Gyroscope::BMI055_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) :
BMI055(DRV_GYR_DEVTYPE_BMI055, "BMI055_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_gyro: DRDY interval");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
}
BMI055_Gyroscope::~BMI055_Gyroscope()
{
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_interval_perf);
}
void BMI055_Gyroscope::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void BMI055_Gyroscope::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf);
_px4_gyro.print_status();
}
int BMI055_Gyroscope::probe()
{
const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID);
if (CHIP_ID != chip_id) {
DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", CHIP_ID);
return PX4_ERROR;
}
return PX4_OK;
}
void BMI055_Gyroscope::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// BGW_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
RegisterWrite(Register::BGW_SOFTRESET, 0xB6);
_reset_timestamp = now;
_consecutive_failures = 0;
_total_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(25_ms);
break;
case STATE::WAIT_FOR_RESET:
if (RegisterRead(Register::CHIP_ID) == chip_id) {
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleDelayed(1_ms);
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
}
}
break;
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading from FIFO
_state = STATE::FIFO_READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
} else {
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
FIFOReset();
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(10_ms);
}
break;
case STATE::FIFO_READ: {
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) {
perf_count_interval(_drdy_interval_perf, now);
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
// always check current FIFO status/count
bool success = false;
const uint8_t FIFO_STATUS = RegisterRead(Register::FIFO_STATUS);
if (FIFO_STATUS & FIFO_STATUS_BIT::fifo_overrun) {
FIFOReset();
perf_count(_fifo_overflow_perf);
} else {
const uint8_t fifo_frame_counter = FIFO_STATUS & FIFO_STATUS_BIT::fifo_frame_counter;
if (fifo_frame_counter > FIFO_MAX_SAMPLES) {
// not necessarily an actual FIFO overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (fifo_frame_counter == 0) {
perf_count(_fifo_empty_perf);
} else if (fifo_frame_counter >= 1) {
if (FIFORead(now, fifo_frame_counter)) {
success = true;
_consecutive_failures = 0;
}
}
}
if (!success) {
_consecutive_failures++;
_total_failures++;
// full reset if things are failing consistently
if (_consecutive_failures > 100 || _total_failures > 1000) {
Reset();
return;
}
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
_checked_register = (_checked_register + 1) % size_register_cfg;
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
}
}
break;
}
}
void BMI055_Gyroscope::ConfigureGyro()
{
const uint8_t RANGE = RegisterRead(Register::RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
switch (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 BMI055_Gyroscope::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 1000; // default to 1000 Hz
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES);
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
ConfigureFIFOWatermark(_fifo_gyro_samples);
}
void BMI055_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 | FIFO_CONFIG_0_BIT::tag;
}
}
}
bool BMI055_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 BMI055_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<BMI055_Gyroscope *>(arg)->DataReady();
return 0;
}
void BMI055_Gyroscope::DataReady()
{
uint8_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
}
bool BMI055_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 BMI055_Gyroscope::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool BMI055_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 BMI055_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 BMI055_Gyroscope::RegisterWrite(Register reg, uint8_t value)
{
uint8_t cmd[2] {(uint8_t)reg, value};
transfer(cmd, cmd, sizeof(cmd));
}
void BMI055_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 BMI055_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);
perf_begin(_transfer_perf);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
perf_count(_bad_transfer_perf);
return false;
}
perf_end(_transfer_perf);
PX4Gyroscope::FIFOSample gyro;
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
const int16_t gyro_x = combine(fifo_sample.RATE_X_MSB, fifo_sample.RATE_X_LSB);
const int16_t gyro_y = combine(fifo_sample.RATE_Y_MSB, fifo_sample.RATE_Y_LSB);
const int16_t gyro_z = combine(fifo_sample.RATE_Z_MSB, fifo_sample.RATE_Z_LSB);
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro_x;
gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_gyro.updateFIFO(gyro);
return true;
}
void BMI055_Gyroscope::FIFOReset()
{
perf_count(_fifo_reset_perf);
// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
RegisterWrite(Register::FIFO_CONFIG_0, 0);
// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO
for (const auto &r : _register_cfg) {
if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
}
}
}
} // namespace Bosch::BMI055::Gyroscope

127
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.hpp Normal file
View File

@@ -0,0 +1,127 @@
/****************************************************************************
*
* 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 "BMI055.hpp"
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include "Bosch_BMI055_Gyroscope_Registers.hpp"
namespace Bosch::BMI055::Gyroscope
{
class BMI055_Gyroscope : public BMI055
{
public:
BMI055_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);
~BMI055_Gyroscope() override;
void RunImpl() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr uint32_t GYRO_RATE{2000}; // 2000 Hz gyro
static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_RATE};
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<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();
PX4Gyroscope _px4_gyro;
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_gyro: transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: bad transfer")};
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME"_gyro: FIFO reset")};
perf_counter_t _drdy_interval_perf{nullptr};
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{9};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::RANGE, RANGE_BIT::gyro_range_2000_dps, 0 },
{ Register::RATE_HBW, RATE_HBW_BIT::data_high_bw, 0 },
{ Register::INT_EN_0, INT_EN_0_BIT::fifo_en, 0 },
{ Register::INT_EN_1, 0, INT_EN_1_BIT::int1_od | INT_EN_1_BIT::int1_lvl },
{ Register::INT_MAP_1, INT_MAP_1_BIT::int1_fifo, 0 },
{ Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 },
{ Register::INT_RST_LATCH, INT_RST_LATCH_BIT::temporary_250us, 0 },
{ Register::FIFO_CONFIG_0, 0, FIFO_CONFIG_0_BIT::tag }, // fifo_water_mark_level_trigger_retain<6:0>
{ Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::fifo_mode, 0 },
};
};
} // namespace Bosch::BMI055::Gyroscope

157
src/drivers/imu/bosch/bmi055/Bosch_BMI055_Accelerometer_Registers.hpp Normal file
View File

@@ -0,0 +1,157 @@
/****************************************************************************
*
* 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::BMI055::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 chip_id = 0b11111010;
enum class Register : uint8_t {
BGW_CHIPID = 0x00,
ACCD_TEMP = 0x08,
INT_STATUS_1 = 0x0A,
FIFO_STATUS = 0x0E,
PMU_RANGE = 0x0F,
ACCD_HBW = 0x13,
BGW_SOFTRESET = 0x14,
INT_EN_1 = 0x17,
INT_MAP_1 = 0x1A,
INT_OUT_CTRL = 0x20,
INT_RST_LATCH = 0x21,
FIFO_CONFIG_0 = 0x30,
FIFO_CONFIG_1 = 0x3E,
FIFO_DATA = 0x3F,
};
// INT_STATUS_1
enum INT_STATUS_1_BIT : uint8_t {
data_int = Bit7,
fifo_wm_int = Bit6,
fifo_full_int = Bit5,
};
// FIFO_STATUS
enum FIFO_STATUS_BIT : uint8_t {
fifo_overrun = Bit7,
fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // fifo_frame_counter<6:0>
};
// ACCD_HBW
enum ACCD_HBW_BIT : uint8_t {
data_high_bw = Bit7, // 1 -> unfiltered
};
// PMU_RANGE
enum PMU_RANGE_BIT : uint8_t {
// range<3:0>
range_2g = Bit1 | Bit0, // ́0011b ́ -> ±2g range
range_4g = Bit2 | Bit0, // ́0101b ́ -> ±4g range
range_8g = Bit3, // ́1000b ́ -> ±8g range
range_16g = Bit3 | Bit2, // ́1100b ́ -> ±16g range
};
// INT_EN_1
enum INT_EN_1_BIT : uint8_t {
int_fwm_en = Bit6,
int_ffull_en = Bit5,
data_en = Bit4,
};
// INT_MAP_1
enum INT_MAP_1_BIT : uint8_t {
int2_data = Bit7,
int2_fwm = Bit6,
int2_ffull = Bit5,
int1_ffull = Bit2,
int1_fwm = Bit1,
int1_data = Bit0,
};
// INT_OUT_CTRL
enum INT_OUT_CTRL_BIT : uint8_t {
int1_od = Bit1,
int1_lvl = Bit0,
};
// INT_RST_LATCH
enum INT_RST_LATCH_BIT : uint8_t {
// latch_int<3:0>
temporary_250us = Bit3 | Bit0, // 1001b -> temporary, 250 us
};
// FIFO_CONFIG_1
enum FIFO_CONFIG_1_BIT : uint8_t {
fifo_mode = Bit6,
};
namespace FIFO
{
struct DATA {
uint8_t ACCD_X_LSB;
uint8_t ACCD_X_MSB;
uint8_t ACCD_Y_LSB;
uint8_t ACCD_Y_MSB;
uint8_t ACCD_Z_LSB;
uint8_t ACCD_Z_MSB;
};
static_assert(sizeof(DATA) == 6);
static constexpr size_t SIZE = sizeof(DATA) * 32; // up to 32 frames of accelerometer data
} // namespace FIFO
} // namespace Bosch::BMI055::Accelerometer

151
src/drivers/imu/bosch/bmi055/Bosch_BMI055_Gyroscope_Registers.hpp Normal file
View File

@@ -0,0 +1,151 @@
/****************************************************************************
*
* 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::BMI055::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 chip_id = 0x0F;
enum class Register : uint8_t {
CHIP_ID = 0x00,
FIFO_STATUS = 0x0E,
RANGE = 0x0F,
RATE_HBW = 0x13,
BGW_SOFTRESET = 0x14,
INT_EN_0 = 0x15,
INT_EN_1 = 0x16,
INT_MAP_1 = 0x18,
FIFO_WM_ENABLE = 0x1E,
INT_RST_LATCH = 0x21,
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, // fifo_frame_counter<6:0>
};
// RANGE
enum 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
};
// RATE_HBW
enum RATE_HBW_BIT : uint8_t {
data_high_bw = Bit7, // 1 -> unfiltered
};
// INT_EN_0
enum INT_EN_0_BIT : uint8_t {
data_en = Bit7,
fifo_en = Bit6,
};
// INT_EN_1
enum INT_EN_1_BIT : uint8_t {
int1_od = Bit1,
int1_lvl = Bit0,
};
// INT_MAP_1
enum INT_MAP_1_BIT : uint8_t {
int1_fifo = Bit2,
int1_data = Bit0,
};
// FIFO_WM_ENABLE
enum FIFO_WM_ENABLE_BIT : uint8_t {
fifo_wm_enable = Bit7,
};
// INT_RST_LATCH
enum INT_RST_LATCH_BIT : uint8_t {
// latch_int<3:0>
temporary_250us = Bit3 | Bit0, // 1001b -> temporary, 250 us
};
// FIFO_CONFIG_0
enum FIFO_CONFIG_0_BIT : uint8_t {
tag = Bit7,
};
// 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::BMI055::Gyroscope

22
src/drivers/imu/bmi055/CMakeLists.txt → src/drivers/imu/bosch/bmi055/CMakeLists.txt
View File

@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -30,13 +30,25 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__bmi055
MODULE drivers__imu__bosch__bmi055
MAIN bmi055
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
BMI055_accel.cpp
BMI055_gyro.cpp
Bosch_BMI055_Accelerometer_Registers.hpp
Bosch_BMI055_Gyroscope_Registers.hpp
BMI055.cpp
BMI055.hpp
BMI055_Accelerometer.cpp
BMI055_Accelerometer.hpp
BMI055_Gyroscope.cpp
BMI055_Gyroscope.hpp
bmi055_main.cpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
px4_work_queue
)

117
src/drivers/imu/bmi055/bmi055_main.cpp → src/drivers/imu/bosch/bmi055/bmi055_main.cpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,16 +31,12 @@
*
****************************************************************************/
#include "BMI055_accel.hpp"
#include "BMI055_gyro.hpp"
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
/** driver 'main' command */
extern "C" { __EXPORT int bmi055_main(int argc, char *argv[]); }
#include "BMI055.hpp"
void
BMI055::print_usage()
void BMI055::print_usage()
{
PRINT_MODULE_USAGE_NAME("bmi055", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
@@ -49,105 +45,16 @@ BMI055::print_usage()
PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_COMMAND("regdump");
PRINT_MODULE_USAGE_COMMAND("testerror");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
I2CSPIDriverBase *BMI055::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
extern "C" int bmi055_main(int argc, char *argv[])
{
BMI055 *instance = nullptr;
if (cli.type == DRV_ACC_DEVTYPE_BMI055) {
instance = new BMI055_accel(iterator.configuredBusOption(), iterator.bus(), BMI055_DEVICE_PATH_ACCEL, iterator.devid(),
cli.rotation, cli.bus_frequency, cli.spi_mode);
} else if (cli.type == DRV_GYR_DEVTYPE_BMI055) {
instance = new BMI055_gyro(iterator.configuredBusOption(), iterator.bus(), BMI055_DEVICE_PATH_GYRO, iterator.devid(),
cli.rotation, cli.bus_frequency, cli.spi_mode);
}
if (instance == nullptr) {
return nullptr;
}
if (OK != instance->init()) {
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
delete instance;
return nullptr;
}
instance->start();
return instance;
}
void
BMI055::custom_method(const BusCLIArguments &cli)
{
switch (cli.custom1) {
case 0: print_registers();
break;
case 1: test_error();
break;
}
}
BMI055::BMI055(uint8_t devtype, const char *name, const char *devname, I2CSPIBusOption bus_option, int bus,
uint32_t device, enum spi_mode_e mode, uint32_t frequency, enum Rotation rotation):
SPI(devtype, name, bus, device, mode, frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, devtype),
_whoami(0),
_register_wait(0),
_reset_wait(0),
_rotation(rotation),
_checked_next(0)
{
}
uint8_t
BMI055::read_reg(unsigned reg)
{
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
uint16_t
BMI055::read_reg16(unsigned reg)
{
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
transfer(cmd, cmd, sizeof(cmd));
return (uint16_t)(cmd[1] << 8) | cmd[2];
}
int
BMI055::write_reg(unsigned reg, uint8_t value)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
return transfer(cmd, nullptr, sizeof(cmd));
}
int
bmi055_main(int argc, char *argv[])
{
using ThisDriver = BMI055;
int ch;
using ThisDriver = BMI055;
BusCLIArguments cli{false, true};
cli.type = 0;
cli.default_spi_frequency = BMI055_BUS_SPEED;
cli.default_spi_frequency = 10000000;
while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) {
switch (ch) {
@@ -186,16 +93,6 @@ bmi055_main(int argc, char *argv[])
return ThisDriver::module_status(iterator);
}
if (!strcmp(verb, "regdump")) {
cli.custom1 = 0;
return ThisDriver::module_custom_method(cli, iterator);
}
if (!strcmp(verb, "testerror")) {
cli.custom1 = 1;
return ThisDriver::module_custom_method(cli, iterator);
}
ThisDriver::print_usage();
return -1;
}

6
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@@ -54,6 +54,8 @@ public:
uint32_t get_device_id() const { return _device_id; }
float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint64_t error_count) { _error_count = error_count; }
@@ -104,4 +106,8 @@ private:
uint32_t _error_count{0};
int16_t _last_sample[3] {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
)
};