Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

ICM42688P - Combine with ICM42605 driver.

Browse source
This commit is contained in:
Dominic Clifton 2021-08-10 21:31:48 +02:00
parent a69aaff981
commit e69ed4f581
16 changed files with 130 additions and 440 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -50,8 +50,7 @@
#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_bmi270.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20649.h"
#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_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_icm42688p.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -237,11 +236,8 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_ACCGYRO_BMI270 #ifdef USE_ACCGYRO_BMI270
bmi270Detect, bmi270Detect,
#endif #endif
#ifdef USE_GYRO_SPI_ICM42605 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
icm42605SpiDetect, icm426xxSpiDetect,
#endif
#ifdef USE_GYRO_SPI_ICM42688P
icm42688PSpiDetect,
#endif #endif
#ifdef USE_GYRO_SPI_ICM20649 #ifdef USE_GYRO_SPI_ICM20649
icm20649SpiDetect, icm20649SpiDetect,

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

@ -46,7 +46,7 @@
#define ICM20649_WHO_AM_I_CONST (0xE1) #define ICM20649_WHO_AM_I_CONST (0xE1)
#define ICM20689_WHO_AM_I_CONST (0x98) #define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM42605_WHO_AM_I_CONST (0x42) #define ICM42605_WHO_AM_I_CONST (0x42)
#define ICM42688P_WHO_AM_I_CONST (0x47) #define ICM42688P_WHO_AM_I_CONST (0x47)
// RA = Register Address // RA = Register Address

286
src/main/drivers/accgyro/accgyro_spi_icm42688p.c
View file

