Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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-04 16:16:40 -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/adis16448
imu/adis16477 imu/adis16477
imu/adis16497 imu/adis16497
imu/bmi088 imu/bosch/bmi088
imu/invensense/icm20649 imu/invensense/icm20649
imu/invensense/icm20689 imu/invensense/icm20689
irlock irlock
boards/hex/cube-orange/default.cmake
-1
View File
@@ -33,7 +33,6 @@ px4_add_board(
imu/adis16448 imu/adis16448
imu/adis16477 imu/adis16477
imu/adis16497 imu/adis16497
imu/bmi088
imu/invensense/icm20602 imu/invensense/icm20602
imu/invensense/icm20649 imu/invensense/icm20649
imu/invensense/icm20948 imu/invensense/icm20948
boards/holybro/durandal-v1/default.cmake
+1 -1
View File
@@ -31,7 +31,7 @@ px4_add_board(
imu/adis16448 imu/adis16448
imu/adis16477 imu/adis16477
imu/adis16497 imu/adis16497
imu/bmi088 imu/bosch/bmi088
imu/invensense/icm20689 imu/invensense/icm20689
irlock irlock
lights/blinkm 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 # Internal SPI bus ICM-20689
icm20689 -R 2 -s start icm20689 -R 2 -s start
# Internal SPI bus BMI088 accel/gyro # Internal SPI BMI088
bmi088 -A -R 10 -s start bmi088 -A -R 2 -s start
bmi088 -G -R 10 -s start bmi088 -G -R 2 -s start
# internal compass # internal compass
ist8310 -I start ist8310 -I start
boards/holybro/durandal-v1/stackcheck.cmake
+1 -1
View File
@@ -32,7 +32,7 @@ px4_add_board(
#imu/adis16448 #imu/adis16448
#imu/adis16477 #imu/adis16477
#imu/adis16497 #imu/adis16497
#imu/bmi088 #imu/bosch/bmi088
imu/invensense/icm20689 imu/invensense/icm20689
#irlock #irlock
#lights/blinkm #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 distance_sensor # all available distance sensor drivers
dshot dshot
gps gps
imu/bmi088 imu/bosch/bmi088
imu/invensense/icm20602 imu/invensense/icm20602
imu/invensense/icm42688p imu/invensense/icm42688p
irlock 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 # Internal SPI bus ICM-42688
icm42688p -R 12 -s start icm42688p -R 12 -s start
# Internal SPI bus BMI088 accel/gyro # Internal SPI4 BMI088
bmi088 -A -R 4 -s start bmi088 -A -R 12 -s start
bmi088 -G -R 4 -s start bmi088 -G -R 12 -s start
# Internal I2C Baro # Internal I2C Baro
bmp388 -I start bmp388 -I start
boards/mro/ctrl-zero-f7/default.cmake
+1 -1
View File
@@ -29,7 +29,7 @@ px4_add_board(
gps gps
#heater #heater
#imu # all available imu drivers #imu # all available imu drivers
imu/bmi088 imu/bosch/bmi088
imu/invensense/icm20602 imu/invensense/icm20602
imu/invensense/icm20948 imu/invensense/icm20948
irlock 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 icm20602 -s -R 8 start
# Internal SPI bus BMI088 accel & gyro # Internal SPI bus BMI088 accel & gyro
bmi088 -A -R 10 -s start bmi088 -A -R 8 -s start
bmi088 -G -R 10 -s start bmi088 -G -R 8 -s start
# Internal ICM-20948 (with magnetometer) # Internal ICM-20948 (with magnetometer)
icm20948 -s -R 8 -M start 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/adis16448
imu/adis16477 imu/adis16477
imu/adis16497 imu/adis16497
imu/bmi088 imu/bosch/bmi088
imu/invensense/icm20602 imu/invensense/icm20602
imu/st/ism330dlc imu/st/ism330dlc
irlock irlock
boards/px4/fmu-v5x/default.cmake
+1 -1
View File
@@ -31,7 +31,7 @@ px4_add_board(
imu/adis16448 imu/adis16448
imu/adis16477 imu/adis16477
imu/adis16497 imu/adis16497
imu/bmi088 imu/bosch/bmi088
imu/invensense/icm20602 imu/invensense/icm20602
imu/st/ism330dlc imu/st/ism330dlc
irlock 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 # Internal SPI bus ISM300DLC
ism330dlc -s start ism330dlc -s start
# Internal SPI bus BMI088 accel/gyro # Internal SPI BMI088
bmi088 -A -R 12 -s start bmi088 -A -R 4 -s start
bmi088 -G -R 12 -s start bmi088 -G -R 4 -s start
# Possible internal compass # Possible internal compass
bmm150 -I start 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}), }, {GPIO::PortD, GPIO::Pin15}),
initSPIBus(SPI::Bus::SPI3, { initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), 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}), }, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, { // initSPIBus(SPI::Bus::SPI4, {
// // no devices // // no devices
platforms/common/include/px4_platform_common/log.h
+1
View File
@@ -122,6 +122,7 @@ __END_DECLS
#include <stdarg.h> #include <stdarg.h>
#include <px4_platform_common/defines.h> #include <px4_platform_common/defines.h>
#include <drivers/drv_hrt.h>
__BEGIN_DECLS __BEGIN_DECLS
src/drivers/imu/bmi088/BMI088_accel.cpp
-584
View File
File diff suppressed because it is too large Load Diff
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -33,66 +33,54 @@
#pragma once #pragma once
#include <drivers/device/spi.h> #include <drivers/drv_hrt.h>
#include <ecl/geo/geo.h> #include <lib/drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#include <lib/perf/perf_counter.h> #include <lib/perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h> #include <px4_platform_common/i2c_spi_buses.h>
#define DIR_READ 0x80 static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; }
#define DIR_WRITE 0x00
//Soft-reset command Value
#define BMI088_SOFT_RESET 0xB6
#define BMI088_BUS_SPEED 10*1000*1000
class BMI088 : public device::SPI, public I2CSPIDriver<BMI088> class BMI088 : public device::SPI, public I2CSPIDriver<BMI088>
{ {
public: public:
BMI088(const char *name, const char *devname, I2CSPIBusOption bus_option, int bus, uint8_t type, uint32_t device, BMI088(uint8_t devtype, const char *name, I2CSPIBusOption bus_option, int bus, uint32_t device, enum spi_mode_e mode,
enum spi_mode_e mode, uint32_t frequency, enum Rotation rotation); uint32_t frequency, spi_drdy_gpio_t drdy_gpio);
virtual ~BMI088() = default; virtual ~BMI088() = default;
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance); int runtime_instance);
static void print_usage(); static void print_usage();
virtual void start() = 0;
virtual void RunImpl() = 0; virtual void RunImpl() = 0;
int init() override;
virtual void print_status() = 0;
protected: 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; hrt_abstime _reset_timestamp{0};
uint64_t _reset_wait; 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,
};
/** STATE _state{STATE::RESET};
* Read a register from the BMI088
* uint16_t _fifo_empty_interval_us{2500}; // 2500 us / 400 Hz transfer interval
* @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);
/**
* 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
File diff suppressed because it is too large Load Diff
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 # Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions # modification, are permitted provided that the following conditions
@@ -30,13 +30,25 @@
# POSSIBILITY OF SUCH DAMAGE. # POSSIBILITY OF SUCH DAMAGE.
# #
############################################################################ ############################################################################
px4_add_module( px4_add_module(
MODULE drivers__bmi88 MODULE drivers__imu__bosch__bmi088
MAIN bmi088 MAIN bmi088
COMPILE_FLAGS COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS SRCS
BMI088_accel.cpp Bosch_BMI088_Accelerometer_Registers.hpp
BMI088_gyro.cpp Bosch_BMI088_Gyroscope_Registers.hpp
BMI088.cpp
BMI088.hpp
BMI088_Accelerometer.cpp
BMI088_Accelerometer.hpp
BMI088_Gyroscope.cpp
BMI088_Gyroscope.hpp
bmi088_main.cpp 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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -31,15 +31,12 @@
* *
****************************************************************************/ ****************************************************************************/
#include "BMI088_accel.hpp" #include <px4_platform_common/getopt.h>
#include "BMI088_gyro.hpp"
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h> #include <px4_platform_common/module.h>
extern "C" __EXPORT int bmi088_main(int argc, char *argv[]); #include "BMI088.hpp"
void void BMI088::print_usage()
BMI088::print_usage()
{ {
PRINT_MODULE_USAGE_NAME("bmi088", "driver"); PRINT_MODULE_USAGE_NAME("bmi088", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_SUBCATEGORY("imu");
@@ -48,107 +45,16 @@ BMI088::print_usage()
PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true); PRINT_MODULE_USAGE_PARAM_FLAG('G', "Gyro", true);
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", 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(); PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
} }
I2CSPIDriverBase *BMI088::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, extern "C" int bmi088_main(int argc, char *argv[])
int runtime_instance)
{ {
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; int ch;
using ThisDriver = BMI088;
BusCLIArguments cli{false, true}; BusCLIArguments cli{false, true};
cli.type = 0; cli.type = 0;
cli.default_spi_frequency = BMI088_BUS_SPEED; cli.default_spi_frequency = 10000000;
while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) { while ((ch = cli.getopt(argc, argv, "AGR:")) != EOF) {
switch (ch) { switch (ch) {
@@ -187,16 +93,6 @@ bmi088_main(int argc, char *argv[])
return ThisDriver::module_status(iterator); 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(); ThisDriver::print_usage();
return -1; return -1;
} }
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+1 -1
View File
@@ -97,7 +97,7 @@ private:
float _range{16 * CONSTANTS_ONE_G}; float _range{16 * CONSTANTS_ONE_G};
float _scale{1.f}; float _scale{1.f};
float _temperature{0.f}; float _temperature{NAN};
float _clip_limit{_range / _scale}; 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 _range{math::radians(2000.f)};
float _scale{1.f}; float _scale{1.f};
float _temperature{0.f}; float _temperature{NAN};
uint32_t _error_count{0}; uint32_t _error_count{0};