Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

mpu9250: accumulated minor improvements and cleanup

Browse Source 
- perform reset as per the datasheet (disable I2C immediately, set power mode, wait for appropriate time, etc)
 - remove interrupt perf counter and instead only count misses
 - minor style changes to stay in sync with the other Invensense drivers
This commit is contained in:
Daniel Agar
2020-07-09 12:23:36 -04:00
parent 6b4c14a15c
commit 33e3456de2
6 changed files with 36 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/mpu9250/CMakeLists.txt
-3
View File
@@ -35,9 +35,6 @@ px4_add_module(
MODULE drivers__imu__invensense__mpu9250
MAIN mpu9250
COMPILE_FLAGS
-O0
-DDEBUG_BUILD
-Wno-error
SRCS
AKM_AK8963_registers.hpp
InvenSense_MPU9250_registers.hpp
src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
+2 -4
View File
@@ -213,12 +213,10 @@ enum PWR_MGMT_1_BIT : uint8_t {
H_RESET = Bit7,
SLEEP = Bit6,
CLKSEL_2 = Bit2,
CLKSEL_1 = Bit1,
CLKSEL_0 = Bit0,
// CLKSEL[2:0]
CLKSEL_0 = Bit0, // It is required that CLKSEL[2:0] be set to 001 to achieve full gyroscope performance.
};
namespace FIFO
{
static constexpr size_t SIZE = 512;
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+21 -18
View File
@@ -51,7 +51,7 @@ MPU9250::MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval");
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
@@ -82,7 +82,7 @@ MPU9250::~MPU9250()
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_interval_perf);
perf_free(_drdy_missed_perf);
delete _slave_ak8963_magnetometer;
}
@@ -118,14 +118,14 @@ void MPU9250::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_interval_perf);
perf_print_counter(_drdy_missed_perf);
if (_slave_ak8963_magnetometer) {
@@ -154,7 +154,7 @@ void MPU9250::RunImpl()
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
_reset_timestamp = now;
_consecutive_failures = 0;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(100_ms);
break;
@@ -167,11 +167,11 @@ void MPU9250::RunImpl()
&& (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
// Wakeup and reset digital signal path
RegisterWrite(Register::PWR_MGMT_1, 0);
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
RegisterWrite(Register::SIGNAL_PATH_RESET,
SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET);
RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS | USER_CTRL_BIT::I2C_MST_RST |
USER_CTRL_BIT::SIG_COND_RST);
RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS |
USER_CTRL_BIT::I2C_MST_RST);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
@@ -233,8 +233,8 @@ void MPU9250::RunImpl()
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);
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
}
// push backup schedule back
@@ -265,22 +265,25 @@ void MPU9250::RunImpl()
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
_consecutive_failures = 0;
if (_failure_count > 0) {
_failure_count--;
}
}
}
}
if (!success) {
_consecutive_failures++;
_failure_count++;
// full reset if things are failing consistently
if (_consecutive_failures > 10) {
if (_failure_count > 10) {
Reset();
return;
}
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
@@ -406,12 +409,12 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
void MPU9250::DataReady()
{
const uint8_t count = _drdy_count.fetch_add(1) + 1;
uint8_t expected = 0;
uint32_t expected = 0;
// at least the required number of samples in the FIFO
if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples)
&& _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
ScheduleNow();
}
src/drivers/imu/invensense/mpu9250/MPU9250.hpp
+6 -6
View File
@@ -145,15 +145,15 @@ private:
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
perf_counter_t _drdy_interval_perf{nullptr};
perf_counter_t _drdy_missed_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
unsigned _consecutive_failures{0};
int _failure_count{0};
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_count{0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
px4::atomic<uint32_t> _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@@ -166,7 +166,7 @@ private:
STATE _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{12};
@@ -183,6 +183,6 @@ private:
{ Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 },
{ Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 },
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, 0 },
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
};
};
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp
+6 -4
View File
@@ -86,7 +86,7 @@ void MPU9250_AK8963::Run()
// CNTL2 SRST: Soft reset
_mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL2, CNTL2_BIT::SRST);
_reset_timestamp = hrt_absolute_time();
_consecutive_failures = 0;
_failure_count = 0;
_state = STATE::READ_WHO_AM_I;
ScheduleDelayed(100_ms);
break;
@@ -190,7 +190,9 @@ void MPU9250_AK8963::Run()
success = true;
_consecutive_failures = 0;
if (_failure_count > 0) {
_failure_count--;
}
ScheduleDelayed(20_ms); // ~50 Hz
return;
@@ -199,9 +201,9 @@ void MPU9250_AK8963::Run()
if (!success) {
perf_count(_bad_transfer_perf);
_consecutive_failures++;
_failure_count++;
if (_consecutive_failures > 100) {
if (_failure_count > 10) {
Reset();
return;
}
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp
+1 -1
View File
@@ -93,7 +93,7 @@ private:
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
unsigned _consecutive_failures{0};
int _failure_count{0};
bool _sensitivity_adjustments_loaded{false};
float _sensitivity[3] {1.f, 1.f, 1.f};