From e27a76e4608ffe688540648bd45676247e7be555 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 9 Nov 2016 19:54:53 +1100
Subject: [PATCH] AP_InertialSensor: fixed auxiliary bus with FIFO enabled
make sure fifo reset doesn't check I2C master enable
---
.../AP_InertialSensor/AP_InertialSensor_MPU6000.cpp | 9 ++++++---
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h | 3 +++
.../AP_InertialSensor/AP_InertialSensor_MPU9250.cpp | 9 ++++++---
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h | 3 +++
4 files changed, 18 insertions(+), 6 deletions(-)
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 52f376bcd3..d49460fad8 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -322,9 +322,10 @@ bool AP_InertialSensor_MPU6000::_init()
void AP_InertialSensor_MPU6000::_fifo_reset()
{
- _register_write(MPUREG_USER_CTRL, 0);
- _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
- _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
+ uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
+ _register_write(MPUREG_USER_CTRL, user_ctrl);
+ _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
+ _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
}
void AP_InertialSensor_MPU6000::_fifo_enable()
@@ -907,6 +908,8 @@ void AP_MPU6000_AuxiliaryBus::_configure_slaves()
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
+ backend._master_i2c_enable = true;
+
/* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL,
BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index e187acbdd4..bdbf847324 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -115,6 +115,9 @@ private:
float _last_temp;
uint8_t _temp_counter;
+ // has master i2c been enabled?
+ bool _master_i2c_enable;
+
// buffer for fifo read
uint8_t *_fifo_buffer;
};
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
index c90dd22f82..031ec62b91 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
@@ -276,9 +276,10 @@ bool AP_InertialSensor_MPU9250::_init()
void AP_InertialSensor_MPU9250::_fifo_reset()
{
- _register_write(MPUREG_USER_CTRL, 0);
- _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_RESET);
- _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_FIFO_EN);
+ uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
+ _register_write(MPUREG_USER_CTRL, user_ctrl);
+ _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
+ _register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
}
void AP_InertialSensor_MPU9250::_fifo_enable()
@@ -781,6 +782,8 @@ void AP_MPU9250_AuxiliaryBus::_configure_slaves()
uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL);
backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN);
+ backend._master_i2c_enable = true;
+
/* stop condition between reads; clock at 400kHz */
backend._register_write(MPUREG_I2C_MST_CTRL,
I2C_MST_CLOCK_400KHZ | I2C_MST_P_NSR);
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
index c27fa5790e..3e7eedcec4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
@@ -100,6 +100,9 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
+
+ // has master i2c been enabled?
+ bool _master_i2c_enable;
// last temperature reading, used to detect FIFO errors
float _last_temp;
--
GitLab