fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

new Bosch BMI088 IMU driver using FIFOs and DRDY

Browse Source
This commit is contained in:
Daniel Agar
2020-06-11 12:25:13 -04:00
parent b6ec00c079
commit 13e34b32e6
30 changed files with 1801 additions and 1739 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/cuav/x7pro/default.cmake
+1 -1
View File
@@ -32,7 +32,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi088
imu/bosch/bmi088
imu/invensense/icm20649
imu/invensense/icm20689
irlock
boards/hex/cube-orange/default.cmake
-1
View File
@@ -33,7 +33,6 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi088
imu/invensense/icm20602
imu/invensense/icm20649
imu/invensense/icm20948
boards/holybro/durandal-v1/default.cmake
+1 -1
View File
@@ -31,7 +31,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi088
imu/bosch/bmi088
imu/invensense/icm20689
irlock
lights/blinkm
boards/holybro/durandal-v1/init/rc.board_sensors
+3 -3
View File
@@ -7,9 +7,9 @@ adc start
# Internal SPI bus ICM-20689
icm20689 -R 2 -s start
# Internal SPI bus BMI088 accel/gyro
bmi088 -A -R 10 -s start
bmi088 -G -R 10 -s start
# Internal SPI BMI088
bmi088 -A -R 2 -s start
bmi088 -G -R 2 -s start
# internal compass
ist8310 -I start
boards/holybro/durandal-v1/stackcheck.cmake
+1 -1
View File
@@ -32,7 +32,7 @@ px4_add_board(
#imu/adis16448
#imu/adis16477
#imu/adis16497
#imu/bmi088
#imu/bosch/bmi088
imu/invensense/icm20689
#irlock
#lights/blinkm
boards/modalai/fc-v1/default.cmake
+1 -1
View File
@@ -24,7 +24,7 @@ px4_add_board(
distance_sensor # all available distance sensor drivers
dshot
gps
imu/bmi088
imu/bosch/bmi088
imu/invensense/icm20602
imu/invensense/icm42688p
irlock
boards/modalai/fc-v1/init/rc.board_sensors
+3 -3
View File
@@ -15,9 +15,9 @@ icm20602 -R 12 -s start
# Internal SPI bus ICM-42688
icm42688p -R 12 -s start
# Internal SPI bus BMI088 accel/gyro
bmi088 -A -R 4 -s start
bmi088 -G -R 4 -s start
# Internal SPI4 BMI088
bmi088 -A -R 12 -s start
bmi088 -G -R 12 -s start
# Internal I2C Baro
bmp388 -I start
boards/mro/ctrl-zero-f7/default.cmake
+1 -1
View File
@@ -29,7 +29,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/bmi088
imu/bosch/bmi088
imu/invensense/icm20602
imu/invensense/icm20948
irlock
boards/mro/ctrl-zero-f7/init/rc.board_sensors
+2 -2
View File
@@ -9,8 +9,8 @@ adc start
icm20602 -s -R 8 start
# Internal SPI bus BMI088 accel & gyro
bmi088 -A -R 10 -s start
bmi088 -G -R 10 -s start
bmi088 -A -R 8 -s start
bmi088 -G -R 8 -s start
# Internal ICM-20948 (with magnetometer)
icm20948 -s -R 8 -M start
boards/px4/fmu-v5x/base_phy_DP83848C.cmake
+1 -1
View File
@@ -31,7 +31,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi088
imu/bosch/bmi088
imu/invensense/icm20602
imu/st/ism330dlc
irlock
boards/px4/fmu-v5x/default.cmake
+1 -1
View File
@@ -31,7 +31,7 @@ px4_add_board(
imu/adis16448
imu/adis16477
imu/adis16497
imu/bmi088
imu/bosch/bmi088
imu/invensense/icm20602
imu/st/ism330dlc
irlock
boards/px4/fmu-v5x/init/rc.board_sensors
+3 -3
View File
@@ -15,9 +15,9 @@ icm20602 -R 2 -s start
# Internal SPI bus ISM300DLC
ism330dlc -s start
# Internal SPI bus BMI088 accel/gyro
bmi088 -A -R 12 -s start
bmi088 -G -R 12 -s start
# Internal SPI BMI088
bmi088 -A -R 4 -s start
bmi088 -G -R 4 -s start
# Possible internal compass
bmm150 -I start
boards/px4/fmu-v5x/src/spi.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
}, {GPIO::PortD, GPIO::Pin15}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
platforms/common/include/px4_platform_common/log.h
+1
View File
@@ -122,6 +122,7 @@ __END_DECLS
#include <stdarg.h>
#include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
__BEGIN_DECLS
src/drivers/imu/bmi088/BMI088_accel.cpp
-584
View File
@@ -1,584 +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 "BMI088_accel.hpp"
using namespace time_literals;
/*
* Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver. The variable is changed in bmi055_accel.cpp.
* This is a HACK! The driver should be rewritten with the gyro as subdriver.
*/
extern float _accel_last_temperature_copy;
/*
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 BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS] = {BMI088_ACC_CHIP_ID,
BMI088_ACC_CONF,
BMI088_ACC_RANGE,
BMI088_ACC_INT1_IO_CONF,
BMI088_ACC_INT1_INT2_MAP_DATA,
BMI088_ACC_PWR_CONF,
BMI088_ACC_PWR_CTRL,
};
BMI088_accel::BMI088_accel(I2CSPIBusOption bus_option, int bus, const char *path_accel, uint32_t device,
enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode) :
BMI088("bmi088_accel", path_accel, bus_option, bus, DRV_ACC_DEVTYPE_BMI088, 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, "bmi088_accel_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")),
_got_duplicate(false)
{
}
BMI088_accel::~BMI088_accel()
{
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
BMI088_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();
}
uint8_t
BMI088_accel::read_reg(unsigned reg)
{
// For the BMI088, you need to read out a dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet)
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0};
transfer(cmd, cmd, sizeof(cmd));
return cmd[2]; // Skip dummy byte in cmd[1] and read out actual data
}
uint16_t
BMI088_accel::read_reg16(unsigned reg)
{
// For the BMI088, you need to read out the dummy byte before you can read out the normal data (see section "SPI interface of accelerometer part" of the BMI088 datasheet)
uint8_t cmd[4] = { (uint8_t)(reg | DIR_READ), 0, 0, 0 };
transfer(cmd, cmd, sizeof(cmd));
return (uint16_t)(cmd[2] << 8) | cmd[3]; // Skip dummy byte in cmd[1]
}
int BMI088_accel::reset()
{
write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET); // Soft-reset
/* After a POR or soft-reset, the sensor needs up to 1ms boot time
* (see section "Power Modes: Accelerometer" in the BMI088 datasheet).
* Based off of testing it seems this value needs to be increased from 1ms.
* Setting it to 5ms.
*/
px4_usleep(5000);
// Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset
// The dummy read basically pulls the chip select line low and then high
// See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details.
read_reg(BMI088_ACC_CHIP_ID);
// Enable Accelerometer
// The accelerometer needs to be enabled first, before writing to its registers
write_checked_reg(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_CTRL_EN);
/* After changing power modes, the sensor requires up to 5ms to settle.
* Any communication with the sensor during this time should be avoided
* (see section "Power Modes: Acceleromter" in the BMI datasheet) */
px4_usleep(5000);
// Set the PWR CONF to be active
write_checked_reg(BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_CONF_ACTIVE); // Sets the accelerometer to active mode
// Write accel bandwidth and output data rate
// ToDo set the bandwidth
accel_set_sample_rate(BMI088_ACCEL_DEFAULT_RATE); //set accel ODR
set_accel_range(BMI088_ACCEL_DEFAULT_RANGE_G); //set accel range
// Configure the accel INT1
write_checked_reg(BMI088_ACC_INT1_IO_CONF,
BMI088_ACC_INT1_IO_CONF_INT1_OUT | BMI088_ACC_INT1_IO_CONF_PP |
BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH); // Configure INT1 pin as output, push-pull, active high
write_checked_reg(BMI088_ACC_INT1_INT2_MAP_DATA,
BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY); // Map DRDY interrupt on pin INT1
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI088_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
BMI088_accel::probe()
{
// Perform a dummy read here to put the accelerometer part of the BMI088 back into SPI mode after the reset
// The dummy read basically pulls the chip select line low and then high
// See section "Serial Peripheral Interface (SPI)" of the BMI088 datasheet for more details.
read_reg(BMI088_ACC_CHIP_ID);
/* look for device ID */
_whoami = read_reg(BMI088_ACC_CHIP_ID);
// verify product revision
switch (_whoami) {
case BMI088_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;
}
int
BMI088_accel::accel_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = 0x0F;
if (frequency < 25) {
setbits |= BMI088_ACC_CONF_ODR_12_5;
//_accel_sample_rate = 12.5f;
} else if (frequency < 50) {
setbits |= BMI088_ACC_CONF_ODR_25;
//_accel_sample_rate = 25.f;
} else if (frequency < 100) {
setbits |= BMI088_ACC_CONF_ODR_50;
//_accel_sample_rate = 50.f;
} else if (frequency < 200) {
setbits |= BMI088_ACC_CONF_ODR_100;
//_accel_sample_rate = 100.f;
} else if (frequency < 400) {
setbits |= BMI088_ACC_CONF_ODR_200;
//_accel_sample_rate = 200.f;
} else if (frequency < 800) {
setbits |= BMI088_ACC_CONF_ODR_400;
//_accel_sample_rate = 400.f;
} else if (frequency < 1600) {
setbits |= BMI088_ACC_CONF_ODR_800;
//_accel_sample_rate = 800.f;
} else if (frequency >= 1600) {
setbits |= BMI088_ACC_CONF_ODR_1600;
//_accel_sample_rate = 1600.f;
} else {
printf("Set sample rate error \n");
return -EINVAL;
}
/* Write accel ODR */
modify_reg(BMI088_ACC_CONF, clearbits, setbits);
return OK;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
BMI088_accel::test_error()
{
write_reg(BMI088_ACC_SOFTRESET, BMI088_SOFT_RESET);
::printf("error triggered\n");
print_registers();
}
void
BMI088_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
BMI088_accel::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < BMI088_ACCEL_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
}
}
}
int
BMI088_accel::set_accel_range(unsigned max_g)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI088_ACCEL_RANGE_24_G;
float lsb_per_g;
if (max_g == 0) {
max_g = 24;
}
if (max_g <= 3) {
//max_accel_g = 3;
setbits |= BMI088_ACCEL_RANGE_3_G;
lsb_per_g = 10922.67;
} else if (max_g <= 6) {
//max_accel_g = 6;
setbits |= BMI088_ACCEL_RANGE_6_G;
lsb_per_g = 5461.33;
} else if (max_g <= 12) {
//max_accel_g = 12;
setbits |= BMI088_ACCEL_RANGE_12_G;
lsb_per_g = 2730.67;
} else if (max_g <= 24) {
//max_accel_g = 24;
setbits |= BMI088_ACCEL_RANGE_24_G;
lsb_per_g = 1365.33;
} else {
return -EINVAL;
}
_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
modify_reg(BMI088_ACC_RANGE, clearbits, setbits);
return OK;
}
void
BMI088_accel::start()
{
// Reset the accelerometer
reset();
/* start polling at the specified rate */
ScheduleOnInterval((1_s / BMI088_ACCEL_DEFAULT_RATE) / 2, 1000);
}
void
BMI088_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(BMI088_ACC_SOFTRESET, BMI088_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) % BMI088_ACCEL_NUM_CHECKED_REGISTERS;
}
void
BMI088_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;
int16_t temp;
} report;
/* start measuring */
perf_begin(_sample_perf);
// Checking the status of new data
uint8_t status;
status = read_reg(BMI088_ACC_STATUS);
if (!(status & BMI088_ACC_STATUS_DRDY)) {
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
return;
}
_got_duplicate = false;
/*
* Fetch the full set of measurements from the BMI088 in one pass.
*/
uint8_t index = 0;
uint8_t accel_data[8]; // Need an extra byte for the command, and an an extra dummy byte for the read (see section "SPI interface of accelerometer part" of the BMI088 datasheet)
accel_data[index] = BMI088_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 = 2; // Skip the dummy byte at index=1
uint16_t lsb, msb, msblsb;
lsb = (uint16_t)accel_data[index++];
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_x = (int16_t)msblsb; /* Data in X axis */
lsb = (uint16_t)accel_data[index++];
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_y = (int16_t)msblsb; /* Data in Y axis */
lsb = (uint16_t)accel_data[index++];
msb = (uint16_t)accel_data[index++];
msblsb = (msb << 8) | lsb;
report.accel_z = (int16_t)msblsb; /* Data in Z axis */
// Extract the temperature data
// Note: the temp sensor data is only updated every 1.28s (see "Register 0x22-0x23 Temperature Sensor Data" section in BMI088 Datasheet)
index = 0;
accel_data[index] = BMI088_ACC_TEMP_H | DIR_READ;
// Need to perform a dummy read, hence the num bytes to read is 3 (plus 1 send byte)
if (OK != transfer(accel_data, accel_data, 4)) {
return;
}
index = 2;
msb = (uint16_t)accel_data[index++];
lsb = (uint16_t)accel_data[index++];
uint16_t temp = msb * 8 + lsb / 32;
if (temp > 1023) {
report.temp = temp - 2048;
} else {
report.temp = temp;
}
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_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 bmi088 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;
}
// don't publish duplicated reads
if ((report.accel_x == _accel_prev[0]) && (report.accel_y == _accel_prev[1]) && (report.accel_z == _accel_prev[2])) {
perf_count(_duplicates);
perf_end(_sample_perf);
return;
} else {
_accel_prev[0] = report.accel_x;
_accel_prev[1] = report.accel_y;
_accel_prev[2] = report.accel_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_accel.set_error_count(error_count);
// Convert the bit-wise representation of temperature to degrees C
_accel_last_temperature_copy = (report.temp * 0.125f) + 23.0f;
_px4_accel.set_temperature(_accel_last_temperature_copy);
/*
* 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
BMI088_accel::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("Accel");
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
_px4_accel.print_status();
}
void
BMI088_accel::print_registers()
{
uint8_t index = 0;
printf("BMI088 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 Conf: %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 Int1 Conf: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Int1-Int2_Map-Data: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Pwr Conf: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
reg = _checked_registers[index++];
v = read_reg(reg);
printf("Accel Pwr Ctrl: %02x:%02x ", (unsigned)reg, (unsigned)v);
printf("\n");
printf("\n");
}
src/drivers/imu/bmi088/BMI088_accel.hpp
-256
View File
@@ -1,256 +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 "BMI088.hpp"
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/conversion/rotation.h>
#define BMI088_DEVICE_PATH_ACCEL "/dev/bmi088_accel"
#define BMI088_DEVICE_PATH_ACCEL_EXT "/dev/bmi088_accel_ext"
// BMI088 Accel registers
#define BMI088_ACC_CHIP_ID 0x00
#define BMI088_ACC_ERR_REG 0x02
#define BMI088_ACC_STATUS 0x03
#define BMI088_ACC_X_L 0x12
#define BMI088_ACC_X_H 0x13
#define BMI088_ACC_Y_L 0x14
#define BMI088_ACC_Y_H 0x15
#define BMI088_ACC_Z_L 0x16
#define BMI088_ACC_Z_H 0x17
#define BMI088_ACC_SENSORTIME_0 0x18
#define BMI088_ACC_SENSORTIME_1 0x19
#define BMI088_ACC_SENSORTIME_2 0x1A
#define BMI088_ACC_INT_STAT_1 0x1D
#define BMI088_ACC_TEMP_H 0x22
#define BMI088_ACC_TEMP_L 0x23
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_RANGE 0x41
#define BMI088_ACC_INT1_IO_CONF 0x53
#define BMI088_ACC_INT2_IO_CONF 0x54
#define BMI088_ACC_INT1_INT2_MAP_DATA 0x58
#define BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_SOFTRESET 0x7E
// BMI088 Accelerometer Chip-Id
#define BMI088_ACC_WHO_AM_I 0x1E
// BMI088_ACC_ERR_REG 0x02
#define BMI088_ACC_ERR_REG_NO_ERROR (0x00<<2)
#define BMI088_ACC_ERR_REG_ERROR (0x01<<2)
#define BMI088_ACC_ERR_REG_FATAL_ERROR (0x01<<0)
// BMI088_ACC_STATUS 0x03
#define BMI088_ACC_STATUS_DRDY (0x01<<7)
// BMI088_ACC_INT_STAT_1 0x01D
#define BMI088_ACC_INT_STAT_1_DRDY (0x01<<7)
// BMI088_ACC_CONF 0x40
#define BMI088_ACC_CONF_BWP_4 (0x08<<4)
#define BMI088_ACC_CONF_BWP_2 (0x09<<4)
#define BMI088_ACC_CONF_BWP_NORMAL (0x0A<<4)
#define BMI088_ACC_CONF_ODR_12_5 (0x05<<0)
#define BMI088_ACC_CONF_ODR_25 (0x06<<0)
#define BMI088_ACC_CONF_ODR_50 (0x07<<0)
#define BMI088_ACC_CONF_ODR_100 (0x08<<0)
#define BMI088_ACC_CONF_ODR_200 (0x09<<0)
#define BMI088_ACC_CONF_ODR_400 (0x0A<<0)
#define BMI088_ACC_CONF_ODR_800 (0x0B<<0)
#define BMI088_ACC_CONF_ODR_1600 (0x0C<<0)
// BMI088_ACC_RANGE 0x41
#define BMI088_ACCEL_RANGE_3_G (0x00<<0)
#define BMI088_ACCEL_RANGE_6_G (0x01<<0)
#define BMI088_ACCEL_RANGE_12_G (0x02<<0)
#define BMI088_ACCEL_RANGE_24_G (0x03<<0)
// BMI088_ACC_INT1_IO_CONF 0x53
#define BMI088_ACC_INT1_IO_CONF_INT1_IN (0x01<<4)
#define BMI088_ACC_INT1_IO_CONF_INT1_OUT (0x01<<3)
#define BMI088_ACC_INT1_IO_CONF_PP (0x00<<2)
#define BMI088_ACC_INT1_IO_CONF_OD (0x01<<2)
#define BMI088_ACC_INT1_IO_CONF_ACTIVE_LOW (0x00<<1)
#define BMI088_ACC_INT1_IO_CONF_ACTIVE_HIGH (0x01<<1)
// BMI088_ACC_INT2_IO_CONF 0x54
#define BMI088_ACC_INT2_IO_CONF_INT1_IN (0x01<<4)
#define BMI088_ACC_INT2_IO_CONF_INT1_OUT (0x01<<3)
#define BMI088_ACC_INT2_IO_CONF_PP (0x00<<2)
#define BMI088_ACC_INT2_IO_CONF_OD (0x01<<2)
#define BMI088_ACC_INT2_IO_CONF_ACTIVE_LOW (0x00<<1)
#define BMI088_ACC_INT2_IO_CONF_ACTIVE_HIGH (0x01<<1)
// BMI088_ACC_INT1_INT2_MAP_DATA 0x54
#define BMI088_ACC_INT1_INT2_MAP_DATA_INT2_DRDY (0x01<<6)
#define BMI088_ACC_INT1_INT2_MAP_DATA_INT1_DRDY (0x01<<2)
// BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_SELF_TEST_OFF (0x00<<0)
#define BMI088_ACC_SELF_TEST_POSITIVE (0x0D<<0)
#define BMI088_ACC_SELF_TEST_NEGATIVE (0x09<<0)
// BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_CONF_SUSPEND (0x03<<0)
#define BMI088_ACC_PWR_CONF_ACTIVE (0x00<<0)
// BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_PWR_CTRL_EN (0x04<<0)
///////
// To Do check these defaults and maks below
///
// Default and Max values
#define BMI088_ACCEL_DEFAULT_RANGE_G 24
#define BMI088_ACCEL_DEFAULT_RATE 800
#define BMI088_ACCEL_MAX_RATE 800
#define BMI088_ACCEL_MAX_PUBLISH_RATE 800
#define BMI088_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
class BMI088_accel : public BMI088
{
public:
BMI088_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 ~BMI088_accel();
int init() override;
// Start automatic measurement.
void start();
// We need to override the read_reg function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
uint8_t read_reg(unsigned reg) override;
// We need to override the read_reg16 function from the BMI088 base class, because the accelerometer requires a dummy byte read before each read operation
uint16_t read_reg16(unsigned reg) override;
void print_status() override;
void print_registers();
// deliberately cause a sensor error
void test_error();
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;
int16_t _accel_prev[3] {};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI088_ACCEL_NUM_CHECKED_REGISTERS 7
static const uint8_t _checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI088_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI088_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 BMI088_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 BMI088_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 BMI088_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);
/**
* Set accel sample rate
*/
int accel_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 */
BMI088_accel(const BMI088_accel &);
BMI088_accel operator=(const BMI088_accel &);
};
src/drivers/imu/bmi088/BMI088_gyro.cpp
-469
View File
@@ -1,469 +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 "BMI088_gyro.hpp"
#include "BMI088_accel.hpp"
using namespace time_literals;
/*
* Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver.
* This is a ugly HACK! The driver should potentially be rewritten with the gyro as subdriver.
*/
__EXPORT float _accel_last_temperature_copy = 0;
/*
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 BMI088_gyro::_checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS] = { BMI088_GYR_CHIP_ID,
BMI088_GYR_LPM1,
BMI088_GYR_BW,
BMI088_GYR_RANGE,
BMI088_GYR_INT_EN_0,
BMI088_GYR_INT_EN_1,
BMI088_GYR_INT_MAP_1
};
BMI088_gyro::BMI088_gyro(I2CSPIBusOption bus_option, int bus, const char *path_gyro, uint32_t device,
enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) :
BMI088("bmi088_gyro", path_gyro, bus_option, bus, DRV_GYR_DEVTYPE_BMI088, 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, "bmi088_gyro_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi088_gyro_duplicates"))
{
}
BMI088_gyro::~BMI088_gyro()
{
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
}
int
BMI088_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 BMI088_gyro::reset()
{
write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);//Soft-reset
usleep(5000);
write_checked_reg(BMI088_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate())
write_checked_reg(BMI088_GYR_RANGE, 0);// Write Gyro range
write_checked_reg(BMI088_GYR_INT_EN_0, BMI088_GYR_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI088_GYR_INT_MAP_1, BMI088_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_gyro_range(BMI088_GYRO_DEFAULT_RANGE_DPS);// set Gyro range
gyro_set_sample_rate(BMI088_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth
//Enable Gyroscope in normal mode
write_reg(BMI088_GYR_LPM1, BMI088_GYRO_NORMAL);
px4_usleep(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI088_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
BMI088_gyro::probe()
{
/* look for device ID */
_whoami = read_reg(BMI088_GYR_CHIP_ID);
// verify product revision
switch (_whoami) {
case BMI088_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;
}
printf("unexpected whoami 0x%02x\n", _whoami);
DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
return -EIO;
}
int
BMI088_gyro::gyro_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI088_GYRO_BW_MASK;
if (frequency <= 100) {
setbits |= BMI088_GYRO_RATE_100; /* 32 Hz cutoff */
//_gyro_sample_rate = 100;
} else if (frequency <= 250) {
setbits |= BMI088_GYRO_RATE_400; /* 47 Hz cutoff */
//_gyro_sample_rate = 400;
} else if (frequency <= 1000) {
setbits |= BMI088_GYRO_RATE_1000; /* 116 Hz cutoff */
//_gyro_sample_rate = 1000;
} else if (frequency > 1000) {
setbits |= BMI088_GYRO_RATE_2000; /* 230 Hz cutoff */
//_gyro_sample_rate = 2000;
} else {
return -EINVAL;
}
modify_reg(BMI088_GYR_BW, clearbits, setbits);
return OK;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
void
BMI088_gyro::test_error()
{
write_reg(BMI088_GYR_SOFTRESET, BMI088_SOFT_RESET);
::printf("error triggered\n");
print_registers();
}
void
BMI088_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
BMI088_gyro::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
}
}
}
int
BMI088_gyro::set_gyro_range(unsigned max_dps)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI088_GYRO_RANGE_125_DPS | BMI088_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 |= BMI088_GYRO_RANGE_125_DPS;
} else if (max_dps <= 250) {
//max_gyro_dps = 250;
lsb_per_dps = 131.2;
setbits |= BMI088_GYRO_RANGE_250_DPS;
} else if (max_dps <= 500) {
//max_gyro_dps = 500;
lsb_per_dps = 65.6;
setbits |= BMI088_GYRO_RANGE_500_DPS;
} else if (max_dps <= 1000) {
//max_gyro_dps = 1000;
lsb_per_dps = 32.8;
setbits |= BMI088_GYRO_RANGE_1000_DPS;
} else if (max_dps <= 2000) {
//max_gyro_dps = 2000;
lsb_per_dps = 16.4;
setbits |= BMI088_GYRO_RANGE_2000_DPS;
} else {
return -EINVAL;
}
_px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
modify_reg(BMI088_GYR_RANGE, clearbits, setbits);
return OK;
}
void
BMI088_gyro::start()
{
/* start polling at the specified rate */
ScheduleOnInterval((1_s / BMI088_GYRO_DEFAULT_RATE) / 2, 1000);
}
void
BMI088_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(BMI088_GYR_SOFTRESET, BMI088_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) % BMI088_GYRO_NUM_CHECKED_REGISTERS;
}
void
BMI088_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 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 BMI088 gyro in one pass.
*/
bmi_gyroreport.cmd = BMI088_GYR_CHIP_ID | 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();
// Get the last temperature from the accelerometer (the Gyro does not have its own temperature measurement)
_last_temperature = _accel_last_temperature_copy;
report.gyro_x = bmi_gyroreport.gyro_x;
report.gyro_y = bmi_gyroreport.gyro_y;
report.gyro_z = bmi_gyroreport.gyro_z;
if ((bmi_gyroreport.chip_id) != BMI088_GYR_WHO_AM_I) {
// If chip id is not right, assume a bus transfer 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 bmi088 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_count(_duplicates);
perf_end(_sample_perf);
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);
// Get the temperature from the accelerometer part of the BMI088, because the gyro part does not have a temperature register
_px4_gyro.set_temperature(_accel_last_temperature_copy);
/*
* 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
BMI088_gyro::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("Gyro");
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);
_px4_gyro.print_status();
}
void
BMI088_gyro::print_registers()
{
uint8_t index = 0;
printf("BMI088 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");
}
src/drivers/imu/bmi088/BMI088_gyro.hpp
-252
View File
@@ -1,252 +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 "BMI088.hpp"
#define BMI088_DEVICE_PATH_GYRO "/dev/bmi088_gyro"
#define BMI088_DEVICE_PATH_GYRO_EXT "/dev/bmi088_gyro_ext"
// BMI088 Gyro registers
#define BMI088_GYR_CHIP_ID 0x00
#define BMI088_GYR_X_L 0x02
#define BMI088_GYR_X_H 0x03
#define BMI088_GYR_Y_L 0x04
#define BMI088_GYR_Y_H 0x05
#define BMI088_GYR_Z_L 0x06
#define BMI088_GYR_Z_H 0x07
#define BMI088_GYR_INT_STATUS_0 0x09
#define BMI088_GYR_INT_STATUS_1 0x0A
#define BMI088_GYR_INT_STATUS_2 0x0B
#define BMI088_GYR_INT_STATUS_3 0x0C
#define BMI088_GYR_FIFO_STATUS 0x0E
#define BMI088_GYR_RANGE 0x0F
#define BMI088_GYR_BW 0x10
#define BMI088_GYR_LPM1 0x11
#define BMI088_GYR_LPM2 0x12
#define BMI088_GYR_RATE_HBW 0x13
#define BMI088_GYR_SOFTRESET 0x14
#define BMI088_GYR_INT_EN_0 0x15
#define BMI088_GYR_INT_EN_1 0x16
#define BMI088_GYR_INT_MAP_0 0x17
#define BMI088_GYR_INT_MAP_1 0x18
#define BMI088_GYR_INT_MAP_2 0x19
#define BMI088_GYRO_0_REG 0x1A
#define BMI088_GYRO_1_REG 0x1B
#define BMI088_GYRO_2_REG 0x1C
#define BMI088_GYRO_3_REG 0x1E
#define BMI088_GYR_INT_LATCH 0x21
#define BMI088_GYR_INT_LH_0 0x22
#define BMI088_GYR_INT_LH_1 0x23
#define BMI088_GYR_INT_LH_2 0x24
#define BMI088_GYR_INT_LH_3 0x25
#define BMI088_GYR_INT_LH_4 0x26
#define BMI088_GYR_INT_LH_5 0x27
#define BMI088_GYR_SOC 0x31
#define BMI088_GYR_A_FOC 0x32
#define BMI088_GYR_TRIM_NVM_CTRL 0x33
#define BMI088_BGW_SPI3_WDT 0x34
#define BMI088_GYR_OFFSET_COMP 0x36
#define BMI088_GYR_OFFSET_COMP_X 0x37
#define BMI088_GYR_OFFSET_COMP_Y 0x38
#define BMI088_GYR_OFFSET_COMP_Z 0x39
#define BMI088_GYR_TRIM_GPO 0x3A
#define BMI088_GYR_TRIM_GP1 0x3B
#define BMI088_GYR_SELF_TEST 0x3C
#define BMI088_GYR_FIFO_CONFIG_0 0x3D
#define BMI088_GYR_FIFO_CONFIG_1 0x3E
#define BMI088_GYR_FIFO_DATA 0x3F
// BMI088 Gyroscope Chip-Id
#define BMI088_GYR_WHO_AM_I 0x0F
//ODR & DLPF filter bandwidth settings (they are coupled)
#define BMI088_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI088_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI088_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI088_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI088_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0)
//BMI088_GYR_LPM1 0x11
#define BMI088_GYRO_NORMAL (0<<7) | (0<<5)
#define BMI088_GYRO_DEEP_SUSPEND (0<<7) | (1<<5)
#define BMI088_GYRO_SUSPEND (1<<7) | (0<<5)
//BMI088_GYR_RANGE 0x0F
#define BMI088_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
#define BMI088_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
#define BMI088_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0)
#define BMI088_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0)
#define BMI088_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0)
//BMI088_GYR_INT_EN_0 0x15
#define BMI088_GYR_DRDY_INT_EN (1<<7)
//BMI088_GYR_INT_MAP_1 0x18
#define BMI088_GYR_DRDY_INT1 (1<<0)
// Default and Max values
#define BMI088_GYRO_DEFAULT_RANGE_DPS 2000
#define BMI088_GYRO_DEFAULT_RATE 1000
#define BMI088_GYRO_MAX_RATE 1000
#define BMI088_GYRO_MAX_PUBLISH_RATE 280
#define BMI088_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50
/* Mask definitions for Gyro bandwidth */
#define BMI088_GYRO_BW_MASK 0x0F
class BMI088_gyro : public BMI088
{
public:
BMI088_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 ~BMI088_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;
int16_t _gyro_prev[3] {};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI088_GYRO_NUM_CHECKED_REGISTERS 7
static const uint8_t _checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI088_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI088_GYRO_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Modify a register in the BMI088_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 BMI088_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 BMI088_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 */
BMI088_gyro(const BMI088_gyro &);
BMI088_gyro operator=(const BMI088_gyro &);
#pragma pack(push, 1)
/**
* Report conversation within the BMI088_gyro, including command byte and
* interrupt status.
*/
struct BMI_GyroReport {
uint8_t cmd;
uint8_t chip_id; // Read to check if bus is workig
uint8_t ununsed;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
};
#pragma pack(pop)
};
src/drivers/imu/bosch/bmi088/BMI088.cpp
+92
View File
@@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI088.hpp"
#include "BMI088_Accelerometer.hpp"
#include "BMI088_Gyroscope.hpp"
I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
BMI088 *instance = nullptr;
if (cli.type == DRV_ACC_DEVTYPE_BMI088) {
instance = new Bosch::BMI088::Accelerometer::BMI088_Accelerometer(iterator.configuredBusOption(), iterator.bus(),
iterator.devid(), cli.rotation, cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
} else if (cli.type == DRV_GYR_DEVTYPE_BMI088) {
instance = new Bosch::BMI088::Gyroscope::BMI088_Gyroscope(iterator.configuredBusOption(), iterator.bus(),
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;
}
BMI088::BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device,
enum spi_mode_e mode, uint32_t frequency, spi_drdy_gpio_t drdy_gpio) :
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 BMI088::init()
{
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
return Reset() ? 0 : -1;
}
bool BMI088::Reset()
{
_state = STATE::RESET;
ScheduleClear();
ScheduleNow();
return true;
}
src/drivers/imu/bmi088/BMI088.hpp → src/drivers/imu/bosch/bmi088/BMI088.hpp
+28 -40
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,66 +33,54 @@
#pragma once
#include <drivers/device/spi.h>
#include <ecl/geo/geo.h>
#include <lib/conversion/rotation.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>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
//Soft-reset command Value
#define BMI088_SOFT_RESET 0xB6
#define BMI088_BUS_SPEED 10*1000*1000
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
class BMI088 : public device::SPI, public I2CSPIDriver<BMI088>
{
public:
BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, uint8_t type, uint32_t device,
enum spi_mode_e mode, uint32_t frequency, enum Rotation rotation);
BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode,
uint32_t frequency, spi_drdy_gpio_t drdy_gpio);
virtual ~BMI088() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
virtual void 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;
void custom_method(const BusCLIArguments &cli) override;
bool Reset();
uint8_t _whoami; ///< whoami result
const spi_drdy_gpio_t _drdy_gpio;
uint8_t _register_wait;
uint64_t _reset_wait;
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};
enum Rotation _rotation;
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
bool _data_ready_interrupt_enabled{false};
uint8_t _checked_next;
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
};
/**
* Read a register from the BMI088
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg) override;
virtual uint16_t read_reg16(unsigned reg);
STATE _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
/**
* Write a register in the BMI088
*
* @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;
};
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
+601
View File
@@ -0,0 +1,601 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI088_Accelerometer.hpp"
#include <ecl/geo/geo.h> // CONSTANTS_ONE_G
using namespace time_literals;
namespace Bosch::BMI088::Accelerometer
{
BMI088_Accelerometer::BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
BMI088(DRV_ACC_DEVTYPE_BMI088, "BMI088_Accelerometer", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
_px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_accel: DRDY interval");
}
ConfigureSampleRate();
}
BMI088_Accelerometer::~BMI088_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 BMI088_Accelerometer::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void BMI088_Accelerometer::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.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 BMI088_Accelerometer::probe()
{
/* 6.1 Serial Peripheral Interface (SPI)
* ... the accelerometer part starts always in I2C mode
* (regardless of the level of the PS pin) and needs to be changed to SPI
* mode actively by sending a rising edge on the CSB1 pin
* (chip select of the accelerometer), on which the accelerometer part
* switches to SPI mode and stays in this mode until the next power-up-reset.
*
* To change the sensor to SPI mode in the initialization phase, the user
* could perfom a dummy SPI read operation, e.g. of register ACC_CHIP_ID
* (the obtained value will be invalid).In case of read operations,
*/
RegisterRead(Register::ACC_CHIP_ID);
const uint8_t ACC_CHIP_ID = RegisterRead(Register::ACC_CHIP_ID);
if (ACC_CHIP_ID != ID) {
DEVICE_DEBUG("unexpected ACC_CHIP_ID 0x%02x", ACC_CHIP_ID);
return PX4_ERROR;
}
return PX4_OK;
}
void BMI088_Accelerometer::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// ACC_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor
RegisterWrite(Register::ACC_SOFTRESET, 0xB6);
_reset_timestamp = now;
_consecutive_failures = 0;
_total_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(1_ms); // Following a delay of 1 ms, all configuration settings are overwritten with their reset value.
break;
case STATE::WAIT_FOR_RESET:
if (RegisterRead(Register::ACC_CHIP_ID) == ID) {
// ACC_PWR_CONF: Power on sensor
RegisterWrite(Register::ACC_PWR_CONF, 0);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleDelayed(10_ms);
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 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: {
uint8_t samples = 0;
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) {
samples = _fifo_accel_samples;
perf_count_interval(_drdy_interval_perf, now);
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
bool success = false;
if (!_data_ready_interrupt_enabled || (samples == 0)) {
// manually check FIFO count if no samples from DRDY
const uint16_t fifo_byte_counter = FIFOReadCount();
samples = fifo_byte_counter / sizeof(FIFO::DATA);
}
if (samples > 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 (samples == 0) {
perf_count(_fifo_empty_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
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();
}
} else {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = now;
}
}
}
break;
}
}
void BMI088_Accelerometer::ConfigureAccel()
{
const uint8_t ACC_RANGE = RegisterRead(Register::ACC_RANGE) & (Bit1 | Bit0);
switch (ACC_RANGE) {
case acc_range_3g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(3.f);
break;
case acc_range_6g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(6.f);
break;
case acc_range_12g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(12.f);
break;
case acc_range_24g:
_px4_accel.set_scale(CONSTANTS_ONE_G * (powf(2, ACC_RANGE + 1) * 1.5f) / 32768.f);
_px4_accel.set_range(24.f);
break;
}
}
void BMI088_Accelerometer::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 800; // default to 800 Hz
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_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 BMI088_Accelerometer::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO_WTM: 13 bit FIFO watermark level value
// unit of the fifo watermark is one byte
const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_WTM_0) {
// fifo_water_mark[7:0]
r.set_bits = fifo_watermark_threshold & 0x00FF;
r.clear_bits = ~r.set_bits;
} else if (r.reg == Register::FIFO_WTM_1) {
// fifo_water_mark[12:8]
r.set_bits = (fifo_watermark_threshold & 0x0700) >> 8;
r.clear_bits = ~r.set_bits;
}
}
}
bool BMI088_Accelerometer::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
ConfigureAccel();
return success;
}
int BMI088_Accelerometer::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<BMI088_Accelerometer *>(arg)->DataReady();
return 0;
}
void BMI088_Accelerometer::DataReady()
{
uint8_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_accel_samples)) {
ScheduleNow();
}
}
bool BMI088_Accelerometer::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool BMI088_Accelerometer::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool BMI088_Accelerometer::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
return success;
}
uint8_t BMI088_Accelerometer::RegisterRead(Register reg)
{
// 6.1.2 SPI interface of accelerometer part
//
// In case of read operations of the accelerometer part, the requested data
// is not sent immediately, but instead first a dummy byte is sent, and
// after this dummy byte the actual requested register content is transmitted.
uint8_t cmd[3] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
// cmd[1] dummy byte
transfer(cmd, cmd, sizeof(cmd));
return cmd[2];
}
void BMI088_Accelerometer::RegisterWrite(Register reg, uint8_t value)
{
uint8_t cmd[2] {(uint8_t)reg, value};
transfer(cmd, cmd, sizeof(cmd));
}
void BMI088_Accelerometer::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
}
uint16_t BMI088_Accelerometer::FIFOReadCount()
{
// FIFO length registers FIFO_LENGTH_1 and FIFO_LENGTH_0 contain the 14 bit FIFO byte
uint8_t fifo_len_buf[4] {};
fifo_len_buf[0] = static_cast<uint8_t>(Register::FIFO_LENGTH_0) | DIR_READ;
// fifo_len_buf[1] dummy byte
if (transfer(&fifo_len_buf[0], &fifo_len_buf[0], sizeof(fifo_len_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
return 0;
}
const uint8_t FIFO_LENGTH_0 = fifo_len_buf[2]; // fifo_byte_counter[7:0]
const uint8_t FIFO_LENGTH_1 = fifo_len_buf[3] & 0x3F; // fifo_byte_counter[13:8]
const uint16_t fifo_byte_counter = combine(FIFO_LENGTH_1, FIFO_LENGTH_0);
// An empty FIFO corresponds to 0x8000
if (fifo_byte_counter == 0x8000) {
return 0;
}
return fifo_byte_counter / sizeof(FIFO::DATA);
}
bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
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 = 0;
accel.dt = FIFO_SAMPLE_DT;
const size_t fifo_byte_counter = combine(buffer.FIFO_LENGTH_1 & 0x3F, buffer.FIFO_LENGTH_0);
// An empty FIFO corresponds to 0x8000
if (fifo_byte_counter == 0x8000) {
perf_count(_fifo_empty_perf);
return false;
} else if (fifo_byte_counter >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
return false;
}
// first find all sensor data frames in the buffer
uint8_t *data_buffer = (uint8_t *)&buffer.f[0];
unsigned fifo_buffer_index = 0; // start of buffer
while (fifo_buffer_index < math::min(fifo_byte_counter, transfer_size - 4)) {
// look for header signature (first 6 bits) followed by two bits indicating the status of INT1 and INT2
switch (data_buffer[fifo_buffer_index] & 0xFC) {
case FIFO::header::sensor_data_frame: {
// Acceleration sensor data frame
// Frame length: 7 bytes (1 byte header + 6 bytes payload)
FIFO::DATA *fifo_sample = (FIFO::DATA *)&data_buffer[fifo_buffer_index];
const int16_t accel_x = combine(fifo_sample->ACC_X_MSB, fifo_sample->ACC_X_LSB);
const int16_t accel_y = combine(fifo_sample->ACC_Y_MSB, fifo_sample->ACC_Y_LSB);
const int16_t accel_z = combine(fifo_sample->ACC_Z_MSB, fifo_sample->ACC_Z_LSB);
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel.samples] = accel_x;
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel.samples++;
fifo_buffer_index += 7; // move forward to next record
}
break;
case FIFO::header::skip_frame:
// Skip Frame
// Frame length: 2 bytes (1 byte header + 1 byte payload)
PX4_DEBUG("Skip Frame");
fifo_buffer_index += 2;
break;
case FIFO::header::sensor_time_frame:
// Sensortime Frame
// Frame length: 4 bytes (1 byte header + 3 bytes payload)
PX4_DEBUG("Sensortime Frame");
fifo_buffer_index += 4;
break;
case FIFO::header::FIFO_input_config_frame:
// FIFO input config Frame
// Frame length: 2 bytes (1 byte header + 1 byte payload)
PX4_DEBUG("FIFO input config Frame");
fifo_buffer_index += 2;
break;
case FIFO::header::sample_drop_frame:
// Sample drop Frame
// Frame length: 2 bytes (1 byte header + 1 byte payload)
PX4_DEBUG("Sample drop Frame");
fifo_buffer_index += 2;
break;
default:
fifo_buffer_index++;
break;
}
}
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
return true;
}
return false;
}
void BMI088_Accelerometer::FIFOReset()
{
perf_count(_fifo_reset_perf);
// ACC_SOFTRESET: trigger a FIFO reset by writing 0xB0 to ACC_SOFTRESET (register 0x7E).
RegisterWrite(Register::ACC_SOFTRESET, 0xB0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
}
void BMI088_Accelerometer::UpdateTemperature()
{
// stored in an 11-bit value in 2’s complement format
uint8_t temperature_buf[4] {};
temperature_buf[0] = static_cast<uint8_t>(Register::TEMP_MSB) | DIR_READ;
// temperature_buf[1] dummy byte
if (transfer(&temperature_buf[0], &temperature_buf[0], sizeof(temperature_buf)) != PX4_OK) {
return;
}
const uint8_t TEMP_MSB = temperature_buf[2];
const uint8_t TEMP_LSB = temperature_buf[3];
// Datasheet 5.3.7: Register 0x22 – 0x23: Temperature sensor data
uint16_t Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32);
int16_t Temp_int11 = 0;
if (Temp_uint11 > 1023) {
Temp_int11 = Temp_uint11 - 2048;
} else {
Temp_int11 = Temp_uint11;
}
float temperature = (Temp_int11 * 0.125f) + 23.f; // Temp_int11 * 0.125°C/LSB + 23°C
if (PX4_ISFINITE(temperature)) {
_px4_accel.set_temperature(temperature);
}
}
} // namespace Bosch::BMI088::Accelerometer
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.hpp
+134
View File
@@ -0,0 +1,134 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "BMI088.hpp"
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include "Bosch_BMI088_Accelerometer_Registers.hpp"
namespace Bosch::BMI088::Accelerometer
{
class BMI088_Accelerometer : public BMI088
{
public:
BMI088_Accelerometer(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
~BMI088_Accelerometer() override;
void RunImpl() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr uint32_t ACCEL_RATE{1600}; // 1600 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_LENGTH_0) | DIR_READ};
uint8_t dummy{0};
uint8_t FIFO_LENGTH_0{0};
uint8_t FIFO_LENGTH_1{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
struct register_config_t {
Register reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override;
bool Configure();
void ConfigureAccel();
void ConfigureSampleRate(int sample_rate = 0);
void ConfigureFIFOWatermark(uint8_t samples);
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
bool RegisterCheck(const register_config_t &reg_cfg);
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &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{10};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::ACC_PWR_CONF, 0, ACC_PWR_CONF_BIT::acc_pwr_save },
{ Register::ACC_PWR_CTRL, ACC_PWR_CTRL_BIT::acc_enable, 0 },
{ Register::ACC_CONF, ACC_CONF_BIT::acc_bwp_Normal | ACC_CONF_BIT::acc_odr_1600, Bit1 | Bit0 },
{ Register::ACC_RANGE, ACC_RANGE_BIT::acc_range_24g, 0 },
{ Register::FIFO_WTM_0, 0, 0 },
{ Register::FIFO_WTM_1, 0, 0 },
{ Register::FIFO_CONFIG_0, FIFO_CONFIG_0_BIT::BIT1_ALWAYS | FIFO_CONFIG_0_BIT::FIFO_mode, 0 },
{ Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::BIT4_ALWAYS | FIFO_CONFIG_1_BIT::Acc_en, 0 },
{ Register::INT1_IO_CONF, INT1_IO_CONF_BIT::int1_out, 0 },
{ Register::INT1_INT2_MAP_DATA, INT1_INT2_MAP_DATA_BIT::int1_fwm, 0},
};
};
} // namespace Bosch::BMI088::Accelerometer
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
+462
View File
@@ -0,0 +1,462 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI088_Gyroscope.hpp"
#include <px4_platform/board_dma_alloc.h>
using namespace time_literals;
namespace Bosch::BMI088::Gyroscope
{
BMI088_Gyroscope::BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
int bus_frequency, spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) :
BMI088(DRV_GYR_DEVTYPE_BMI088, "BMI088_Gyroscope", bus_option, bus, device, spi_mode, bus_frequency, drdy_gpio),
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME"_gyro: DRDY interval");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
}
BMI088_Gyroscope::~BMI088_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 BMI088_Gyroscope::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void BMI088_Gyroscope::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.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 BMI088_Gyroscope::probe()
{
const uint8_t chipid = RegisterRead(Register::GYRO_CHIP_ID);
if (chipid != ID) {
DEVICE_DEBUG("unexpected GYRO_CHIP_ID 0x%02x", chipid);
return PX4_ERROR;
}
return PX4_OK;
}
void BMI088_Gyroscope::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// GYRO_SOFTRESET: Writing a value of 0xB6 to this register resets the sensor.
// Following a delay of 30 ms, all configuration settings are overwritten with their reset value.
RegisterWrite(Register::GYRO_SOFTRESET, 0xB6);
_reset_timestamp = now;
_consecutive_failures = 0;
_total_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(30_ms);
break;
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::GYRO_CHIP_ID) == ID)) {
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleDelayed(1_ms);
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 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 BMI088_Gyroscope::ConfigureGyro()
{
const uint8_t GYRO_RANGE = RegisterRead(Register::GYRO_RANGE) & (Bit3 | Bit2 | Bit1 | Bit0);
switch (GYRO_RANGE) {
case gyro_range_2000_dps:
_px4_gyro.set_scale(math::radians(1.f / 16.384f));
_px4_gyro.set_range(math::radians(2000.f));
break;
case gyro_range_1000_dps:
_px4_gyro.set_scale(math::radians(1.f / 32.768f));
_px4_gyro.set_range(math::radians(1000.f));
break;
case gyro_range_500_dps:
_px4_gyro.set_scale(math::radians(1.f / 65.536f));
_px4_gyro.set_range(math::radians(500.f));
break;
case gyro_range_250_dps:
_px4_gyro.set_scale(math::radians(1.f / 131.072f));
_px4_gyro.set_range(math::radians(250.f));
break;
case gyro_range_125_dps:
_px4_gyro.set_scale(math::radians(1.f / 262.144f));
_px4_gyro.set_range(math::radians(125.f));
break;
}
}
void BMI088_Gyroscope::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 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 BMI088_Gyroscope::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO watermark threshold
for (auto &r : _register_cfg) {
if (r.reg == Register::FIFO_CONFIG_0) {
r.set_bits = samples;
r.clear_bits = ~r.set_bits;
}
}
}
bool BMI088_Gyroscope::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
ConfigureGyro();
return success;
}
int BMI088_Gyroscope::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
static_cast<BMI088_Gyroscope *>(arg)->DataReady();
return 0;
}
void BMI088_Gyroscope::DataReady()
{
uint8_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
}
bool BMI088_Gyroscope::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool BMI088_Gyroscope::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool BMI088_Gyroscope::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
return success;
}
uint8_t BMI088_Gyroscope::RegisterRead(Register reg)
{
uint8_t cmd[2] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
void BMI088_Gyroscope::RegisterWrite(Register reg, uint8_t value)
{
uint8_t cmd[2] {(uint8_t)reg, value};
transfer(cmd, cmd, sizeof(cmd));
}
void BMI088_Gyroscope::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
}
bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
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 BMI088_Gyroscope::FIFOReset()
{
perf_count(_fifo_reset_perf);
// FIFO_CONFIG_0: Writing to water mark level trigger in register 0x3D (FIFO_CONFIG_0) clears the FIFO buffer.
RegisterWrite(Register::FIFO_CONFIG_0, 0);
// FIFO_CONFIG_1: FIFO overrun condition can only be cleared by writing to the FIFO configuration register FIFO_CONFIG_1
RegisterWrite(Register::FIFO_CONFIG_1, 0);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
// FIFO_CONFIG_0: restore FIFO watermark
// FIFO_CONFIG_1: re-enable FIFO
for (const auto &r : _register_cfg) {
if ((r.reg == Register::FIFO_CONFIG_0) || (r.reg == Register::FIFO_CONFIG_1)) {
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
}
}
}
} // namespace Bosch::BMI088::Gyroscope
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.hpp
+126
View File
@@ -0,0 +1,126 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "BMI088.hpp"
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include "Bosch_BMI088_Gyroscope_Registers.hpp"
namespace Bosch::BMI088::Gyroscope
{
class BMI088_Gyroscope : public BMI088
{
public:
BMI088_Gyroscope(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
~BMI088_Gyroscope() override;
void RunImpl() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr uint32_t 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{8};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::GYRO_RANGE, GYRO_RANGE_BIT::gyro_range_2000_dps, 0 },
{ Register::GYRO_BANDWIDTH, 0, GYRO_BANDWIDTH_BIT::gyro_bw_532_Hz },
{ Register::GYRO_INT_CTRL, GYRO_INT_CTRL_BIT::fifo_en, 0 },
{ Register::INT3_INT4_IO_CONF, 0, INT3_INT4_IO_CONF_BIT::Int3_od | INT3_INT4_IO_CONF_BIT::Int3_lvl },
{ Register::INT3_INT4_IO_MAP, INT3_INT4_IO_MAP_BIT::Int3_fifo, 0 },
{ Register::FIFO_WM_ENABLE, FIFO_WM_ENABLE_BIT::fifo_wm_enable, 0 },
{ Register::FIFO_CONFIG_0, 0, 0 }, // fifo_water_mark_level_trigger_retain<6:0>
{ Register::FIFO_CONFIG_1, FIFO_CONFIG_1_BIT::FIFO_MODE, 0 },
};
};
} // namespace Bosch::BMI088::Gyroscope
src/drivers/imu/bosch/bmi088/Bosch_BMI088_Accelerometer_Registers.hpp
+171
View File
@@ -0,0 +1,171 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
namespace Bosch::BMI088::Accelerometer
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t ID = 0x1E;
enum class Register : uint8_t {
ACC_CHIP_ID = 0x00,
TEMP_MSB = 0x22,
TEMP_LSB = 0x23,
FIFO_LENGTH_0 = 0x24,
FIFO_LENGTH_1 = 0x25,
FIFO_DATA = 0x26,
ACC_CONF = 0x40,
ACC_RANGE = 0x41,
FIFO_WTM_0 = 0x46,
FIFO_WTM_1 = 0x47,
FIFO_CONFIG_0 = 0x48,
FIFO_CONFIG_1 = 0x49,
INT1_IO_CONF = 0x53,
INT1_INT2_MAP_DATA = 0x58,
ACC_PWR_CONF = 0x7C,
ACC_PWR_CTRL = 0x7D,
ACC_SOFTRESET = 0x7E,
};
// ACC_CONF
enum ACC_CONF_BIT : uint8_t {
// [7:4] acc_bwp
acc_bwp_Normal = Bit7 | Bit5, // Filter setting normal
// [3:0] acc_odr
acc_odr_1600 = Bit3 | Bit2, // ODR 1600 Hz
};
// ACC_RANGE
enum ACC_RANGE_BIT : uint8_t {
acc_range_3g = 0, // ±3g
acc_range_6g = Bit0, // ±6g
acc_range_12g = Bit1, // ±12g
acc_range_24g = Bit1 | Bit0, // ±24g
};
// FIFO_CONFIG_0
enum FIFO_CONFIG_0_BIT : uint8_t {
BIT1_ALWAYS = Bit1, // This bit must always be ‘1’.
FIFO_mode = Bit0,
};
// FIFO_CONFIG_1
enum FIFO_CONFIG_1_BIT : uint8_t {
Acc_en = Bit6,
BIT4_ALWAYS = Bit4, // This bit must always be ‘1’.
};
// INT1_IO_CONF
enum INT1_IO_CONF_BIT : uint8_t {
int1_in = Bit4,
int1_out = Bit3,
int1_lvl = Bit1,
};
// INT1_INT2_MAP_DATA
enum INT1_INT2_MAP_DATA_BIT : uint8_t {
int2_drdy = Bit6,
int2_fwm = Bit5,
int2_ffull = Bit4,
int1_drdy = Bit2,
int1_fwm = Bit1,
int1_ffull = Bit0,
};
// ACC_PWR_CONF
enum ACC_PWR_CONF_BIT : uint8_t {
acc_pwr_save = 0x03
};
// ACC_PWR_CTRL
enum ACC_PWR_CTRL_BIT : uint8_t {
acc_enable = 0x04
};
namespace FIFO
{
static constexpr size_t SIZE = 1024;
// 1. Acceleration sensor data frame - Frame length: 7 bytes (1 byte header + 6 bytes payload)
// Payload: the next bytes contain the sensor data in the same order as defined in the register map (addresses 0x12 – 0x17).
// 2. Skip Frame - Frame length: 2 bytes (1 byte header + 1 byte payload)
// Payload: one byte containing the number of skipped frames. When more than 0xFF frames have been skipped, 0xFF is returned.
// 3. Sensortime Frame - Frame length: 4 bytes (1 byte header + 3 bytes payload)
// Payload: Sensortime (content of registers 0x18 – 0x1A), taken when the last byte of the last frame is read.
struct DATA {
uint8_t Header;
uint8_t ACC_X_LSB;
uint8_t ACC_X_MSB;
uint8_t ACC_Y_LSB;
uint8_t ACC_Y_MSB;
uint8_t ACC_Z_LSB;
uint8_t ACC_Z_MSB;
};
static_assert(sizeof(DATA) == 7);
enum header : uint8_t {
sensor_data_frame = 0b10000100,
skip_frame = 0b01000000,
sensor_time_frame = 0b01000100,
FIFO_input_config_frame = 0b01001000,
sample_drop_frame = 0b01010000,
};
} // namespace FIFO
} // namespace Bosch::BMI088::Accelerometer
src/drivers/imu/bosch/bmi088/Bosch_BMI088_Gyroscope_Registers.hpp
+141
View File
@@ -0,0 +1,141 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
namespace Bosch::BMI088::Gyroscope
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t ID = 0x0F;
enum class Register : uint8_t {
GYRO_CHIP_ID = 0x00,
FIFO_STATUS = 0x0E,
GYRO_RANGE = 0x0F,
GYRO_BANDWIDTH = 0x10,
GYRO_SOFTRESET = 0x14,
GYRO_INT_CTRL = 0x15,
INT3_INT4_IO_CONF = 0x16,
INT3_INT4_IO_MAP = 0x18,
FIFO_WM_ENABLE = 0x1E,
FIFO_CONFIG_0 = 0x3D,
FIFO_CONFIG_1 = 0x3E,
FIFO_DATA = 0x3F,
};
// FIFO_STATUS
enum FIFO_STATUS_BIT : uint8_t {
Fifo_overrun = Bit7,
Fifo_frame_counter = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0,
};
// GYRO_RANGE
enum GYRO_RANGE_BIT : uint8_t {
gyro_range_2000_dps = 0x00, // ±2000
gyro_range_1000_dps = 0x01, // ±1000
gyro_range_500_dps = 0x02, // ±500
gyro_range_250_dps = 0x04, // ±250
gyro_range_125_dps = 0x05, // ±125
};
// GYRO_BANDWIDTH
enum GYRO_BANDWIDTH_BIT : uint8_t {
gyro_bw_532_Hz = Bit2 | Bit1 | Bit0
};
// GYRO_INT_CTRL
enum GYRO_INT_CTRL_BIT : uint8_t {
data_en = Bit7,
fifo_en = Bit6,
};
// INT3_INT4_IO_CONF
enum INT3_INT4_IO_CONF_BIT : uint8_t {
Int3_od = Bit1, // ‘0’ Push-pull
Int3_lvl = Bit0, // ‘0’ Active low
};
// INT3_INT4_IO_MAP
enum INT3_INT4_IO_MAP_BIT : uint8_t {
Int4_data = Bit7,
Int4_fifo = Bit5,
Int3_fifo = Bit2,
Int3_data = Bit0,
};
// FIFO_WM_ENABLE
enum FIFO_WM_ENABLE_BIT : uint8_t {
fifo_wm_enable = Bit7 | Bit3,
fifo_wm_disable = Bit3,
};
// FIFO_CONFIG_1
enum FIFO_CONFIG_1_BIT : uint8_t {
FIFO_MODE = Bit6,
};
namespace FIFO
{
struct DATA {
uint8_t RATE_X_LSB;
uint8_t RATE_X_MSB;
uint8_t RATE_Y_LSB;
uint8_t RATE_Y_MSB;
uint8_t RATE_Z_LSB;
uint8_t RATE_Z_MSB;
};
static_assert(sizeof(DATA) == 6);
// 100 frames of data in FIFO mode
static constexpr size_t SIZE = sizeof(DATA) * 100;
} // namespace FIFO
} // namespace Bosch::BMI088::Gyroscope
src/drivers/imu/bmi088/CMakeLists.txt → src/drivers/imu/bosch/bmi088/CMakeLists.txt
+17 -5
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -30,13 +30,25 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__bmi88
MODULE drivers__imu__bosch__bmi088
MAIN bmi088
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
BMI088_accel.cpp
BMI088_gyro.cpp
Bosch_BMI088_Accelerometer_Registers.hpp
Bosch_BMI088_Gyroscope_Registers.hpp
BMI088.cpp
BMI088.hpp
BMI088_Accelerometer.cpp
BMI088_Accelerometer.hpp
BMI088_Gyroscope.cpp
BMI088_Gyroscope.hpp
bmi088_main.cpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
px4_work_queue
)
src/drivers/imu/bmi088/bmi088_main.cpp → src/drivers/imu/bosch/bmi088/bmi088_main.cpp
+7 -111
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,15 +31,12 @@
*
****************************************************************************/
#include "BMI088_accel.hpp"
#include "BMI088_gyro.hpp"
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
extern "C" __EXPORT int bmi088_main(int argc, char *argv[]);
#include "BMI088.hpp"
void
BMI088::print_usage()
void BMI088::print_usage()
{
PRINT_MODULE_USAGE_NAME("bmi088", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
@@ -48,107 +45,16 @@ BMI088::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 *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
extern "C" int bmi088_main(int argc, char *argv[])
{
BMI088 *instance = nullptr;
if (cli.type == DRV_ACC_DEVTYPE_BMI088) {
instance = new BMI088_accel(iterator.configuredBusOption(), iterator.bus(), BMI088_DEVICE_PATH_ACCEL, iterator.devid(),
cli.rotation, cli.bus_frequency, cli.spi_mode);
} else if (cli.type == DRV_GYR_DEVTYPE_BMI088) {
instance = new BMI088_gyro(iterator.configuredBusOption(), iterator.bus(), BMI088_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
BMI088::custom_method(const BusCLIArguments &cli)
{
switch (cli.custom1) {
case 0: print_registers();
break;
case 1: test_error();
break;
}
}
BMI088::BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, uint8_t type,
uint32_t device,
enum spi_mode_e mode,
uint32_t frequency, enum Rotation rotation):
SPI(type, MODULE_NAME, bus, device, mode, frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, type),
_whoami(0),
_register_wait(0),
_reset_wait(0),
_rotation(rotation),
_checked_next(0)
{
}
uint8_t
BMI088::read_reg(unsigned reg)
{
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
uint16_t
BMI088::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
BMI088::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
bmi088_main(int argc, char *argv[])
{
using ThisDriver = BMI088;
int ch;
using ThisDriver = BMI088;
BusCLIArguments cli{false, true};
cli.type = 0;
cli.default_spi_frequency = BMI088_BUS_SPEED;
cli.default_spi_frequency = 10000000;
while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) {
switch (ch) {
@@ -187,16 +93,6 @@ bmi088_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;
}
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+1 -1
View File
@@ -97,7 +97,7 @@ private:
float _range{16 * CONSTANTS_ONE_G};
float _scale{1.f};
float _temperature{0.f};
float _temperature{NAN};
float _clip_limit{_range / _scale};
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+1 -1
View File
@@ -95,7 +95,7 @@ private:
float _range{math::radians(2000.f)};
float _scale{1.f};
float _temperature{0.f};
float _temperature{NAN};
uint32_t _error_count{0};