rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ICM42688P - Initial sensor support based on ICM42605 driver.

This commit is contained in:
Dominic Clifton 2021-04-21 17:50:03 +02:00 committed by Dominic Clifton
parent 8ca4fdc586
commit 46c62cfcf2
11 changed files with 352 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/cli/settings.c
View File

@ -131,14 +131,14 @@
// sync with accelerationSensor_e
const char * const lookupTableAccHardware[] = {
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
"BMI160", "BMI270", "LSM6DSO", "FAKE"
};
// sync with gyroHardware_e
const char * const lookupTableGyroHardware[] = {
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
"BMI160", "BMI270", "LSM6SDO", "FAKE"
};

1
src/main/drivers/accgyro/accgyro.h
View File

@ -54,6 +54,7 @@ typedef enum {
GYRO_ICM20649,
GYRO_ICM20689,
GYRO_ICM42605,
GYRO_ICM42688P,
GYRO_BMI160,
GYRO_BMI270,
GYRO_LSM6DSO,

4
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -51,6 +51,7 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm42605.h"
#include "drivers/accgyro/accgyro_spi_icm42688p.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -239,6 +240,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_ICM42605
icm42605SpiDetect,
#endif
#ifdef USE_GYRO_SPI_ICM42688P
icm42688PSpiDetect,
#endif
#ifdef USE_GYRO_SPI_ICM20649
icm20649SpiDetect,
#endif

2
src/main/drivers/accgyro/accgyro_mpu.h
View File

@ -46,6 +46,7 @@
#define ICM20649_WHO_AM_I_CONST (0xE1)
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM42605_WHO_AM_I_CONST (0x42)
#define ICM42688P_WHO_AM_I_CONST (0x47)
// RA = Register Address
@ -200,6 +201,7 @@ typedef enum {
ICM_20649_SPI,
ICM_20689_SPI,
ICM_42605_SPI,
ICM_42688P_SPI,
BMI_160_SPI,
BMI_270_SPI,
LSM6DSO_SPI,

283
src/main/drivers/accgyro/accgyro_spi_icm42688p.c Normal file
View File

@ -0,0 +1,283 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
/*
* Author: Dominic Clifton
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include "platform.h"
#ifdef USE_GYRO_SPI_ICM42688P
#include "common/axis.h"
#include "common/maths.h"
#include "build/debug.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_spi_icm42688P.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
// 24 MHz max SPI frequency
#define ICM42688P_MAX_SPI_CLK_HZ 24000000
// 10 MHz max SPI frequency for intialisation
#define ICM42688P_MAX_SPI_INIT_CLK_HZ 1000000
#define ICM42688P_RA_PWR_MGMT0 0x4E
#define ICM42688P_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
#define ICM42688P_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
#define ICM42688P_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
#define ICM42688P_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
#define ICM42688P_RA_GYRO_CONFIG0 0x4F
#define ICM42688P_RA_ACCEL_CONFIG0 0x50
#define ICM42688P_RA_GYRO_ACCEL_CONFIG0 0x52
#define ICM42688P_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
#define ICM42688P_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
#define ICM42688P_RA_GYRO_DATA_X1 0x25
#define ICM42688P_RA_ACCEL_DATA_X1 0x1F
#define ICM42688P_RA_INT_CONFIG 0x14
#define ICM42688P_INT1_MODE_PULSED (0 << 2)
#define ICM42688P_INT1_MODE_LATCHED (1 << 2)
#define ICM42688P_INT1_DRIVE_CIRCUIT_OD (0 << 1)
#define ICM42688P_INT1_DRIVE_CIRCUIT_PP (1 << 1)
#define ICM42688P_INT1_POLARITY_ACTIVE_LOW (0 << 0)
#define ICM42688P_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
#define ICM42688P_RA_INT_CONFIG0 0x63
#define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
#define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (1 << 4)) // duplicate settings in datasheet, Rev 1.3.
#define ICM42688P_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
#define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
#define ICM42688P_RA_INT_CONFIG1 0x64
#define ICM42688P_INT_ASYNC_RESET_BIT 4
#define ICM42688P_INT_TDEASSERT_DISABLE_BIT 5
#define ICM42688P_INT_TDEASSERT_ENABLED (0 << ICM42688P_INT_TDEASSERT_DISABLE_BIT)
#define ICM42688P_INT_TDEASSERT_DISABLED (1 << ICM42688P_INT_TDEASSERT_DISABLE_BIT)
#define ICM42688P_INT_TPULSE_DURATION_BIT 6
#define ICM42688P_INT_TPULSE_DURATION_100 (0 << ICM42688P_INT_TPULSE_DURATION_BIT)
#define ICM42688P_INT_TPULSE_DURATION_8 (1 << ICM42688P_INT_TPULSE_DURATION_BIT)
#define ICM42688P_RA_INT_SOURCE0 0x65
#define ICM42688P_UI_DRDY_INT1_EN_DISABLED (0 << 3)
#define ICM42688P_UI_DRDY_INT1_EN_ENABLED (1 << 3)
static void icm42688PSpiInit(const extDevice_t *dev)
{
static bool hardwareInitialised = false;
if (hardwareInitialised) {
return;
}
spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
hardwareInitialised = true;
}
uint8_t icm42688PSpiDetect(const extDevice_t *dev)
{
icm42688PSpiInit(dev);
spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_INIT_CLK_HZ));
spiWriteReg(dev, ICM42688P_RA_PWR_MGMT0, 0x00);
uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20;
do {
delay(150);
const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
switch (whoAmI) {
case ICM42688P_WHO_AM_I_CONST:
icmDetected = ICM_42688P_SPI;
break;
default:
icmDetected = MPU_NONE;
break;
}
if (icmDetected != MPU_NONE) {
break;
}
if (!attemptsRemaining) {
return MPU_NONE;
}
} while (attemptsRemaining--);
spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
return icmDetected;
}
void icm42688PAccInit(accDev_t *acc)
{
acc->acc_1G = 512 * 4;
}
bool icm42688PAccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM42688P_RA_ACCEL_DATA_X1, data, 6);
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
return true;
}
bool icm42688PSpiAccDetect(accDev_t *acc)
{
switch (acc->mpuDetectionResult.sensor) {
case ICM_42688P_SPI:
break;
default:
return false;
}
acc->initFn = icm42688PAccInit;
acc->readFn = icm42688PAccRead;
return true;
}
typedef struct odrEntry_s {
uint8_t khz;
uint8_t odr; // See GYRO_ODR in datasheet.
} odrEntry_t;
static odrEntry_t icm42688PkhzToSupportedODRMap[] = {
{ 8, 3 },
{ 4, 4 },
{ 2, 5 },
{ 1, 6 },
};
void icm42688PGyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42688P_MAX_SPI_INIT_CLK_HZ));
spiWriteReg(&gyro->dev, ICM42688P_RA_PWR_MGMT0, ICM42688P_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42688P_PWR_MGMT0_ACCEL_MODE_LN | ICM42688P_PWR_MGMT0_GYRO_MODE_LN);
delay(15);
uint8_t outputDataRate = 0;
bool supportedODRFound = false;
if (gyro->gyroRateKHz) {
uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c
uint8_t desiredODRKhz = 8 / gyroSyncDenominator;
for (uint32_t i = 0; i < ARRAYLEN(icm42688PkhzToSupportedODRMap); i++) {
if (icm42688PkhzToSupportedODRMap[i].khz == desiredODRKhz) {
outputDataRate = icm42688PkhzToSupportedODRMap[i].odr;
supportedODRFound = true;
break;
}
}
}
if (!supportedODRFound) {
outputDataRate = 6;
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
}
STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
spiWriteReg(&gyro->dev, ICM42688P_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
delay(15);
STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
spiWriteReg(&gyro->dev, ICM42688P_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
delay(15);
spiWriteReg(&gyro->dev, ICM42688P_RA_GYRO_ACCEL_CONFIG0, ICM42688P_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42688P_GYRO_UI_FILT_BW_LOW_LATENCY);
spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG, ICM42688P_INT1_MODE_PULSED | ICM42688P_INT1_DRIVE_CIRCUIT_PP | ICM42688P_INT1_POLARITY_ACTIVE_HIGH);
spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG0, ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR);
#ifdef USE_MPU_DATA_READY_SIGNAL
spiWriteReg(&gyro->dev, ICM42688P_RA_INT_SOURCE0, ICM42688P_UI_DRDY_INT1_EN_ENABLED);
uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM42688P_RA_INT_CONFIG1);
// Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
intConfig1Value &= ~(1 << ICM42688P_INT_ASYNC_RESET_BIT);
intConfig1Value |= (ICM42688P_INT_TPULSE_DURATION_8 | ICM42688P_INT_TDEASSERT_DISABLED);
spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG1, intConfig1Value);
#endif
//
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
}
bool icm42688PGyroReadSPI(gyroDev_t *gyro)
{
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42688P_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
STATIC_DMA_DATA_AUTO uint8_t data[7];
const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
if (!ack) {
return false;
}
gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
return true;
}
bool icm42688PSpiGyroDetect(gyroDev_t *gyro)
{
switch (gyro->mpuDetectionResult.sensor) {
case ICM_42688P_SPI:
break;
default:
return false;
}
gyro->initFn = icm42688PGyroInit;
gyro->readFn = icm42688PGyroReadSPI;
gyro->scale = GYRO_SCALE_2000DPS;
return true;
}
#endif

34
src/main/drivers/accgyro/accgyro_spi_icm42688p.h Normal file
View File

@ -0,0 +1,34 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/bus.h"
bool icm42688PAccDetect(accDev_t *acc);
bool icm42688PGyroDetect(gyroDev_t *gyro);
void icm42688PAccInit(accDev_t *acc);
void icm42688PGyroInit(gyroDev_t *gyro);
uint8_t icm42688PSpiDetect(const extDevice_t *dev);
bool icm42688PSpiAccDetect(accDev_t *acc);
bool icm42688PSpiGyroDetect(gyroDev_t *gyro);

3
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View File

@ -77,6 +77,9 @@ uint8_t mpu6500SpiDetect(const extDevice_t *dev)
case ICM42605_WHO_AM_I_CONST:
mpuDetected = ICM_42605_SPI;
break;
case ICM42688P_WHO_AM_I_CONST:
mpuDetected = ICM_42688P_SPI;
break;
default:
mpuDetected = MPU_NONE;
}

1
src/main/sensors/acceleration.h
View File

@ -43,6 +43,7 @@ typedef enum {
ACC_ICM20649,
ACC_ICM20689,
ACC_ICM42605,
ACC_ICM42688P,
ACC_BMI160,
ACC_BMI270,
ACC_LSM6DSO,

10
src/main/sensors/acceleration_init.c
View File

@ -47,6 +47,7 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm42605.h"
#include "drivers/accgyro/accgyro_spi_icm42688p.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -277,6 +278,15 @@ retry:
FALLTHROUGH;
#endif
#ifdef USE_ACC_SPI_ICM42688P
case ACC_ICM42688P:
if (icm42688PSpiAccDetect(dev)) {
accHardware = ACC_ICM42688P;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_BMI160
case ACC_BMI160:
if (bmi160SpiAccDetect(dev)) {

12
src/main/sensors/gyro_init.c
View File

@ -46,6 +46,7 @@
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm42605.h"
#include "drivers/accgyro/accgyro_spi_icm42688p.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -475,6 +476,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
#ifdef USE_GYRO_SPI_ICM42688P
case GYRO_ICM42688P:
if (icm42688PSpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM42688P;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_BMI160
case GYRO_BMI160:
if (bmi160SpiGyroDetect(dev)) {
@ -527,7 +537,7 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
{
#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_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605)
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);

2
src/main/target/common_post.h
View File

@ -244,7 +244,7 @@
#define USE_I2C_GYRO
#endif
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605)
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
#define USE_SPI_GYRO
#endif