ICM-20689 Gyro
This commit is contained in:
commit
dc4b4ac986
|
@ -410,7 +410,7 @@ void createDefaultConfig(master_t *config)
|
|||
#ifdef STM32F10X
|
||||
config->gyro_sync_denom = 8;
|
||||
config->pid_process_denom = 1;
|
||||
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500)
|
||||
#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
|
||||
config->gyro_sync_denom = 1;
|
||||
config->pid_process_denom = 4;
|
||||
#else
|
||||
|
|
|
@ -41,6 +41,7 @@
|
|||
#include "accgyro_mpu6500.h"
|
||||
#include "accgyro_spi_mpu6000.h"
|
||||
#include "accgyro_spi_mpu6500.h"
|
||||
#include "accgyro_spi_icm20689.h"
|
||||
#include "accgyro_spi_mpu9250.h"
|
||||
#include "accgyro_mpu.h"
|
||||
|
||||
|
@ -144,6 +145,16 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
if (icm20689SpiDetect()) {
|
||||
mpuDetectionResult.sensor = ICM_20689_SPI;
|
||||
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||
mpuConfiguration.read = icm20689ReadRegister;
|
||||
mpuConfiguration.write = icm20689WriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_MPU6000
|
||||
if (mpu6000SpiDetect()) {
|
||||
mpuDetectionResult.sensor = MPU_60x0_SPI;
|
||||
|
|
|
@ -168,7 +168,8 @@ typedef enum {
|
|||
MPU_60x0_SPI,
|
||||
MPU_65xx_I2C,
|
||||
MPU_65xx_SPI,
|
||||
MPU_9250_SPI
|
||||
MPU_9250_SPI,
|
||||
ICM_20689_SPI
|
||||
} detectedMPUSensor_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
@ -0,0 +1,166 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "system.h"
|
||||
#include "exti.h"
|
||||
#include "io.h"
|
||||
#include "bus_spi.h"
|
||||
#include "gpio.h"
|
||||
#include "gyro_sync.h"
|
||||
|
||||
#include "sensor.h"
|
||||
#include "accgyro.h"
|
||||
#include "accgyro_mpu.h"
|
||||
#include "accgyro_spi_icm20689.h"
|
||||
|
||||
#define DISABLE_ICM20689 IOHi(icmSpi20689CsPin)
|
||||
#define ENABLE_ICM20689 IOLo(icmSpi20689CsPin)
|
||||
|
||||
static IO_t icmSpi20689CsPin = IO_NONE;
|
||||
|
||||
bool icm20689WriteRegister(uint8_t reg, uint8_t data)
|
||||
{
|
||||
ENABLE_ICM20689;
|
||||
spiTransferByte(ICM20689_SPI_INSTANCE, reg);
|
||||
spiTransferByte(ICM20689_SPI_INSTANCE, data);
|
||||
DISABLE_ICM20689;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||
{
|
||||
ENABLE_ICM20689;
|
||||
spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||
spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length);
|
||||
DISABLE_ICM20689;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void icm20689SpiInit(void)
|
||||
{
|
||||
static bool hardwareInitialised = false;
|
||||
|
||||
if (hardwareInitialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
icmSpi20689CsPin = IOGetByTag(IO_TAG(ICM20689_CS_PIN));
|
||||
IOInit(icmSpi20689CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
|
||||
IOConfigGPIO(icmSpi20689CsPin, SPI_IO_CS_CFG);
|
||||
|
||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||
|
||||
hardwareInitialised = true;
|
||||
}
|
||||
|
||||
bool icm20689SpiDetect(void)
|
||||
{
|
||||
uint8_t tmp;
|
||||
|
||||
icm20689SpiInit();
|
||||
|
||||
icm20689ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
|
||||
|
||||
if (tmp == ICM20689_WHO_AM_I_CONST) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool icm20689SpiAccDetect(acc_t *acc)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
|
||||
return false;
|
||||
}
|
||||
|
||||
acc->init = icm20689AccInit;
|
||||
acc->read = mpuAccRead;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool icm20689SpiGyroDetect(gyro_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
|
||||
return false;
|
||||
}
|
||||
|
||||
gyro->init = icm20689GyroInit;
|
||||
gyro->read = mpuGyroRead;
|
||||
gyro->intStatus = checkMPUDataReady;
|
||||
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = 1.0f / 16.4f;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void icm20689AccInit(acc_t *acc)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
|
||||
acc->acc_1G = 512 * 4;
|
||||
}
|
||||
|
||||
void icm20689GyroInit(uint8_t lpf)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
|
||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||
|
||||
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||
delay(100);
|
||||
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
||||
delay(100);
|
||||
// mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
|
||||
// delay(100);
|
||||
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
|
||||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
|
||||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
|
||||
delay(100);
|
||||
|
||||
// Data ready interrupt configuration
|
||||
// mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
|
||||
delay(15);
|
||||
|
||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||
mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
||||
#endif
|
||||
|
||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||
|
||||
}
|
|
@ -0,0 +1,34 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#define ICM20689_WHO_AM_I_CONST (0x98)
|
||||
#define ICM20689_BIT_RESET (0x80)
|
||||
|
||||
bool icm20689AccDetect(acc_t *acc);
|
||||
bool icm20689GyroDetect(gyro_t *gyro);
|
||||
|
||||
void icm20689AccInit(acc_t *acc);
|
||||
void icm20689GyroInit(uint8_t lpf);
|
||||
|
||||
bool icm20689SpiDetect(void);
|
||||
|
||||
bool icm20689SpiAccDetect(acc_t *acc);
|
||||
bool icm20689SpiGyroDetect(gyro_t *gyro);
|
||||
|
||||
bool icm20689WriteRegister(uint8_t reg, uint8_t data);
|
||||
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
|
@ -180,7 +180,6 @@ bool mpu6000SpiDetect(void)
|
|||
}
|
||||
} while (attemptsRemaining--);
|
||||
|
||||
|
||||
mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
|
||||
|
||||
/* look for a product ID we recognise */
|
||||
|
@ -244,7 +243,6 @@ static void mpu6000AccAndGyroInit(void)
|
|||
mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||
delayMicroseconds(15);
|
||||
|
||||
|
||||
mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
|
||||
delayMicroseconds(15);
|
||||
|
||||
|
|
|
@ -242,9 +242,9 @@ static const char * const sensorTypeNames[] = {
|
|||
|
||||
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
|
||||
|
||||
static const char * const sensorHardwareNames[4][12] = {
|
||||
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE", NULL },
|
||||
{ "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL },
|
||||
static const char * const sensorHardwareNames[4][13] = {
|
||||
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20689", "FAKE", NULL },
|
||||
{ "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20689", "FAKE", NULL },
|
||||
{ "", "None", "BMP085", "MS5611", "BMP280", NULL },
|
||||
{ "", "None", "HMC5883", "AK8975", "AK8963", NULL }
|
||||
};
|
||||
|
@ -454,6 +454,7 @@ static const char * const lookupTableAccHardware[] = {
|
|||
"MPU6000",
|
||||
"MPU6500",
|
||||
"MPU9250",
|
||||
"ICM20689",
|
||||
"FAKE"
|
||||
};
|
||||
|
||||
|
|
|
@ -28,7 +28,8 @@ typedef enum {
|
|||
ACC_LSM303DLHC = 6,
|
||||
ACC_MPU6000 = 7,
|
||||
ACC_MPU6500 = 8,
|
||||
ACC_FAKE = 9,
|
||||
ACC_ICM20689 = 9,
|
||||
ACC_FAKE = 10,
|
||||
ACC_MAX = ACC_FAKE
|
||||
} accelerationSensor_e;
|
||||
|
||||
|
|
|
@ -27,6 +27,7 @@ typedef enum {
|
|||
GYRO_MPU6000,
|
||||
GYRO_MPU6500,
|
||||
GYRO_MPU9250,
|
||||
GYRO_ICM20689,
|
||||
GYRO_FAKE,
|
||||
GYRO_MAX = GYRO_FAKE
|
||||
} gyroSensor_e;
|
||||
|
|
|
@ -255,6 +255,21 @@ bool detectGyro(void)
|
|||
}
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_ICM20689:
|
||||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
if (icm20689SpiGyroDetect(&gyro))
|
||||
{
|
||||
gyroHardware = GYRO_ICM20689;
|
||||
#ifdef GYRO_ICM20689_ALIGN
|
||||
gyroAlign = GYRO_ICM20689_ALIGN;
|
||||
#endif
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_FAKE:
|
||||
#ifdef USE_FAKE_GYRO
|
||||
if (fakeGyroDetect(&gyro)) {
|
||||
|
@ -382,6 +397,19 @@ retry:
|
|||
accHardware = ACC_MPU6500;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
; // fallthrough
|
||||
case ACC_ICM20689:
|
||||
#ifdef USE_ACC_SPI_ICM20689
|
||||
|
||||
if (icm20689SpiAccDetect(&acc))
|
||||
{
|
||||
#ifdef ACC_ICM20689_ALIGN
|
||||
accAlign = ACC_ICM20689_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_ICM20689;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
; // fallthrough
|
||||
case ACC_FAKE:
|
||||
|
@ -604,7 +632,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
|
|||
memset(&acc, 0, sizeof(acc));
|
||||
memset(&gyro, 0, sizeof(gyro));
|
||||
|
||||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250)
|
||||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
|
||||
|
||||
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
|
||||
|
||||
|
|
|
@ -33,12 +33,18 @@
|
|||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define ICM20689_CS_PIN PA4
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
|
||||
#define GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
|
@ -49,6 +55,9 @@
|
|||
#define GYRO_MPU6500_ALIGN CW90_DEG
|
||||
|
||||
#define ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW90_DEG
|
||||
|
|
|
@ -8,6 +8,7 @@ TARGET_SRC = \
|
|||
drivers/accgyro_spi_mpu6000.c \
|
||||
drivers/accgyro_mpu6500.c \
|
||||
drivers/accgyro_spi_mpu6500.c \
|
||||
drivers/accgyro_spi_icm20689.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/light_ws2811strip_stm32f30x.c \
|
||||
drivers/sonar_hcsr04.c \
|
||||
|
|
|
@ -37,12 +37,19 @@
|
|||
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready (mag disabled)
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define ICM20689_CS_PIN PA4
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
|
||||
#define GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
|
@ -52,6 +59,13 @@
|
|||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
|
||||
#define ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
|
|
|
@ -5,6 +5,7 @@ TARGET_SRC = \
|
|||
drivers/accgyro_spi_mpu6000.c \
|
||||
drivers/accgyro_spi_mpu6500.c \
|
||||
drivers/accgyro_mpu6500.c \
|
||||
drivers/accgyro_spi_icm20689.c \
|
||||
drivers/barometer_ms5611.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/light_ws2811strip_stm32f4xx.c
|
||||
|
|
Loading…
Reference in New Issue