@ -1,286 +0,0 @@
/*
* 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 ICM42605_WHO_AM_I_CONST:
icmDetected = ICM_42605_SPI;
break;
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
View file

@ -1,34 +0,0 @@
/*
* 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);

154
src/main/drivers/accgyro/accgyro_spi_icm42605.c → src/main/drivers/accgyro/accgyro_spi_icm426xx.c
View file

@ -28,7 +28,7 @@
#include "platform.h" #include "platform.h"
#ifdef USE_GYRO_SPI_ICM42605 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
@ -36,7 +36,7 @@
#include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_mpu.h" #include "drivers/accgyro/accgyro_mpu.h"
#include "drivers/accgyro/accgyro_spi_icm42605.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/bus_spi.h" #include "drivers/bus_spi.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/io.h" #include "drivers/io.h"
@ -44,58 +44,58 @@
#include "drivers/time.h" #include "drivers/time.h"
// 24 MHz max SPI frequency // 24 MHz max SPI frequency
#define ICM42605_MAX_SPI_CLK_HZ 24000000 #define ICM426XX_MAX_SPI_CLK_HZ 24000000
// 10 MHz max SPI frequency for intialisation // 10 MHz max SPI frequency for intialisation
#define ICM42605_MAX_SPI_INIT_CLK_HZ 1000000 #define ICM426XX_MAX_SPI_INIT_CLK_HZ 1000000
#define ICM42605_RA_PWR_MGMT0 0x4E #define ICM426XX_RA_PWR_MGMT0 0x4E
#define ICM42605_PWR_MGMT0_ACCEL_MODE_LN (3 << 0) #define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
#define ICM42605_PWR_MGMT0_GYRO_MODE_LN (3 << 2) #define ICM426XX_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
#define ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5) #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
#define ICM42605_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5) #define ICM426XX_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
#define ICM42605_RA_GYRO_CONFIG0 0x4F #define ICM426XX_RA_GYRO_CONFIG0 0x4F
#define ICM42605_RA_ACCEL_CONFIG0 0x50 #define ICM426XX_RA_ACCEL_CONFIG0 0x50
#define ICM42605_RA_GYRO_ACCEL_CONFIG0 0x52 #define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52
#define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4) #define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
#define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0) #define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
#define ICM42605_RA_GYRO_DATA_X1 0x25 #define ICM426XX_RA_GYRO_DATA_X1 0x25
#define ICM42605_RA_ACCEL_DATA_X1 0x1F #define ICM426XX_RA_ACCEL_DATA_X1 0x1F
#define ICM42605_RA_INT_CONFIG 0x14 #define ICM426XX_RA_INT_CONFIG 0x14
#define ICM42605_INT1_MODE_PULSED (0 << 2) #define ICM426XX_INT1_MODE_PULSED (0 << 2)
#define ICM42605_INT1_MODE_LATCHED (1 << 2) #define ICM426XX_INT1_MODE_LATCHED (1 << 2)
#define ICM42605_INT1_DRIVE_CIRCUIT_OD (0 << 1) #define ICM426XX_INT1_DRIVE_CIRCUIT_OD (0 << 1)
#define ICM42605_INT1_DRIVE_CIRCUIT_PP (1 << 1) #define ICM426XX_INT1_DRIVE_CIRCUIT_PP (1 << 1)
#define ICM42605_INT1_POLARITY_ACTIVE_LOW (0 << 0) #define ICM426XX_INT1_POLARITY_ACTIVE_LOW (0 << 0)
#define ICM42605_INT1_POLARITY_ACTIVE_HIGH (1 << 0) #define ICM426XX_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
#define ICM42605_RA_INT_CONFIG0 0x63 #define ICM426XX_RA_INT_CONFIG0 0x63
#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4)) #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2. #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
#define ICM42605_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4)) #define ICM426XX_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4)) #define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
#define ICM42605_RA_INT_CONFIG1 0x64 #define ICM426XX_RA_INT_CONFIG1 0x64
#define ICM42605_INT_ASYNC_RESET_BIT 4 #define ICM426XX_INT_ASYNC_RESET_BIT 4
#define ICM42605_INT_TDEASSERT_DISABLE_BIT 5 #define ICM426XX_INT_TDEASSERT_DISABLE_BIT 5
#define ICM42605_INT_TDEASSERT_ENABLED (0 << ICM42605_INT_TDEASSERT_DISABLE_BIT) #define ICM426XX_INT_TDEASSERT_ENABLED (0 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
#define ICM42605_INT_TDEASSERT_DISABLED (1 << ICM42605_INT_TDEASSERT_DISABLE_BIT) #define ICM426XX_INT_TDEASSERT_DISABLED (1 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
#define ICM42605_INT_TPULSE_DURATION_BIT 6 #define ICM426XX_INT_TPULSE_DURATION_BIT 6
#define ICM42605_INT_TPULSE_DURATION_100 (0 << ICM42605_INT_TPULSE_DURATION_BIT) #define ICM426XX_INT_TPULSE_DURATION_100 (0 << ICM426XX_INT_TPULSE_DURATION_BIT)
#define ICM42605_INT_TPULSE_DURATION_8 (1 << ICM42605_INT_TPULSE_DURATION_BIT) #define ICM426XX_INT_TPULSE_DURATION_8 (1 << ICM426XX_INT_TPULSE_DURATION_BIT)
#define ICM42605_RA_INT_SOURCE0 0x65 #define ICM426XX_RA_INT_SOURCE0 0x65
#define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3) #define ICM426XX_UI_DRDY_INT1_EN_DISABLED (0 << 3)
#define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3) #define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 3)
static void icm42605SpiInit(const extDevice_t *dev) static void icm426xxSpiInit(const extDevice_t *dev)
{ {
static bool hardwareInitialised = false; static bool hardwareInitialised = false;
@ -104,18 +104,18 @@ static void icm42605SpiInit(const extDevice_t *dev)
} }
spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
hardwareInitialised = true; hardwareInitialised = true;
} }
uint8_t icm42605SpiDetect(const extDevice_t *dev) uint8_t icm426xxSpiDetect(const extDevice_t *dev)
{ {
icm42605SpiInit(dev); icm426xxSpiInit(dev);
spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ)); spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_INIT_CLK_HZ));
spiWriteReg(dev, ICM42605_RA_PWR_MGMT0, 0x00); spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
uint8_t icmDetected = MPU_NONE; uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20; uint8_t attemptsRemaining = 20;
@ -141,21 +141,21 @@ uint8_t icm42605SpiDetect(const extDevice_t *dev)
} }
} while (attemptsRemaining--); } while (attemptsRemaining--);
spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
return icmDetected; return icmDetected;
} }
void icm42605AccInit(accDev_t *acc) void icm426xxAccInit(accDev_t *acc)
{ {
acc->acc_1G = 512 * 4; acc->acc_1G = 512 * 4;
} }
bool icm42605AccRead(accDev_t *acc) bool icm426xxAccRead(accDev_t *acc)
{ {
uint8_t data[6]; uint8_t data[6];
const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM42605_RA_ACCEL_DATA_X1, data, 6); const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM426XX_RA_ACCEL_DATA_X1, data, 6);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -166,17 +166,19 @@ bool icm42605AccRead(accDev_t *acc)
return true; return true;
} }
bool icm42605SpiAccDetect(accDev_t *acc) bool icm426xxSpiAccDetect(accDev_t *acc)
{ {
switch (acc->mpuDetectionResult.sensor) { switch (acc->mpuDetectionResult.sensor) {
case ICM_42605_SPI: case ICM_42605_SPI:
break; break;
case ICM_42688P_SPI:
break;
default: default:
return false; return false;
} }
acc->initFn = icm42605AccInit; acc->initFn = icm426xxAccInit;
acc->readFn = icm42605AccRead; acc->readFn = icm426xxAccRead;
return true; return true;
} }
@ -186,20 +188,20 @@ typedef struct odrEntry_s {
uint8_t odr; // See GYRO_ODR in datasheet. uint8_t odr; // See GYRO_ODR in datasheet.
} odrEntry_t; } odrEntry_t;
static odrEntry_t icm42605PkhzToSupportedODRMap[] = { static odrEntry_t icm426xxPkhzToSupportedODRMap[] = {
{ 8, 3 }, { 8, 3 },
{ 4, 4 }, { 4, 4 },
{ 2, 5 }, { 2, 5 },
{ 1, 6 }, { 1, 6 },
}; };
void icm42605GyroInit(gyroDev_t *gyro) void icm426xxGyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ)); spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM426XX_MAX_SPI_INIT_CLK_HZ));
spiWriteReg(&gyro->dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN); spiWriteReg(&gyro->dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_PWR_MGMT0_GYRO_MODE_LN);
delay(15); delay(15);
uint8_t outputDataRate = 0; uint8_t outputDataRate = 0;
@ -208,9 +210,9 @@ void icm42605GyroInit(gyroDev_t *gyro)
if (gyro->gyroRateKHz) { if (gyro->gyroRateKHz) {
uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c
uint8_t desiredODRKhz = 8 / gyroSyncDenominator; uint8_t desiredODRKhz = 8 / gyroSyncDenominator;
for (uint32_t i = 0; i < ARRAYLEN(icm42605PkhzToSupportedODRMap); i++) { for (uint32_t i = 0; i < ARRAYLEN(icm426xxPkhzToSupportedODRMap); i++) {
if (icm42605PkhzToSupportedODRMap[i].khz == desiredODRKhz) { if (icm426xxPkhzToSupportedODRMap[i].khz == desiredODRKhz) {
outputDataRate = icm42605PkhzToSupportedODRMap[i].odr; outputDataRate = icm426xxPkhzToSupportedODRMap[i].odr;
supportedODRFound = true; supportedODRFound = true;
break; break;
} }
@ -223,36 +225,36 @@ void icm42605GyroInit(gyroDev_t *gyro)
} }
STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value"); STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F)); spiWriteReg(&gyro->dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
delay(15); delay(15);
STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value"); STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
spiWriteReg(&gyro->dev, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F)); spiWriteReg(&gyro->dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
delay(15); delay(15);
spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY); spiWriteReg(&gyro->dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH); spiWriteReg(&gyro->dev, ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR); spiWriteReg(&gyro->dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
#ifdef USE_MPU_DATA_READY_SIGNAL #ifdef USE_MPU_DATA_READY_SIGNAL
spiWriteReg(&gyro->dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED); spiWriteReg(&gyro->dev, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM42605_RA_INT_CONFIG1); uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM426XX_RA_INT_CONFIG1);
// Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation" // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT); intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT);
intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED); intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG1, intConfig1Value); spiWriteReg(&gyro->dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
#endif #endif
// //
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ)); spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
} }
bool icm42605GyroReadSPI(gyroDev_t *gyro) bool icm426xxGyroReadSPI(gyroDev_t *gyro)
{ {
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM426XX_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
STATIC_DMA_DATA_AUTO uint8_t data[7]; STATIC_DMA_DATA_AUTO uint8_t data[7];
const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7); const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
@ -267,17 +269,19 @@ bool icm42605GyroReadSPI(gyroDev_t *gyro)
return true; return true;
} }
bool icm42605SpiGyroDetect(gyroDev_t *gyro) bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
{ {
switch (gyro->mpuDetectionResult.sensor) { switch (gyro->mpuDetectionResult.sensor) {
case ICM_42605_SPI: case ICM_42605_SPI:
break; break;
case ICM_42688P_SPI:
break;
default: default:
return false; return false;
} }
gyro->initFn = icm42605GyroInit; gyro->initFn = icm426xxGyroInit;
gyro->readFn = icm42605GyroReadSPI; gyro->readFn = icm426xxGyroReadSPI;
gyro->scale = GYRO_SCALE_2000DPS; gyro->scale = GYRO_SCALE_2000DPS;

14
src/main/drivers/accgyro/accgyro_spi_icm42605.h → src/main/drivers/accgyro/accgyro_spi_icm426xx.h
View file

@ -22,13 +22,13 @@
#include "drivers/bus.h" #include "drivers/bus.h"
bool icm42605AccDetect(accDev_t *acc); bool icm426xxAccDetect(accDev_t *acc);
bool icm42605GyroDetect(gyroDev_t *gyro); bool icm426xxGyroDetect(gyroDev_t *gyro);
void icm42605AccInit(accDev_t *acc); void icm426xxAccInit(accDev_t *acc);
void icm42605GyroInit(gyroDev_t *gyro); void icm426xxGyroInit(gyroDev_t *gyro);
uint8_t icm42605SpiDetect(const extDevice_t *dev); uint8_t icm426xxSpiDetect(const extDevice_t *dev);
bool icm42605SpiAccDetect(accDev_t *acc); bool icm426xxSpiAccDetect(accDev_t *acc);
bool icm42605SpiGyroDetect(gyroDev_t *gyro); bool icm426xxSpiGyroDetect(gyroDev_t *gyro);

27
src/main/sensors/acceleration_init.c
View file

@ -46,8 +46,7 @@
#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_bmi270.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20649.h"
#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_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_icm42688p.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -269,19 +268,21 @@ retry:
FALLTHROUGH; FALLTHROUGH;
#endif #endif
#ifdef USE_ACC_SPI_ICM42605 #if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P)
case ACC_ICM42605: case ACC_ICM42605:
if (icm42605SpiAccDetect(dev)) {
accHardware = ACC_ICM42605;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACC_SPI_ICM42688P
case ACC_ICM42688P: case ACC_ICM42688P:
if (icm42688PSpiAccDetect(dev)) { if (icm426xxSpiAccDetect(dev)) {
accHardware = ACC_ICM42688P; switch (dev->mpuDetectionResult.sensor) {
case ICM_42605_SPI:
accHardware = ACC_ICM42605;
break;
case ICM_42688P_SPI:
accHardware = ACC_ICM42688P;
break;
default:
accHardware = ACC_NONE;
break;
}
break; break;
} }
FALLTHROUGH; FALLTHROUGH;

27
src/main/sensors/gyro_init.c
View file

@ -45,8 +45,7 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm20689.h"
#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_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_icm42688p.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -467,19 +466,21 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH; FALLTHROUGH;
#endif #endif
#ifdef USE_GYRO_SPI_ICM42605 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
case GYRO_ICM42605: case GYRO_ICM42605:
if (icm42605SpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM42605;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_GYRO_SPI_ICM42688P
case GYRO_ICM42688P: case GYRO_ICM42688P:
if (icm42688PSpiGyroDetect(dev)) { if (icm426xxSpiGyroDetect(dev)) {
gyroHardware = GYRO_ICM42688P; switch (dev->mpuDetectionResult.sensor) {
case ICM_42605_SPI:
gyroHardware = GYRO_ICM42605;
break;
case ICM_42688P_SPI:
gyroHardware = GYRO_ICM42688P;
break;
default:
gyroHardware = GYRO_NONE;
break;
}
break; break;
} }
FALLTHROUGH; FALLTHROUGH;

2
src/main/target/NUCLEOH723ZG/target.h
View file

@ -202,6 +202,8 @@
#define USE_ACC_SPI_MPU9250 #define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605 #define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605 #define USE_ACC_SPI_ICM42605
#define USE_GYRO_SPI_ICM42688P
#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15 #define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1 #define GYRO_1_SPI_INSTANCE SPI1

2
src/main/target/NUCLEOH723ZG/target.mk
View file

@ -22,7 +22,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \ drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/accgyro/accgyro_spi_icm42605.c \ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp280.c \

2
src/main/target/NUCLEOH725ZG/target.h
View file

@ -211,6 +211,8 @@
#define USE_ACC_SPI_MPU9250 #define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605 #define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605 #define USE_ACC_SPI_ICM42605
#define USE_GYRO_SPI_ICM42688P
#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15 #define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1 #define GYRO_1_SPI_INSTANCE SPI1

2
src/main/target/NUCLEOH725ZG/target.mk
View file

@ -22,7 +22,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \ drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/accgyro/accgyro_spi_icm42605.c \ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp280.c \

2
src/main/target/NUCLEOH743/target.h
View file

@ -211,6 +211,8 @@
#define USE_ACC_SPI_MPU9250 #define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605 #define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605 #define USE_ACC_SPI_ICM42605
#define USE_GYRO_SPI_ICM42688P
#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15 #define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1 #define GYRO_1_SPI_INSTANCE SPI1

2
src/main/target/NUCLEOH743/target.mk
View file

@ -25,7 +25,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \ drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/accgyro/accgyro_spi_icm42605.c \ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp280.c \

2
src/main/target/NUCLEOH7A3ZI/target.h
View file

@ -202,6 +202,8 @@
#define USE_ACC_SPI_MPU9250 #define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605 #define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605 #define USE_ACC_SPI_ICM42605
#define USE_GYRO_SPI_ICM42688P
#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15 #define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1 #define GYRO_1_SPI_INSTANCE SPI1

2
src/main/target/NUCLEOH7A3ZI/target.mk
View file

@ -25,7 +25,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \ drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/accgyro/accgyro_spi_icm42605.c \ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp280.c \