diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index ab535e2bd2..6ffef1769d 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -307,7 +307,10 @@ static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
- i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
+ if (!i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf)) {
+ return;
+ }
+
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
@@ -338,7 +341,9 @@ static void mpu6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
- i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
+ if (!i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf)) {
+ return;
+ }
gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
diff --git a/src/main/drivers/accgyro_mpu9150.c b/src/main/drivers/accgyro_mpu9150.c
new file mode 100644
index 0000000000..df349d3ecd
--- /dev/null
+++ b/src/main/drivers/accgyro_mpu9150.c
@@ -0,0 +1,353 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+// NOTE: this file is a copy of the mpu6050 driver with very minimal changes.
+// some de-duplication needs to occur...
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "common/maths.h"
+
+#include "system.h"
+#include "gpio.h"
+#include "bus_i2c.h"
+
+#include "accgyro.h"
+#include "accgyro_mpu9150.h"
+
+#define MPU9150_ADDRESS 0xD0
+
+#define DMP_MEM_START_ADDR 0x6E
+#define DMP_MEM_R_W 0x6F
+
+#define MPU_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
+#define MPU_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
+#define MPU_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
+#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
+#define MPU_RA_XA_OFFS_L_TC 0x07
+#define MPU_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
+#define MPU_RA_YA_OFFS_L_TC 0x09
+#define MPU_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
+#define MPU_RA_ZA_OFFS_L_TC 0x0B
+#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register
+#define MPU_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
+#define MPU_RA_XG_OFFS_USRL 0x14
+#define MPU_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
+#define MPU_RA_YG_OFFS_USRL 0x16
+#define MPU_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
+#define MPU_RA_ZG_OFFS_USRL 0x18
+#define MPU_RA_SMPLRT_DIV 0x19
+#define MPU_RA_CONFIG 0x1A
+#define MPU_RA_GYRO_CONFIG 0x1B
+#define MPU_RA_ACCEL_CONFIG 0x1C
+#define MPU_RA_FF_THR 0x1D
+#define MPU_RA_FF_DUR 0x1E
+#define MPU_RA_MOT_THR 0x1F
+#define MPU_RA_MOT_DUR 0x20
+#define MPU_RA_ZRMOT_THR 0x21
+#define MPU_RA_ZRMOT_DUR 0x22
+#define MPU_RA_FIFO_EN 0x23
+#define MPU_RA_I2C_MST_CTRL 0x24
+#define MPU_RA_I2C_SLV0_ADDR 0x25
+#define MPU_RA_I2C_SLV0_REG 0x26
+#define MPU_RA_I2C_SLV0_CTRL 0x27
+#define MPU_RA_I2C_SLV1_ADDR 0x28
+#define MPU_RA_I2C_SLV1_REG 0x29
+#define MPU_RA_I2C_SLV1_CTRL 0x2A
+#define MPU_RA_I2C_SLV2_ADDR 0x2B
+#define MPU_RA_I2C_SLV2_REG 0x2C
+#define MPU_RA_I2C_SLV2_CTRL 0x2D
+#define MPU_RA_I2C_SLV3_ADDR 0x2E
+#define MPU_RA_I2C_SLV3_REG 0x2F
+#define MPU_RA_I2C_SLV3_CTRL 0x30
+#define MPU_RA_I2C_SLV4_ADDR 0x31
+#define MPU_RA_I2C_SLV4_REG 0x32
+#define MPU_RA_I2C_SLV4_DO 0x33
+#define MPU_RA_I2C_SLV4_CTRL 0x34
+#define MPU_RA_I2C_SLV4_DI 0x35
+#define MPU_RA_I2C_MST_STATUS 0x36
+#define MPU_RA_INT_PIN_CFG 0x37
+#define MPU_RA_INT_ENABLE 0x38
+#define MPU_RA_DMP_INT_STATUS 0x39
+#define MPU_RA_INT_STATUS 0x3A
+#define MPU_RA_ACCEL_XOUT_H 0x3B
+#define MPU_RA_ACCEL_XOUT_L 0x3C
+#define MPU_RA_ACCEL_YOUT_H 0x3D
+#define MPU_RA_ACCEL_YOUT_L 0x3E
+#define MPU_RA_ACCEL_ZOUT_H 0x3F
+#define MPU_RA_ACCEL_ZOUT_L 0x40
+#define MPU_RA_TEMP_OUT_H 0x41
+#define MPU_RA_TEMP_OUT_L 0x42
+#define MPU_RA_GYRO_XOUT_H 0x43
+#define MPU_RA_GYRO_XOUT_L 0x44
+#define MPU_RA_GYRO_YOUT_H 0x45
+#define MPU_RA_GYRO_YOUT_L 0x46
+#define MPU_RA_GYRO_ZOUT_H 0x47
+#define MPU_RA_GYRO_ZOUT_L 0x48
+#define MPU_RA_EXT_SENS_DATA_00 0x49
+#define MPU_RA_MOT_DETECT_STATUS 0x61
+#define MPU_RA_I2C_SLV0_DO 0x63
+#define MPU_RA_I2C_SLV1_DO 0x64
+#define MPU_RA_I2C_SLV2_DO 0x65
+#define MPU_RA_I2C_SLV3_DO 0x66
+#define MPU_RA_I2C_MST_DELAY_CTRL 0x67
+#define MPU_RA_SIGNAL_PATH_RESET 0x68
+#define MPU_RA_MOT_DETECT_CTRL 0x69
+#define MPU_RA_USER_CTRL 0x6A
+#define MPU_RA_PWR_MGMT_1 0x6B
+#define MPU_RA_PWR_MGMT_2 0x6C
+#define MPU_RA_BANK_SEL 0x6D
+#define MPU_RA_MEM_START_ADDR 0x6E
+#define MPU_RA_MEM_R_W 0x6F
+#define MPU_RA_DMP_CFG_1 0x70
+#define MPU_RA_DMP_CFG_2 0x71
+#define MPU_RA_FIFO_COUNTH 0x72
+#define MPU_RA_FIFO_COUNTL 0x73
+#define MPU_RA_FIFO_R_W 0x74
+#define MPU_RA_WHO_AM_I 0x75
+
+#define MPU9150_SMPLRT_DIV 0 // 8000Hz
+
+enum lpf_e {
+ INV_FILTER_256HZ_NOLPF2 = 0,
+ INV_FILTER_188HZ,
+ INV_FILTER_98HZ,
+ INV_FILTER_42HZ,
+ INV_FILTER_20HZ,
+ INV_FILTER_10HZ,
+ INV_FILTER_5HZ,
+ INV_FILTER_2100HZ_NOLPF,
+ NUM_FILTER
+};
+enum gyro_fsr_e {
+ INV_FSR_250DPS = 0,
+ INV_FSR_500DPS,
+ INV_FSR_1000DPS,
+ INV_FSR_2000DPS,
+ NUM_GYRO_FSR
+};
+enum clock_sel_e {
+ INV_CLK_INTERNAL = 0,
+ INV_CLK_PLL,
+ NUM_CLK
+};
+enum accel_fsr_e {
+ INV_FSR_2G = 0,
+ INV_FSR_4G,
+ INV_FSR_8G,
+ INV_FSR_16G,
+ NUM_ACCEL_FSR
+};
+
+static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+static void mpu9150AccInit(void);
+static void mpu9150AccRead(int16_t *accData);
+static void mpu9150GyroInit(void);
+static void mpu9150GyroRead(int16_t *gyroData);
+
+typedef enum {
+ MPU_9150_HALF_RESOLUTION,
+ MPU_9150_FULL_RESOLUTION
+} mpu9150Resolution_e;
+
+static mpu9150Resolution_e mpuAccelTrim;
+
+static const mpu9150Config_t *mpu9150Config = NULL;
+
+void mpu9150GpioInit(void) {
+ gpio_config_t gpio;
+
+ if (mpu9150Config->gpioAPB2Peripherals) {
+ RCC_APB2PeriphClockCmd(mpu9150Config->gpioAPB2Peripherals, ENABLE);
+ }
+
+ gpio.pin = mpu9150Config->gpioPin;
+ gpio.speed = Speed_2MHz;
+ gpio.mode = Mode_IN_FLOATING;
+ gpioInit(mpu9150Config->gpioPort, &gpio);
+}
+
+static bool mpu9150Detect(void)
+{
+ bool ack;
+ uint8_t sig;
+
+ ack = i2cRead(MPU9150_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
+ if (!ack) {
+ return false;
+ }
+
+ // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
+ // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address.
+ // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already).
+ // But here's the best part: The value of the AD0 pin is not reflected in this register.
+
+ return true;
+
+ if (sig != (MPU9150_ADDRESS & 0x7e))
+ return false;
+
+ return true;
+}
+
+bool mpu9150AccDetect(const mpu9150Config_t *configToUse, acc_t *acc)
+{
+ uint8_t readBuffer[6];
+ uint8_t revision;
+ uint8_t productId;
+
+ mpu9150Config = configToUse;
+
+ if (!mpu9150Detect()) {
+ return false;
+ }
+
+ // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
+ // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
+
+ // determine product ID and accel revision
+ i2cRead(MPU9150_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer);
+ revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
+ if (revision) {
+ /* Congrats, these parts are better. */
+ if (revision == 1) {
+ mpuAccelTrim = MPU_9150_HALF_RESOLUTION;
+ } else if (revision == 2) {
+ mpuAccelTrim = MPU_9150_FULL_RESOLUTION;
+ } else {
+ failureMode(5);
+ }
+ } else {
+ i2cRead(MPU9150_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId);
+ revision = productId & 0x0F;
+ if (!revision) {
+ failureMode(5);
+ } else if (revision == 4) {
+ mpuAccelTrim = MPU_9150_HALF_RESOLUTION;
+ } else {
+ mpuAccelTrim = MPU_9150_FULL_RESOLUTION;
+ }
+ }
+
+ acc->init = mpu9150AccInit;
+ acc->read = mpu9150AccRead;
+ acc->revisionCode = (mpuAccelTrim == MPU_9150_HALF_RESOLUTION ? 'o' : 'n');
+
+ return true;
+}
+
+bool mpu9150GyroDetect(const mpu9150Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
+{
+ mpu9150Config = configToUse;
+
+ if (!mpu9150Detect()) {
+ return false;
+ }
+
+
+ gyro->init = mpu9150GyroInit;
+ gyro->read = mpu9150GyroRead;
+
+ // 16.4 dps/lsb scalefactor
+ gyro->scale = 1.0f / 16.4f;
+
+ if (lpf >= 188)
+ mpuLowPassFilter = INV_FILTER_188HZ;
+ else if (lpf >= 98)
+ mpuLowPassFilter = INV_FILTER_98HZ;
+ else if (lpf >= 42)
+ mpuLowPassFilter = INV_FILTER_42HZ;
+ else if (lpf >= 20)
+ mpuLowPassFilter = INV_FILTER_20HZ;
+ else if (lpf >= 10)
+ mpuLowPassFilter = INV_FILTER_10HZ;
+ else
+ mpuLowPassFilter = INV_FILTER_5HZ;
+
+ return true;
+}
+
+static void mpu9150AccInit(void)
+{
+ if (mpu9150Config) {
+ mpu9150GpioInit();
+ mpu9150Config = NULL; // avoid re-initialisation of GPIO;
+ }
+
+ switch (mpuAccelTrim) {
+ case MPU_9150_HALF_RESOLUTION:
+ acc_1G = 256 * 8;
+ break;
+ case MPU_9150_FULL_RESOLUTION:
+ acc_1G = 512 * 8;
+ break;
+ }
+}
+
+static void mpu9150AccRead(int16_t *accData)
+{
+ uint8_t buf[6];
+
+ if (!i2cRead(MPU9150_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf)) {
+ return;
+ }
+
+ accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+ accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+ accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
+}
+
+static void mpu9150GyroInit(void)
+{
+ if (mpu9150Config) {
+ mpu9150GpioInit();
+ mpu9150Config = NULL; // avoid re-initialisation of GPIO;
+ }
+
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
+ delay(100);
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_INT_PIN_CFG,
+ 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
+
+ // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops.
+ // Accel scale 8g (4096 LSB/g)
+ i2cWrite(MPU9150_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+}
+
+static void mpu9150GyroRead(int16_t *gyroData)
+{
+ uint8_t buf[6];
+
+ if (!i2cRead(MPU9150_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf)) {
+ return;
+ }
+
+ gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+ gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+ gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
+}
diff --git a/src/main/drivers/accgyro_mpu9150.h b/src/main/drivers/accgyro_mpu9150.h
new file mode 100644
index 0000000000..e511881703
--- /dev/null
+++ b/src/main/drivers/accgyro_mpu9150.h
@@ -0,0 +1,29 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#pragma once
+
+typedef struct mpu9150Config_s {
+ uint32_t gpioAPB2Peripherals;
+ uint16_t gpioPin;
+ GPIO_TypeDef *gpioPort;
+} mpu9150Config_t;
+
+bool mpu9150AccDetect(const mpu9150Config_t *config,acc_t *acc);
+bool mpu9150GyroDetect(const mpu9150Config_t *config, gyro_t *gyro, uint16_t lpf);
+void mpu9150DmpLoop(void);
+void mpu9150DmpResetFifo(void);
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 97a85e9a20..7b2a32dff2 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -132,7 +132,7 @@ static const char * const sensorNames[] = {
};
static const char * const accNames[] = {
- "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL
+ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9150", "FAKE", "None", NULL
};
typedef struct {
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 2b7e8ce94d..f10817629c 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -27,8 +27,9 @@ typedef enum AccelSensors {
ACC_LSM303DLHC = 5,
ACC_SPI_MPU6000 = 6,
ACC_SPI_MPU6500 = 7,
- ACC_FAKE = 8,
- ACC_NONE = 9
+ ACC_MPU9150 = 8,
+ ACC_FAKE = 9,
+ ACC_NONE = 10
} AccelSensors;
extern uint8_t accHardware;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 2ca392eedd..63cb47c225 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -109,6 +109,8 @@ static void applyGyroZero(void)
void gyroGetADC(void)
{
+ // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.
+
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
alignSensors(gyroADC, gyroADC, gyroAlign);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 3b168a058e..323e0538a0 100644
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -32,6 +32,7 @@
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
+#include "drivers/accgyro_mpu9150.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
@@ -139,6 +140,12 @@ bool detectGyro(uint16_t gyroLpf)
}
#endif
+#ifdef USE_GYRO_MPU9150
+ if (mpu9150GyroDetect(NULL, &gyro, gyroLpf)) {
+ return true;
+ }
+#endif
+
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
#ifdef NAZE
@@ -245,6 +252,15 @@ retry:
}
; // fallthrough
#endif
+#ifdef USE_ACC_MPU9150
+ case ACC_MPU9150: // MPU9150
+ if (mpu9150AccDetect(NULL, &acc)) {
+ accHardware = ACC_MPU9150;
+ if (accHardwareToUse == ACC_MPU9150)
+ break;
+ }
+ ; // fallthrough
+#endif
#ifdef USE_ACC_MMA8452
case ACC_MMA8452: // MMA8452
#ifdef NAZE