PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <bmi160.hpp>
Classes | |
struct | BMIReport |
Report conversation within the BMI160, including command byte and interrupt status. More... | |
Public Member Functions | |
BMI160 (int bus, uint32_t device, enum Rotation rotation) | |
virtual | ~BMI160 () |
virtual int | init () |
void | print_info () |
Diagnostics - print some basic information about the driver. More... | |
void | print_registers () |
void | test_error () |
Protected Member Functions | |
virtual int | probe () |
Private Member Functions | |
void | start () |
Start automatic measurement. More... | |
void | stop () |
Stop automatic measurement. More... | |
int | reset () |
Reset chip. More... | |
void | Run () override |
void | measure () |
Fetch measurements from the sensor and update the report buffers. More... | |
uint8_t | read_reg (unsigned reg) |
Read a register from the BMI160. More... | |
uint16_t | read_reg16 (unsigned reg) |
void | write_reg (unsigned reg, uint8_t value) |
Write a register in the BMI160. More... | |
void | modify_reg (unsigned reg, uint8_t clearbits, uint8_t setbits) |
Modify a register in the BMI160. More... | |
void | write_checked_reg (unsigned reg, uint8_t value) |
Write a register in the BMI160, updating _checked_values. More... | |
int | set_accel_range (unsigned max_g) |
Set the BMI160 measurement range. More... | |
int | set_gyro_range (unsigned max_dps) |
uint16_t | swap16 (uint16_t val) |
Swap a 16-bit value read from the BMI160 to native byte order. More... | |
void | _set_dlpf_filter (uint16_t frequency_hz) |
int | accel_set_sample_rate (float desired_sample_rate_hz) |
int | gyro_set_sample_rate (float desired_sample_rate_hz) |
void | check_registers (void) |
BMI160 (const BMI160 &) | |
BMI160 | operator= (const BMI160 &) |
Private Attributes | |
PX4Accelerometer | _px4_accel |
PX4Gyroscope | _px4_gyro |
uint8_t | _whoami |
unsigned | _dlpf_freq |
whoami result More... | |
float | _accel_sample_rate {BMI160_ACCEL_DEFAULT_RATE} |
float | _gyro_sample_rate {BMI160_GYRO_DEFAULT_RATE} |
perf_counter_t | _accel_reads |
perf_counter_t | _gyro_reads |
perf_counter_t | _sample_perf |
perf_counter_t | _bad_transfers |
perf_counter_t | _bad_registers |
perf_counter_t | _good_transfers |
perf_counter_t | _reset_retries |
perf_counter_t | _duplicates |
uint8_t | _register_wait {0} |
uint64_t | _reset_wait {0} |
uint8_t | _checked_values [BMI160_NUM_CHECKED_REGISTERS] |
uint8_t | _checked_bad [BMI160_NUM_CHECKED_REGISTERS] |
uint8_t | _checked_next {0} |
uint16_t | _last_accel [3] {} |
bool | _got_duplicate {false} |
Static Private Attributes | |
static constexpr int | BMI160_NUM_CHECKED_REGISTERS {10} |
static const uint8_t | _checked_registers [BMI160_NUM_CHECKED_REGISTERS] |
Definition at line 246 of file bmi160.hpp.
BMI160::BMI160 | ( | int | bus, |
uint32_t | device, | ||
enum Rotation | rotation | ||
) |
Definition at line 52 of file bmi160.cpp.
References _px4_accel, _px4_gyro, BMI160_ACCEL_DEFAULT_RATE, BMI160_GYRO_DEFAULT_RATE, DRV_ACC_DEVTYPE_BMI160, DRV_GYR_DEVTYPE_BMI160, PX4Accelerometer::set_device_type(), PX4Gyroscope::set_device_type(), and PX4Accelerometer::set_sample_rate().
|
virtual |
Definition at line 73 of file bmi160.cpp.
References _accel_reads, _bad_registers, _bad_transfers, _duplicates, _good_transfers, _gyro_reads, _reset_retries, _sample_perf, perf_free(), and stop().
|
private |
|
private |
Definition at line 308 of file bmi160.cpp.
References _dlpf_freq.
|
private |
Definition at line 190 of file bmi160.cpp.
References _accel_sample_rate, BMI_ACCEL_RATE_100, BMI_ACCEL_RATE_1600, BMI_ACCEL_RATE_200, BMI_ACCEL_RATE_25, BMI_ACCEL_RATE_25_16, BMI_ACCEL_RATE_25_2, BMI_ACCEL_RATE_25_32, BMI_ACCEL_RATE_25_4, BMI_ACCEL_RATE_25_8, BMI_ACCEL_RATE_400, BMI_ACCEL_RATE_50, BMI_ACCEL_RATE_800, BMIREG_ACC_CONF, modify_reg(), and OK.
Referenced by reset().
|
private |
Definition at line 523 of file bmi160.cpp.
References _bad_registers, _checked_bad, _checked_next, _checked_registers, _checked_values, _register_wait, _reset_wait, BMI160_NUM_CHECKED_REGISTERS, BMI160_SOFT_RESET, BMIREG_CMD, hrt_absolute_time(), perf_count(), read_reg(), and write_reg().
Referenced by measure().
|
private |
Definition at line 257 of file bmi160.cpp.
References _gyro_sample_rate, BMI_GYRO_RATE_100, BMI_GYRO_RATE_1600, BMI_GYRO_RATE_200, BMI_GYRO_RATE_25, BMI_GYRO_RATE_3200, BMI_GYRO_RATE_400, BMI_GYRO_RATE_50, BMI_GYRO_RATE_800, BMIREG_GYR_CONF, modify_reg(), and OK.
Referenced by reset().
|
virtual |
Definition at line 90 of file bmi160.cpp.
References DEVICE_DEBUG, ToneAlarmInterface::init(), OK, reset(), and start().
|
private |
Fetch measurements from the sensor and update the report buffers.
Definition at line 566 of file bmi160.cpp.
References _bad_registers, _bad_transfers, _duplicates, _good_transfers, _got_duplicate, _last_accel, _px4_accel, _px4_gyro, _register_wait, _reset_wait, _sample_perf, BMI160::BMIReport::accel_x, BMI160::BMIReport::accel_y, BMI160::BMIReport::accel_z, BMIREG_GYR_X_L, BMIREG_STATUS, BMIREG_TEMP_0, BMIREG_TEMP_1, check_registers(), BMI160::BMIReport::cmd, DIR_READ, BMI160::BMIReport::gyro_x, BMI160::BMIReport::gyro_y, BMI160::BMIReport::gyro_z, hrt_absolute_time(), hrt_abstime, OK, perf_begin(), perf_count(), perf_end(), perf_event_count(), read_reg(), PX4Accelerometer::set_error_count(), PX4Gyroscope::set_error_count(), PX4Accelerometer::set_temperature(), PX4Gyroscope::set_temperature(), status, PX4Accelerometer::update(), and PX4Gyroscope::update().
Referenced by Run().
|
private |
Modify a register in the BMI160.
Bits are cleared before bits are set.
reg | The register to modify. |
clearbits | Bits in the register to clear. |
setbits | Bits in the register to set. |
Definition at line 384 of file bmi160.cpp.
References read_reg(), and write_checked_reg().
Referenced by accel_set_sample_rate(), gyro_set_sample_rate(), set_accel_range(), and set_gyro_range().
void BMI160::print_info | ( | ) |
Diagnostics - print some basic information about the driver.
Definition at line 696 of file bmi160.cpp.
References _accel_reads, _bad_registers, _bad_transfers, _checked_bad, _checked_next, _checked_registers, _checked_values, _duplicates, _good_transfers, _gyro_reads, _px4_accel, _px4_gyro, _reset_retries, _sample_perf, BMI160_NUM_CHECKED_REGISTERS, perf_print_counter(), PX4Accelerometer::print_status(), PX4Gyroscope::print_status(), and read_reg().
void BMI160::print_registers | ( | ) |
Definition at line 732 of file bmi160.cpp.
References read_reg().
Referenced by test_error().
|
protectedvirtual |
Definition at line 170 of file bmi160.cpp.
References _checked_bad, _checked_values, _whoami, BMI160_WHO_AM_I, BMIREG_CHIP_ID, DEVICE_DEBUG, OK, and read_reg().
|
private |
Read a register from the BMI160.
The | register to read. |
Definition at line 353 of file bmi160.cpp.
References DIR_READ.
Referenced by check_registers(), measure(), modify_reg(), print_info(), print_registers(), probe(), and reset().
|
private |
Definition at line 363 of file bmi160.cpp.
References DIR_READ.
|
private |
Reset chip.
Resets the chip and measurements ranges, but not scale and offset.
Definition at line 112 of file bmi160.cpp.
References _accel_reads, _checked_registers, _checked_values, _gyro_reads, accel_set_sample_rate(), BMI160_ACCEL_DEFAULT_RANGE_G, BMI160_ACCEL_DEFAULT_RATE, BMI160_GYRO_DEFAULT_RANGE_DPS, BMI160_GYRO_DEFAULT_RATE, BMI160_NUM_CHECKED_REGISTERS, BMI_ACCEL_BWP_NORMAL, BMI_ACCEL_NORMAL_MODE, BMI_ACCEL_US, BMI_AUTO_DIS_SEC, BMI_DRDY_INT1, BMI_DRDY_INT_EN, BMI_GYRO_BWP_NORMAL, BMI_GYRO_NORMAL_MODE, BMI_INT1_EN, BMI_SPI, BMI_SPI_4_WIRE, BMIREG_ACC_CONF, BMIREG_ACC_RANGE, BMIREG_CMD, BMIREG_CONF, BMIREG_GYR_CONF, BMIREG_GYR_RANGE, BMIREG_IF_CONF, BMIREG_INT_EN_1, BMIREG_INT_MAP_1, BMIREG_INT_OUT_CTRL, BMIREG_NV_CONF, gyro_set_sample_rate(), OK, read_reg(), set_accel_range(), set_gyro_range(), write_checked_reg(), and write_reg().
Referenced by init(), and start().
|
overrideprivate |
Definition at line 516 of file bmi160.cpp.
References measure().
|
private |
Set the BMI160 measurement range.
max_g | The maximum G value the range must support. |
max_dps | The maximum DPS value the range must support. |
Definition at line 408 of file bmi160.cpp.
References _px4_accel, BMI_ACCEL_RANGE_16_G, BMI_ACCEL_RANGE_2_G, BMI_ACCEL_RANGE_4_G, BMI_ACCEL_RANGE_8_G, BMIREG_ACC_RANGE, CONSTANTS_ONE_G, modify_reg(), OK, and PX4Accelerometer::set_scale().
Referenced by reset().
|
private |
Definition at line 450 of file bmi160.cpp.
References _px4_gyro, BMI_GYRO_RANGE_1000_DPS, BMI_GYRO_RANGE_125_DPS, BMI_GYRO_RANGE_2000_DPS, BMI_GYRO_RANGE_250_DPS, BMI_GYRO_RANGE_500_DPS, BMIREG_GYR_RANGE, f(), M_PI_F, modify_reg(), OK, and PX4Gyroscope::set_scale().
Referenced by reset().
|
private |
Start automatic measurement.
Definition at line 498 of file bmi160.cpp.
References BMI160_GYRO_DEFAULT_RATE, BMI160_TIMER_REDUCTION, reset(), and stop().
Referenced by init().
|
private |
Stop automatic measurement.
Definition at line 510 of file bmi160.cpp.
Referenced by start(), and ~BMI160().
|
inlineprivate |
Swap a 16-bit value read from the BMI160 to native byte order.
Definition at line 377 of file bmi160.hpp.
void BMI160::test_error | ( | ) |
Definition at line 345 of file bmi160.cpp.
References BMI160_SOFT_RESET, BMIREG_CMD, print_registers(), and write_reg().
|
private |
Write a register in the BMI160, updating _checked_values.
reg | The register to write. |
value | The new value to write. |
Definition at line 395 of file bmi160.cpp.
References _checked_bad, _checked_registers, _checked_values, BMI160_NUM_CHECKED_REGISTERS, and write_reg().
Referenced by modify_reg(), and reset().
|
private |
Write a register in the BMI160.
reg | The register to write. |
value | The new value to write. |
Definition at line 373 of file bmi160.cpp.
References DIR_WRITE.
Referenced by check_registers(), reset(), test_error(), and write_checked_reg().
|
private |
Definition at line 279 of file bmi160.hpp.
Referenced by print_info(), reset(), and ~BMI160().
|
private |
Definition at line 276 of file bmi160.hpp.
Referenced by accel_set_sample_rate().
|
private |
Definition at line 283 of file bmi160.hpp.
Referenced by check_registers(), measure(), print_info(), and ~BMI160().
|
private |
Definition at line 282 of file bmi160.hpp.
Referenced by measure(), print_info(), and ~BMI160().
|
private |
Definition at line 297 of file bmi160.hpp.
Referenced by check_registers(), print_info(), probe(), and write_checked_reg().
|
private |
Definition at line 298 of file bmi160.hpp.
Referenced by check_registers(), and print_info().
|
staticprivate |
Definition at line 295 of file bmi160.hpp.
Referenced by check_registers(), print_info(), reset(), and write_checked_reg().
|
private |
Definition at line 296 of file bmi160.hpp.
Referenced by check_registers(), print_info(), probe(), reset(), and write_checked_reg().
|
private |
|
private |
Definition at line 286 of file bmi160.hpp.
Referenced by measure(), print_info(), and ~BMI160().
|
private |
Definition at line 284 of file bmi160.hpp.
Referenced by measure(), print_info(), and ~BMI160().
|
private |
Definition at line 302 of file bmi160.hpp.
Referenced by measure().
|
private |
Definition at line 280 of file bmi160.hpp.
Referenced by print_info(), reset(), and ~BMI160().
|
private |
Definition at line 277 of file bmi160.hpp.
Referenced by gyro_set_sample_rate().
|
private |
Definition at line 301 of file bmi160.hpp.
Referenced by measure().
|
private |
Definition at line 269 of file bmi160.hpp.
Referenced by BMI160(), measure(), print_info(), and set_accel_range().
|
private |
Definition at line 270 of file bmi160.hpp.
Referenced by BMI160(), measure(), print_info(), and set_gyro_range().
|
private |
Definition at line 288 of file bmi160.hpp.
Referenced by check_registers(), and measure().
|
private |
Definition at line 285 of file bmi160.hpp.
Referenced by print_info(), and ~BMI160().
|
private |
Definition at line 289 of file bmi160.hpp.
Referenced by check_registers(), and measure().
|
private |
Definition at line 281 of file bmi160.hpp.
Referenced by measure(), print_info(), and ~BMI160().
|
private |
Definition at line 272 of file bmi160.hpp.
Referenced by probe().
|
staticprivate |
Definition at line 294 of file bmi160.hpp.
Referenced by check_registers(), print_info(), reset(), and write_checked_reg().