1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 03:20:00 +03:00

Merge branch 'master' into RP2350

This commit is contained in:
blckmn 2025-06-12 04:52:17 +10:00
commit ab65694b57
29 changed files with 249 additions and 194 deletions

@ -1 +1 @@
Subproject commit 737c3460fa621696af079367c86ca2be28194631 Subproject commit ec429513a68b3362647ead039719f12f156edcf4

View file

@ -164,6 +164,7 @@ const char * const lookupTableAccHardware[] = {
"ICM45605", "ICM45605",
"ICM45686", "ICM45686",
"ICM40609D", "ICM40609D",
"IIM42652",
"VIRTUAL" "VIRTUAL"
}; };
@ -191,6 +192,7 @@ const char * const lookupTableGyroHardware[] = {
"ICM45605", "ICM45605",
"ICM45686", "ICM45686",
"ICM40609D", "ICM40609D",
"IIM42652",
"VIRTUAL" "VIRTUAL"
}; };

View file

@ -65,6 +65,7 @@ typedef enum {
GYRO_ICM45605, GYRO_ICM45605,
GYRO_ICM45686, GYRO_ICM45686,
GYRO_ICM40609D, GYRO_ICM40609D,
GYRO_IIM42652,
GYRO_VIRTUAL GYRO_VIRTUAL
} gyroHardware_e; } gyroHardware_e;

View file

@ -47,6 +47,7 @@
#define ICM42688P_WHO_AM_I_CONST (0x47) #define ICM42688P_WHO_AM_I_CONST (0x47)
#define ICM45686_WHO_AM_I_CONST (0xE9) #define ICM45686_WHO_AM_I_CONST (0xE9)
#define ICM45605_WHO_AM_I_CONST (0xE5) #define ICM45605_WHO_AM_I_CONST (0xE5)
#define IIM42652_WHO_AM_I_CONST (0x6F)
#define IIM42653_WHO_AM_I_CONST (0x56) #define IIM42653_WHO_AM_I_CONST (0x56)
#define LSM6DSV16X_WHO_AM_I_CONST (0x70) #define LSM6DSV16X_WHO_AM_I_CONST (0x70)
#define ICM40609_WHO_AM_I_CONST (0x3B) #define ICM40609_WHO_AM_I_CONST (0x3B)
@ -207,6 +208,7 @@ typedef enum {
ICM_20689_SPI, ICM_20689_SPI,
ICM_42605_SPI, ICM_42605_SPI,
ICM_42688P_SPI, ICM_42688P_SPI,
IIM_42652_SPI,
IIM_42653_SPI, IIM_42653_SPI,
BMI_160_SPI, BMI_160_SPI,
BMI_270_SPI, BMI_270_SPI,

View file

@ -28,7 +28,7 @@
#include "platform.h" #include "platform.h"
#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653) #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42652) || defined(USE_ACCGYRO_IIM42653)
#include "common/axis.h" #include "common/axis.h"
#include "common/utils.h" #include "common/utils.h"
@ -284,6 +284,9 @@ uint8_t icm426xxSpiDetect(const extDevice_t *dev)
case ICM42688P_WHO_AM_I_CONST: case ICM42688P_WHO_AM_I_CONST:
icmDetected = ICM_42688P_SPI; icmDetected = ICM_42688P_SPI;
break; break;
case IIM42652_WHO_AM_I_CONST:
icmDetected = IIM_42652_SPI;
break;
case IIM42653_WHO_AM_I_CONST: case IIM42653_WHO_AM_I_CONST:
icmDetected = IIM_42653_SPI; icmDetected = IIM_42653_SPI;
break; break;
@ -306,6 +309,7 @@ void icm426xxAccInit(accDev_t *acc)
{ {
switch (acc->mpuDetectionResult.sensor) { switch (acc->mpuDetectionResult.sensor) {
case IIM_42653_SPI: case IIM_42653_SPI:
case IIM_42652_SPI:
acc->acc_1G = 512 * 2; // Accel scale 32g (1024 LSB/g) acc->acc_1G = 512 * 2; // Accel scale 32g (1024 LSB/g)
break; break;
default: default:
@ -319,6 +323,7 @@ bool icm426xxSpiAccDetect(accDev_t *acc)
switch (acc->mpuDetectionResult.sensor) { switch (acc->mpuDetectionResult.sensor) {
case ICM_42605_SPI: case ICM_42605_SPI:
case ICM_42688P_SPI: case ICM_42688P_SPI:
case IIM_42652_SPI:
case IIM_42653_SPI: case IIM_42653_SPI:
break; break;
default: default:
@ -427,6 +432,7 @@ bool icm426xxSpiGyroDetect(gyroDev_t *gyro)
case ICM_42688P_SPI: case ICM_42688P_SPI:
gyro->scale = GYRO_SCALE_2000DPS; gyro->scale = GYRO_SCALE_2000DPS;
break; break;
case IIM_42652_SPI:
case IIM_42653_SPI: case IIM_42653_SPI:
gyro->scale = GYRO_SCALE_4000DPS; gyro->scale = GYRO_SCALE_4000DPS;
break; break;
@ -444,6 +450,8 @@ static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig
{ {
switch (gyroModel){ switch (gyroModel){
case ICM_42605_SPI: case ICM_42605_SPI:
case IIM_42652_SPI:
case IIM_42653_SPI:
switch (config) { switch (config) {
case GYRO_HARDWARE_LPF_NORMAL: case GYRO_HARDWARE_LPF_NORMAL:
return aafLUT42605[AAF_CONFIG_258HZ]; return aafLUT42605[AAF_CONFIG_258HZ];
@ -456,7 +464,6 @@ static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig
} }
case ICM_42688P_SPI: case ICM_42688P_SPI:
case IIM_42653_SPI:
default: default:
switch (config) { switch (config) {
case GYRO_HARDWARE_LPF_NORMAL: case GYRO_HARDWARE_LPF_NORMAL:
@ -475,4 +482,4 @@ static aafConfig_t getGyroAafConfig(const mpuSensor_e gyroModel, const aafConfig
} }
} }
#endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P || USE_ACCGYRO_IIM42653 #endif // USE_GYRO_SPI_ICM42605 || USE_GYRO_SPI_ICM42688P || USE_ACCGYRO_IIM42652 || USE_ACCGYRO_IIM42653

View file

@ -51,7 +51,7 @@ typedef enum I2CDevice {
#define I2C_ADDR7_MAX 119 #define I2C_ADDR7_MAX 119
struct i2cConfig_s; struct i2cConfig_s;
void i2cHardwareConfigure(const struct i2cConfig_s *i2cConfig); void i2cPinConfigure(const struct i2cConfig_s *i2cConfig);
void i2cInit(I2CDevice device); void i2cInit(I2CDevice device);
bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data); bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data);

View file

@ -624,7 +624,7 @@ void init(void)
#endif #endif
#ifdef USE_I2C #ifdef USE_I2C
i2cHardwareConfigure(i2cConfig(0)); i2cPinConfigure(i2cConfig(0));
// Note: Unlike UARTs which are configured when client is present, // Note: Unlike UARTs which are configured when client is present,
// I2C buses are initialized unconditionally if they are configured. // I2C buses are initialized unconditionally if they are configured.

View file

@ -54,6 +54,7 @@ typedef enum {
ACC_ICM45605, ACC_ICM45605,
ACC_ICM45686, ACC_ICM45686,
ACC_ICM40609D, ACC_ICM40609D,
ACC_IIM42652,
ACC_VIRTUAL ACC_VIRTUAL
} accelerationSensor_e; } accelerationSensor_e;

View file

@ -218,9 +218,10 @@ retry:
FALLTHROUGH; FALLTHROUGH;
#endif #endif
#if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653) #if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42652) || defined(USE_ACCGYRO_IIM42653)
case ACC_ICM42605: case ACC_ICM42605:
case ACC_ICM42688P: case ACC_ICM42688P:
case ACC_IIM42652:
case ACC_IIM42653: case ACC_IIM42653:
if (icm426xxSpiAccDetect(dev)) { if (icm426xxSpiAccDetect(dev)) {
switch (dev->mpuDetectionResult.sensor) { switch (dev->mpuDetectionResult.sensor) {
@ -230,6 +231,9 @@ retry:
case ICM_42688P_SPI: case ICM_42688P_SPI:
accHardware = ACC_ICM42688P; accHardware = ACC_ICM42688P;
break; break;
case IIM_42652_SPI:
accHardware = ACC_IIM42652;
break;
case IIM_42653_SPI: case IIM_42653_SPI:
accHardware = ACC_IIM42653; accHardware = ACC_IIM42653;
break; break;

View file

@ -318,6 +318,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
case GYRO_LSM6DSO: case GYRO_LSM6DSO:
case GYRO_LSM6DSV16X: case GYRO_LSM6DSV16X:
case GYRO_ICM42688P: case GYRO_ICM42688P:
case GYRO_IIM42652:
case GYRO_IIM42653: case GYRO_IIM42653:
case GYRO_ICM42605: case GYRO_ICM42605:
case GYRO_ICM45686: case GYRO_ICM45686:
@ -434,9 +435,10 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH; FALLTHROUGH;
#endif #endif
#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42653) #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_IIM42652) || defined(USE_ACCGYRO_IIM42653)
case GYRO_ICM42605: case GYRO_ICM42605:
case GYRO_ICM42688P: case GYRO_ICM42688P:
case GYRO_IIM42652:
case GYRO_IIM42653: case GYRO_IIM42653:
if (icm426xxSpiGyroDetect(dev)) { if (icm426xxSpiGyroDetect(dev)) {
switch (dev->mpuDetectionResult.sensor) { switch (dev->mpuDetectionResult.sensor) {
@ -446,6 +448,9 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
case ICM_42688P_SPI: case ICM_42688P_SPI:
gyroHardware = GYRO_ICM42688P; gyroHardware = GYRO_ICM42688P;
break; break;
case IIM_42652_SPI:
gyroHardware = GYRO_IIM42652;
break;
case IIM_42653_SPI: case IIM_42653_SPI:
gyroHardware = GYRO_IIM42653; gyroHardware = GYRO_IIM42653;
break; break;

View file

@ -114,8 +114,9 @@
&& !defined(USE_ACC_SPI_MPU6000) \ && !defined(USE_ACC_SPI_MPU6000) \
&& !defined(USE_ACC_SPI_MPU6500) \ && !defined(USE_ACC_SPI_MPU6500) \
&& !defined(USE_ACC_SPI_MPU9250) \ && !defined(USE_ACC_SPI_MPU9250) \
&& !defined(USE_VIRTUAL_ACC) \ && !defined(USE_ACCGYRO_IIM42652) \
&& !defined(USE_ACCGYRO_IIM42653) && !defined(USE_ACCGYRO_IIM42653) \
&& !defined(USE_VIRTUAL_ACC)
#error At least one USE_ACC device definition required #error At least one USE_ACC device definition required
#endif #endif
@ -137,8 +138,9 @@
&& !defined(USE_GYRO_SPI_MPU6000) \ && !defined(USE_GYRO_SPI_MPU6000) \
&& !defined(USE_GYRO_SPI_MPU6500) \ && !defined(USE_GYRO_SPI_MPU6500) \
&& !defined(USE_GYRO_SPI_MPU9250) \ && !defined(USE_GYRO_SPI_MPU9250) \
&& !defined(USE_VIRTUAL_GYRO) \ && !defined(USE_ACCGYRO_IIM42652) \
&& !defined(USE_ACCGYRO_IIM42653) && !defined(USE_ACCGYRO_IIM42653) \
&& !defined(USE_VIRTUAL_GYRO)
#error At least one USE_GYRO device definition required #error At least one USE_GYRO device definition required
#endif #endif
@ -476,11 +478,11 @@
#define USE_GYRO_SPI_MPU6500 #define USE_GYRO_SPI_MPU6500
#endif #endif
// Generate USE_SPI_GYRO or USE_I2C_GYRO // Generate USE_SPI_GYRO
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) \ #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) || defined(USE_ACCGYRO_ICM45686) \ || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_ICM45686) \
|| defined(USE_ACCGYRO_ICM45605) || defined(USE_ACCGYRO_IIM42653) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \ || defined(USE_ACCGYRO_ICM45605) || defined(USE_ACCGYRO_IIM42653) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \
|| defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_ACCGYRO_ICM40609D) || defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_ACCGYRO_ICM40609D) || defined(USE_ACCGYRO_IIM42652)
#ifndef USE_SPI_GYRO #ifndef USE_SPI_GYRO
#define USE_SPI_GYRO #define USE_SPI_GYRO
#endif #endif
@ -602,7 +604,7 @@
#endif #endif
#endif // USE_OPTICALFLOW_MT #endif // USE_OPTICALFLOW_MT
#if defined(USE_RANGEFINDER_HCSR04) || defined(USE_RANGEFINDER_SRF10) || defined(USE_RANGEFINDER_HCSR04_I2C) || defined(USE_RANGEFINDER_VL53L0X) || defined(USE_RANGEFINDER_UIB) || defined(USE_RANGEFINDER_TF) || defined(USE_RANGEFINDER_MT) #if defined(USE_RANGEFINDER_HCSR04) || defined(USE_RANGEFINDER_TF) || defined(USE_RANGEFINDER_MT)
#ifndef USE_RANGEFINDER #ifndef USE_RANGEFINDER
#define USE_RANGEFINDER #define USE_RANGEFINDER
#endif #endif

View file

@ -116,6 +116,7 @@
#define USE_GYRO_SPI_ICM42688P #define USE_GYRO_SPI_ICM42688P
#define USE_ACCGYRO_ICM45686 #define USE_ACCGYRO_ICM45686
#define USE_ACCGYRO_ICM45605 #define USE_ACCGYRO_ICM45605
#define USE_ACCGYRO_IIM42652
#define USE_ACCGYRO_IIM42653 #define USE_ACCGYRO_IIM42653
#define USE_ACC_SPI_ICM42605 #define USE_ACC_SPI_ICM42605
#define USE_ACC_SPI_ICM42688P #define USE_ACC_SPI_ICM42688P

View file

@ -270,8 +270,7 @@ void adcInit(const adcConfig_t *config)
adcInitDevice(&adcInternal, 2); adcInitDevice(&adcInternal, 2);
DDL_ADC_Enable(adcInternal.ADCx); DDL_ADC_Enable(adcInternal.ADCx);
adcInitInternalInjected(&adcInternal); adcInitInternalInjected(&adcInternal);
} } else {
else {
// Initialize for injected conversion // Initialize for injected conversion
adcInitInternalInjected(&adc); adcInitInternalInjected(&adc);
} }

View file

@ -31,10 +31,12 @@ __STATIC_INLINE void DDL_EX_TMR_DisableIT(TMR_TypeDef *TMRx, uint32_t Sources)
{ {
CLEAR_BIT(TMRx->DIEN, Sources); CLEAR_BIT(TMRx->DIEN, Sources);
} }
__STATIC_INLINE void DDL_EX_TMR_EnableIT(TMR_TypeDef *TMRx, uint32_t Sources) __STATIC_INLINE void DDL_EX_TMR_EnableIT(TMR_TypeDef *TMRx, uint32_t Sources)
{ {
SET_BIT(TMRx->DIEN, Sources); SET_BIT(TMRx->DIEN, Sources);
} }
__STATIC_INLINE uint32_t DDL_EX_DMA_Stream_to_Stream(DMA_Stream_TypeDef *DMAx_Streamy) __STATIC_INLINE uint32_t DDL_EX_DMA_Stream_to_Stream(DMA_Stream_TypeDef *DMAx_Streamy)
{ {
STATIC_ASSERT(DMA1_Stream0_BASE - DMA1_BASE == sizeof(DMA_TypeDef), DMA_TypeDef_has_padding); STATIC_ASSERT(DMA1_Stream0_BASE - DMA1_BASE == sizeof(DMA_TypeDef), DMA_TypeDef_has_padding);
@ -45,11 +47,13 @@ __STATIC_INLINE uint32_t DDL_EX_DMA_Stream_to_Stream(DMA_Stream_TypeDef *DMAx_St
return (((uint32_t) DMAx_Streamy & 0XFF) - firstStreamOffset) / streamSize; return (((uint32_t) DMAx_Streamy & 0XFF) - firstStreamOffset) / streamSize;
} }
__STATIC_INLINE DMA_TypeDef *DDL_EX_DMA_Stream_to_DMA(DMA_Stream_TypeDef *DMAx_Streamy) __STATIC_INLINE DMA_TypeDef *DDL_EX_DMA_Stream_to_DMA(DMA_Stream_TypeDef *DMAx_Streamy)
{ {
// clear stream address // clear stream address
return (DMA_TypeDef *) (((uint32_t) DMAx_Streamy) & ((uint32_t) ~0XFF)); return (DMA_TypeDef *) (((uint32_t) DMAx_Streamy) & ((uint32_t) ~0XFF));
} }
__STATIC_INLINE uint32_t DDL_EX_DMA_DeInit(DMA_Stream_TypeDef *DMAx_Streamy) __STATIC_INLINE uint32_t DDL_EX_DMA_DeInit(DMA_Stream_TypeDef *DMAx_Streamy)
{ {
DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy); DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy);
@ -57,6 +61,7 @@ __STATIC_INLINE uint32_t DDL_EX_DMA_DeInit(DMA_Stream_TypeDef *DMAx_Streamy)
return DDL_DMA_DeInit(DMA, Stream); return DDL_DMA_DeInit(DMA, Stream);
} }
__STATIC_INLINE uint32_t DDL_EX_DMA_Init(DMA_Stream_TypeDef *DMAx_Streamy, DDL_DMA_InitTypeDef *DMA_InitStruct) __STATIC_INLINE uint32_t DDL_EX_DMA_Init(DMA_Stream_TypeDef *DMAx_Streamy, DDL_DMA_InitTypeDef *DMA_InitStruct)
{ {
DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy); DMA_TypeDef *DMA = DDL_EX_DMA_Stream_to_DMA(DMAx_Streamy);
@ -64,6 +69,7 @@ __STATIC_INLINE uint32_t DDL_EX_DMA_Init(DMA_Stream_TypeDef *DMAx_Streamy, DDL_D
return DDL_DMA_Init(DMA, Stream, DMA_InitStruct); return DDL_DMA_Init(DMA, Stream, DMA_InitStruct);
} }
__STATIC_INLINE void DDL_EX_DMA_SetDataLength(DMA_Stream_TypeDef* DMAx_Streamy, uint32_t NbData) __STATIC_INLINE void DDL_EX_DMA_SetDataLength(DMA_Stream_TypeDef* DMAx_Streamy, uint32_t NbData)
{ {
MODIFY_REG(DMAx_Streamy->NDATA, DMA_NDATAx, NbData); MODIFY_REG(DMAx_Streamy->NDATA, DMA_NDATAx, NbData);
@ -81,6 +87,7 @@ __STATIC_INLINE void DDL_EX_DMA_EnableResource(DMA_Stream_TypeDef *DMAx_Streamy)
{ {
SET_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_EN); SET_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_EN);
} }
__STATIC_INLINE void DDL_EX_DMA_DisableResource(DMA_Stream_TypeDef *DMAx_Streamy) __STATIC_INLINE void DDL_EX_DMA_DisableResource(DMA_Stream_TypeDef *DMAx_Streamy)
{ {
CLEAR_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_EN); CLEAR_BIT(DMAx_Streamy->SCFG, DMA_SCFGx_EN);

View file

@ -76,7 +76,7 @@ static volatile uint16_t i2cErrorCount = 0;
static bool i2cHandleHardwareFailure(I2CDevice device) static bool i2cHandleHardwareFailure(I2CDevice device)
{ {
(void)device; UNUSED(device);
i2cErrorCount++; i2cErrorCount++;
// reinit peripheral + clock out garbage // reinit peripheral + clock out garbage
//i2cInit(device); //i2cInit(device);
@ -103,13 +103,15 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
HAL_StatusTypeDef status; HAL_StatusTypeDef status;
if (reg_ == 0xFF) if (reg_ == 0xFF) {
status = DAL_I2C_Master_Transmit(pHandle ,addr_ << 1, &data, 1, I2C_TIMEOUT_SYS_TICKS); status = DAL_I2C_Master_Transmit(pHandle ,addr_ << 1, &data, 1, I2C_TIMEOUT_SYS_TICKS);
else } else {
status = DAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, &data, 1, I2C_TIMEOUT_SYS_TICKS); status = DAL_I2C_Mem_Write(pHandle ,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT, &data, 1, I2C_TIMEOUT_SYS_TICKS);
}
if (status != DAL_OK) if (status != DAL_OK) {
return i2cHandleHardwareFailure(device); return i2cHandleHardwareFailure(device);
}
return true; return true;
} }
@ -135,8 +137,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_,
return false; return false;
} }
if (status != DAL_OK) if (status != DAL_OK) {
{
return i2cHandleHardwareFailure(device); return i2cHandleHardwareFailure(device);
} }
@ -158,10 +159,11 @@ bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t
DAL_StatusTypeDef status; DAL_StatusTypeDef status;
if (reg_ == 0xFF) if (reg_ == 0xFF) {
status = DAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_TIMEOUT_SYS_TICKS); status = DAL_I2C_Master_Receive(pHandle ,addr_ << 1, buf, len, I2C_TIMEOUT_SYS_TICKS);
else } else {
status = DAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_TIMEOUT_SYS_TICKS); status = DAL_I2C_Mem_Read(pHandle, addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_TIMEOUT_SYS_TICKS);
}
if (status != DAL_OK) { if (status != DAL_OK) {
return i2cHandleHardwareFailure(device); return i2cHandleHardwareFailure(device);
@ -206,8 +208,7 @@ bool i2cBusy(I2CDevice device, bool *error)
*error = pHandle->ErrorCode; *error = pHandle->ErrorCode;
} }
if (pHandle->State == DAL_I2C_STATE_READY) if (pHandle->State == DAL_I2C_STATE_READY) {
{
if (__DAL_I2C_GET_FLAG(pHandle, I2C_FLAG_BUSY) == SET) if (__DAL_I2C_GET_FLAG(pHandle, I2C_FLAG_BUSY) == SET)
{ {
return true; return true;
@ -218,5 +219,4 @@ bool i2cBusy(I2CDevice device, bool *error)
return true; return true;
} }
#endif #endif

View file

@ -347,8 +347,7 @@ FAST_CODE void spiSequenceStart(const extDevice_t *dev)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
DDL_SPI_SetClockPhase(instance, DDL_SPI_PHASE_1EDGE); DDL_SPI_SetClockPhase(instance, DDL_SPI_PHASE_1EDGE);
DDL_SPI_SetClockPolarity(instance, DDL_SPI_POLARITY_LOW); DDL_SPI_SetClockPolarity(instance, DDL_SPI_POLARITY_LOW);
} } else {
else {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF); IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
DDL_SPI_SetClockPhase(instance, DDL_SPI_PHASE_2EDGE); DDL_SPI_SetClockPhase(instance, DDL_SPI_PHASE_2EDGE);
DDL_SPI_SetClockPolarity(instance, DDL_SPI_POLARITY_HIGH); DDL_SPI_SetClockPolarity(instance, DDL_SPI_POLARITY_HIGH);

View file

@ -450,14 +450,15 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorProtocolTypes_e pwmP
bbGpioSetup(&bbMotors[motorIndex]); bbGpioSetup(&bbMotors[motorIndex]);
do {
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) { if (useDshotTelemetry) {
bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_INVERTED); bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_INVERTED);
} else break;
#endif
{
bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_NONINVERTED);
} }
#endif
bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_NONINVERTED);
} while (false);
bbSwitchToOutput(bbPort); bbSwitchToOutput(bbPort);
@ -577,14 +578,15 @@ static void bbWriteInt(uint8_t motorIndex, uint16_t value)
bbPort_t *bbPort = bbmotor->bbPort; bbPort_t *bbPort = bbmotor->bbPort;
do {
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) { if (useDshotTelemetry) {
bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_INVERTED); bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_INVERTED);
} else break;
}
#endif #endif
{
bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_NONINVERTED); bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_NONINVERTED);
} } while (false);
} }
static void bbWrite(uint8_t motorIndex, float value) static void bbWrite(uint8_t motorIndex, float value)
@ -614,11 +616,8 @@ static void bbUpdateComplete(void)
bbPort->inputActive = false; bbPort->inputActive = false;
bbSwitchToOutput(bbPort); bbSwitchToOutput(bbPort);
} }
} else
#endif
{
// Nothing to do
} }
#endif
bbDMA_Cmd(bbPort, ENABLE); bbDMA_Cmd(bbPort, ENABLE);
} }

View file

@ -56,23 +56,12 @@ void bbGpioSetup(bbMotor_t *bbMotor)
bbPort->gpioModeInput |= (DDL_GPIO_MODE_INPUT << (pinIndex * 2)); bbPort->gpioModeInput |= (DDL_GPIO_MODE_INPUT << (pinIndex * 2));
bbPort->gpioModeOutput |= (DDL_GPIO_MODE_OUTPUT << (pinIndex * 2)); bbPort->gpioModeOutput |= (DDL_GPIO_MODE_OUTPUT << (pinIndex * 2));
bool inverted = false;
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) { inverted = true;
bbPort->gpioIdleBSRR |= (1 << pinIndex); // BS (lower half)
} else
#endif #endif
{ bbPort->gpioIdleBSRR |= (1 << pinIndex) + (inverted ? 0 : 16); // write BITSET or BITRESET half of BSRR
bbPort->gpioIdleBSRR |= (1 << (pinIndex + 16)); // BR (higher half) IOWrite(bbMotor->io, inverted);
}
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
IOWrite(bbMotor->io, 1);
} else
#endif
{
IOWrite(bbMotor->io, 0);
}
} }
void bbTimerChannelInit(bbPort_t *bbPort) void bbTimerChannelInit(bbPort_t *bbPort)

View file

@ -180,7 +180,7 @@ MCU_COMMON_SRC = \
APM32/serial_uart_apm32f4xx.c \ APM32/serial_uart_apm32f4xx.c \
drivers/adc.c \ drivers/adc.c \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_config.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_hw.c \ common/stm32/bus_spi_hw.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
common/stm32/serial_uart_hw.c \ common/stm32/serial_uart_hw.c \
@ -218,7 +218,7 @@ SIZE_OPTIMISED_SRC += \
APM32/usb/vcp/serial_usb_vcp.c \ APM32/usb/vcp/serial_usb_vcp.c \
drivers/inverter.c \ drivers/inverter.c \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_config.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
drivers/serial_escserial.c \ drivers/serial_escserial.c \
drivers/serial_pinconfig.c \ drivers/serial_pinconfig.c \

View file

@ -133,18 +133,19 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(void)
} }
for (int i = 0; i < dmaMotorTimerCount; i++) { for (int i = 0; i < dmaMotorTimerCount; i++) {
do {
#ifdef USE_DSHOT_DMAR #ifdef USE_DSHOT_DMAR
if (useBurstDshot) { if (useBurstDshot) {
xDDL_EX_DMA_SetDataLength(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength); xDDL_EX_DMA_SetDataLength(dmaMotorTimers[i].dmaBurstRef, dmaMotorTimers[i].dmaBurstLength);
xDDL_EX_DMA_EnableResource(dmaMotorTimers[i].dmaBurstRef); xDDL_EX_DMA_EnableResource(dmaMotorTimers[i].dmaBurstRef);
/* configure the DMA Burst Mode */ /* configure the DMA Burst Mode */
DDL_TMR_ConfigDMABurst(dmaMotorTimers[i].timer, DDL_TMR_DMABURST_BASEADDR_CC1, DDL_TMR_DMABURST_LENGTH_4TRANSFERS); DDL_TMR_ConfigDMABurst(dmaMotorTimers[i].timer, DDL_TMR_DMABURST_BASEADDR_CC1, DDL_TMR_DMABURST_LENGTH_4TRANSFERS);
/* Enable the TIM DMA Request */ /* Enable the TIM DMA Request */
DDL_TMR_EnableDMAReq_UPDATE(dmaMotorTimers[i].timer); DDL_TMR_EnableDMAReq_UPDATE(dmaMotorTimers[i].timer);
} else break;
}
#endif #endif
{
DDL_TMR_DisableARRPreload(dmaMotorTimers[i].timer); DDL_TMR_DisableARRPreload(dmaMotorTimers[i].timer);
dmaMotorTimers[i].timer->AUTORLD = dmaMotorTimers[i].outputPeriod; dmaMotorTimers[i].timer->AUTORLD = dmaMotorTimers[i].outputPeriod;
@ -153,7 +154,7 @@ FAST_CODE void pwmCompleteDshotMotorUpdate(void)
/* Enable channel DMA requests */ /* Enable channel DMA requests */
DDL_EX_TMR_EnableIT(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources); DDL_EX_TMR_EnableIT(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources);
dmaMotorTimers[i].timerDmaSources = 0; dmaMotorTimers[i].timerDmaSources = 0;
} } while (false);
} }
} }
@ -164,16 +165,17 @@ FAST_CODE static void motor_DMA_IRQHandler(dmaChannelDescriptor_t* descriptor)
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
dshotDMAHandlerCycleCounters.irqAt = getCycleCounter(); dshotDMAHandlerCycleCounters.irqAt = getCycleCounter();
#endif #endif
do {
#ifdef USE_DSHOT_DMAR #ifdef USE_DSHOT_DMAR
if (useBurstDshot) { if (useBurstDshot) {
xDDL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef); xDDL_EX_DMA_DisableResource(motor->timerHardware->dmaTimUPRef);
DDL_TMR_DisableDMAReq_UPDATE(motor->timerHardware->tim); DDL_TMR_DisableDMAReq_UPDATE(motor->timerHardware->tim);
} else break;
}
#endif #endif
{
xDDL_EX_DMA_DisableResource(motor->dmaRef); xDDL_EX_DMA_DisableResource(motor->dmaRef);
DDL_EX_TMR_DisableIT(motor->timerHardware->tim, motor->timerDmaSource); DDL_EX_TMR_DisableIT(motor->timerHardware->tim, motor->timerDmaSource);
} } while (false);
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) { if (useDshotTelemetry) {
@ -228,21 +230,22 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef); dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dmaRef);
bool dmaIsConfigured = false; bool dmaIsConfigured = false;
do {
#ifdef USE_DSHOT_DMAR #ifdef USE_DSHOT_DMAR
if (useBurstDshot) { if (useBurstDshot) {
const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier); const resourceOwner_t *owner = dmaGetOwner(dmaIdentifier);
if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) { if (owner->owner == OWNER_TIMUP && owner->resourceIndex == timerGetTIMNumber(timerHardware->tim)) {
dmaIsConfigured = true; dmaIsConfigured = true;
} else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) { } else if (!dmaAllocate(dmaIdentifier, OWNER_TIMUP, timerGetTIMNumber(timerHardware->tim))) {
return false; return false;
}
break;
} }
} else
#endif #endif
{
if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) { if (!dmaAllocate(dmaIdentifier, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex))) {
return false; return false;
} }
} } while (false);
motorDmaOutput_t * const motor = &dmaMotors[motorIndex]; motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
motor->dmaRef = dmaRef; motor->dmaRef = dmaRef;
@ -312,18 +315,19 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
} }
motor->llChannel = channel; motor->llChannel = channel;
do {
#ifdef USE_DSHOT_DMAR #ifdef USE_DSHOT_DMAR
if (useBurstDshot) { if (useBurstDshot) {
motor->timer->dmaBurstRef = dmaRef; motor->timer->dmaBurstRef = dmaRef;
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
motor->dmaRef = dmaRef; motor->dmaRef = dmaRef;
#endif #endif
} else break;
}
#endif #endif
{
motor->timerDmaSource = timerDmaSource(timerHardware->channel); motor->timerDmaSource = timerDmaSource(timerHardware->channel);
motor->timer->timerDmaSources &= ~motor->timerDmaSource; motor->timer->timerDmaSources &= ~motor->timerDmaSource;
} } while (false);
if (!dmaIsConfigured) { if (!dmaIsConfigured) {
xDDL_EX_DMA_DisableResource(dmaRef); xDDL_EX_DMA_DisableResource(dmaRef);
@ -333,24 +337,25 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
} }
DDL_DMA_StructInit(&DMAINIT); DDL_DMA_StructInit(&DMAINIT);
do {
#ifdef USE_DSHOT_DMAR #ifdef USE_DSHOT_DMAR
if (useBurstDshot) { if (useBurstDshot) {
motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0]; motor->timer->dmaBurstBuffer = &dshotBurstDmaBuffer[timerIndex][0];
DMAINIT.Channel = dmaChannel; DMAINIT.Channel = dmaChannel;
DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer; DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->timer->dmaBurstBuffer;
DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_FULL; DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_FULL;
DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR; DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)&timerHardware->tim->DMAR;
} else break;
}
#endif #endif
{
motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0]; motor->dmaBuffer = &dshotDmaBuffer[motorIndex][0];
DMAINIT.Channel = dmaChannel; DMAINIT.Channel = dmaChannel;
DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer; DMAINIT.MemoryOrM2MDstAddress = (uint32_t)motor->dmaBuffer;
DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_1_4; DMAINIT.FIFOThreshold = DDL_DMA_FIFOTHRESHOLD_1_4;
DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware); DMAINIT.PeriphOrM2MSrcAddress = (uint32_t)timerChCCR(timerHardware);
} } while (false);
DMAINIT.Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH; DMAINIT.Direction = DDL_DMA_DIRECTION_MEMORY_TO_PERIPH;
DMAINIT.FIFOMode = DDL_DMA_FIFOMODE_ENABLE; DMAINIT.FIFOMode = DDL_DMA_FIFOMODE_ENABLE;
@ -380,16 +385,17 @@ bool pwmDshotMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t m
pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT); pwmDshotSetDirectionOutput(motor, &OCINIT, &DMAINIT);
#endif #endif
do {
#ifdef USE_DSHOT_DMAR #ifdef USE_DSHOT_DMAR
if (useBurstDshot) { if (useBurstDshot) {
if (!dmaIsConfigured) { if (!dmaIsConfigured) {
dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index); dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
}
break;
} }
} else
#endif #endif
{
dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index); dmaSetHandler(dmaIdentifier, motor_DMA_IRQHandler, NVIC_PRIO_DSHOT_DMA, motor->index);
} } while (false);
DDL_TMR_OC_Init(timer, channel, &OCINIT); DDL_TMR_OC_Init(timer, channel, &OCINIT);
DDL_TMR_OC_EnablePreload(timer, channel); DDL_TMR_OC_EnablePreload(timer, channel);

View file

@ -82,35 +82,37 @@ void uartReconfigure(uartPort_t *uartPort)
// Receive DMA or IRQ // Receive DMA or IRQ
if (uartPort->port.mode & MODE_RX) { if (uartPort->port.mode & MODE_RX) {
do {
#ifdef USE_DMA #ifdef USE_DMA
if (uartPort->rxDMAResource) { if (uartPort->rxDMAResource) {
uartPort->rxDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->rxDMAResource; uartPort->rxDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->rxDMAResource;
uartPort->txDMAHandle.Init.Channel = uartPort->rxDMAChannel; uartPort->txDMAHandle.Init.Channel = uartPort->rxDMAChannel;
uartPort->rxDMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; uartPort->rxDMAHandle.Init.Direction = DMA_PERIPH_TO_MEMORY;
uartPort->rxDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE; uartPort->rxDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
uartPort->rxDMAHandle.Init.MemInc = DMA_MINC_ENABLE; uartPort->rxDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
uartPort->rxDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; uartPort->rxDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
uartPort->rxDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; uartPort->rxDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
uartPort->rxDMAHandle.Init.Mode = DMA_CIRCULAR; uartPort->rxDMAHandle.Init.Mode = DMA_CIRCULAR;
#if defined(APM32F4) #if defined(APM32F4)
uartPort->rxDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; uartPort->rxDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
uartPort->rxDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL; uartPort->rxDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
uartPort->rxDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; uartPort->rxDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
uartPort->rxDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE; uartPort->rxDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
#endif #endif
uartPort->rxDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM; uartPort->rxDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
DAL_DMA_DeInit(&uartPort->rxDMAHandle); DAL_DMA_DeInit(&uartPort->rxDMAHandle);
DAL_DMA_Init(&uartPort->rxDMAHandle); DAL_DMA_Init(&uartPort->rxDMAHandle);
/* Associate the initialized DMA handle to the UART handle */ /* Associate the initialized DMA handle to the UART handle */
__DAL_LINKDMA(&uartPort->Handle, hdmarx, uartPort->rxDMAHandle); __DAL_LINKDMA(&uartPort->Handle, hdmarx, uartPort->rxDMAHandle);
DAL_UART_Receive_DMA(&uartPort->Handle, (uint8_t*)uartPort->port.rxBuffer, uartPort->port.rxBufferSize); DAL_UART_Receive_DMA(&uartPort->Handle, (uint8_t*)uartPort->port.rxBuffer, uartPort->port.rxBufferSize);
uartPort->rxDMAPos = __DAL_DMA_GET_COUNTER(&uartPort->rxDMAHandle); uartPort->rxDMAPos = __DAL_DMA_GET_COUNTER(&uartPort->rxDMAHandle);
} else break;
}
#endif #endif
{
/* Enable the UART Parity Error Interrupt */ /* Enable the UART Parity Error Interrupt */
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_PEIEN); SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_PEIEN);
@ -122,44 +124,44 @@ void uartReconfigure(uartPort_t *uartPort)
/* Enable Idle Line detection */ /* Enable Idle Line detection */
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_IDLEIEN); SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_IDLEIEN);
} } while(false);
} }
// Transmit DMA or IRQ // Transmit DMA or IRQ
if (uartPort->port.mode & MODE_TX) { if (uartPort->port.mode & MODE_TX) {
do {
#ifdef USE_DMA #ifdef USE_DMA
if (uartPort->txDMAResource) { if (uartPort->txDMAResource) {
uartPort->txDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->txDMAResource; uartPort->txDMAHandle.Instance = (DMA_ARCH_TYPE *)uartPort->txDMAResource;
uartPort->txDMAHandle.Init.Channel = uartPort->txDMAChannel; uartPort->txDMAHandle.Init.Channel = uartPort->txDMAChannel;
uartPort->txDMAHandle.Init.Direction = DMA_MEMORY_TO_PERIPH; uartPort->txDMAHandle.Init.Direction = DMA_MEMORY_TO_PERIPH;
uartPort->txDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE; uartPort->txDMAHandle.Init.PeriphInc = DMA_PINC_DISABLE;
uartPort->txDMAHandle.Init.MemInc = DMA_MINC_ENABLE; uartPort->txDMAHandle.Init.MemInc = DMA_MINC_ENABLE;
uartPort->txDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; uartPort->txDMAHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
uartPort->txDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; uartPort->txDMAHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
uartPort->txDMAHandle.Init.Mode = DMA_NORMAL; uartPort->txDMAHandle.Init.Mode = DMA_NORMAL;
uartPort->txDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; uartPort->txDMAHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
uartPort->txDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL; uartPort->txDMAHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_1QUARTERFULL;
uartPort->txDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; uartPort->txDMAHandle.Init.PeriphBurst = DMA_PBURST_SINGLE;
uartPort->txDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE; uartPort->txDMAHandle.Init.MemBurst = DMA_MBURST_SINGLE;
uartPort->txDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM; uartPort->txDMAHandle.Init.Priority = DMA_PRIORITY_MEDIUM;
DAL_DMA_DeInit(&uartPort->txDMAHandle); DAL_DMA_DeInit(&uartPort->txDMAHandle);
DAL_StatusTypeDef status = DAL_DMA_Init(&uartPort->txDMAHandle); DAL_StatusTypeDef status = DAL_DMA_Init(&uartPort->txDMAHandle);
if (status != DAL_OK) { if (status != DAL_OK) {
while (1); while (1);
}
/* Associate the initialized DMA handle to the UART handle */
__DAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle);
__DAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
break;
} }
/* Associate the initialized DMA handle to the UART handle */
__DAL_LINKDMA(&uartPort->Handle, hdmatx, uartPort->txDMAHandle);
__DAL_DMA_SET_COUNTER(&uartPort->txDMAHandle, 0);
} else
#endif #endif
{
/* Enable the UART Transmit Data Register Empty Interrupt */ /* Enable the UART Transmit Data Register Empty Interrupt */
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXBEIEN); SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXBEIEN);
SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXCIEN); SET_BIT(uartPort->USARTx->CTRL1, USART_CTRL1_TXCIEN);
} } while(false);
} }
} }

View file

@ -269,10 +269,12 @@ void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor)
} }
handleUsartTxDma(s); handleUsartTxDma(s);
} }
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF))
{ {
DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF); DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF);
} }
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_DMEIF)) if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_DMEIF))
{ {
DMA_CLEAR_FLAG(descriptor, DMA_IT_DMEIF); DMA_CLEAR_FLAG(descriptor, DMA_IT_DMEIF);

View file

@ -121,10 +121,11 @@ void enableGPIOPowerUsageAndNoiseReductions(void)
bool isMPUSoftReset(void) bool isMPUSoftReset(void)
{ {
if (cachedRccCsrValue & RCM_CSTS_SWRSTFLG) if (cachedRccCsrValue & RCM_CSTS_SWRSTFLG) {
return true; return true;
else } else {
return false; return false;
}
} }
void systemInit(void) void systemInit(void)

View file

@ -340,7 +340,9 @@ TMR_HandleTypeDef* timerFindTimerHandle(TMR_TypeDef *tim)
void timerReconfigureTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz) void timerReconfigureTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz)
{ {
TMR_HandleTypeDef* handle = timerFindTimerHandle(tim); TMR_HandleTypeDef* handle = timerFindTimerHandle(tim);
if (handle == NULL) return; if (handle == NULL) {
return;
}
handle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR handle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
handle->Init.Prescaler = (timerClock(tim) / hz) - 1; handle->Init.Prescaler = (timerClock(tim) / hz) - 1;
@ -351,7 +353,9 @@ void timerReconfigureTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz)
void configTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz) void configTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz)
{ {
TMR_HandleTypeDef* handle = timerFindTimerHandle(tim); TMR_HandleTypeDef* handle = timerFindTimerHandle(tim);
if (handle == NULL) return; if (handle == NULL) {
return;
}
if (handle->Instance == tim) { if (handle->Instance == tim) {
// already configured // already configured
@ -368,8 +372,7 @@ void configTimeBase(TMR_TypeDef *tim, uint16_t period, uint32_t hz)
handle->Init.RepetitionCounter = 0x0000; handle->Init.RepetitionCounter = 0x0000;
DAL_TMR_Base_Init(handle); DAL_TMR_Base_Init(handle);
if (tim == TMR1 || tim == TMR2 || tim == TMR3 || tim == TMR4 || tim == TMR5 || tim == TMR8 || tim == TMR9) if (tim == TMR1 || tim == TMR2 || tim == TMR3 || tim == TMR4 || tim == TMR5 || tim == TMR8 || tim == TMR9) {
{
TMR_ClockConfigTypeDef sClockSourceConfig; TMR_ClockConfigTypeDef sClockSourceConfig;
memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig)); memset(&sClockSourceConfig, 0, sizeof(sClockSourceConfig));
sClockSourceConfig.ClockSource = TMR_CLOCKSOURCE_INTERNAL; sClockSourceConfig.ClockSource = TMR_CLOCKSOURCE_INTERNAL;
@ -402,7 +405,6 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
timerNVICConfigure(irq); timerNVICConfigure(irq);
// HACK - enable second IRQ on timers that need it // HACK - enable second IRQ on timers that need it
switch (irq) { switch (irq) {
case TMR1_CC_IRQn: case TMR1_CC_IRQn:
timerNVICConfigure(TMR1_UP_TMR10_IRQn); timerNVICConfigure(TMR1_UP_TMR10_IRQn);
break; break;
@ -421,13 +423,16 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori
return; return;
} }
unsigned channel = timHw - TIMER_HARDWARE; unsigned channel = timHw - TIMER_HARDWARE;
if (channel >= TIMER_CHANNEL_COUNT) if (channel >= TIMER_CHANNEL_COUNT) {
return; return;
}
timerChannelInfo[channel].type = type; timerChannelInfo[channel].type = type;
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if (timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT) {
return; return;
}
if (irqPriority < timerInfo[timer].priority) { if (irqPriority < timerInfo[timer].priority) {
// it would be better to set priority in the end, but current startup sequence is not ready // it would be better to set priority in the end, but current startup sequence is not ready
configTimeBase(usedTimers[timer], 0, 1); configTimeBase(usedTimers[timer], 0, 1);
@ -476,10 +481,11 @@ static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, const TMR_TypeDef *
*chain = NULL; *chain = NULL;
} }
// enable or disable IRQ // enable or disable IRQ
if (cfg->overflowCallbackActive) if (cfg->overflowCallbackActive) {
__DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TMR_IT_UPDATE); __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TMR_IT_UPDATE);
else } else {
__DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TMR_IT_UPDATE); __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TMR_IT_UPDATE);
}
} }
// config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive // config edge and overflow callback for channel. Try to avoid overflowCallback, it is a bit expensive
@ -490,14 +496,17 @@ void timerChConfigCallbacks(const timerHardware_t *timHw, timerCCHandlerRec_t *e
return; return;
} }
uint8_t channelIndex = lookupChannelIndex(timHw->channel); uint8_t channelIndex = lookupChannelIndex(timHw->channel);
if (edgeCallback == NULL) // disable irq before changing callback to NULL if (edgeCallback == NULL) { // disable irq before changing callback to NULL
__DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
}
// setup callback info // setup callback info
timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback; timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallback;
timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback; timerConfig[timerIndex].overflowCallback[channelIndex] = overflowCallback;
// enable channel IRQ // enable channel IRQ
if (edgeCallback) if (edgeCallback) {
__DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
}
timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim); timerChConfig_UpdateOverflow(&timerConfig[timerIndex], timHw->tim);
} }
@ -525,10 +534,13 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_
uint16_t chHi = timHw->channel | TMR_CHANNEL_2; // upper channel uint16_t chHi = timHw->channel | TMR_CHANNEL_2; // upper channel
uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel uint8_t channelIndex = lookupChannelIndex(chLo); // get index of lower channel
if (edgeCallbackLo == NULL) // disable irq before changing setting callback to NULL if (edgeCallbackLo == NULL) { // disable irq before changing setting callback to NULL
__DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
if (edgeCallbackHi == NULL) // disable irq before changing setting callback to NULL }
if (edgeCallbackHi == NULL) { // disable irq before changing setting callback to NULL
__DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
}
// setup callback info // setup callback info
timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallbackLo; timerConfig[timerIndex].edgeCallback[channelIndex] = edgeCallbackLo;
@ -541,6 +553,7 @@ void timerChConfigCallbacksDual(const timerHardware_t *timHw, timerCCHandlerRec_
__DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
__DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo)); __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chLo));
} }
if (edgeCallbackHi) { if (edgeCallbackHi) {
__DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __DAL_TMR_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
__DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi)); __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(chHi));
@ -561,10 +574,11 @@ void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newStat
return; return;
} }
if (newState) if (newState) {
__DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel & ~TMR_CHANNEL_2)); __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel & ~TMR_CHANNEL_2));
else } else {
__DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel & ~TMR_CHANNEL_2)); __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel & ~TMR_CHANNEL_2));
}
} }
//// enable or disable IRQ //// enable or disable IRQ
@ -580,10 +594,11 @@ void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
return; return;
} }
if (newState) if (newState) {
__DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __DAL_TMR_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
else } else {
__DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel)); __DAL_TMR_DISABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
}
} }
// clear Compare/Capture flag for channel // clear Compare/Capture flag for channel
@ -623,9 +638,11 @@ static unsigned getFilter(unsigned ticks)
16*5, 16*6, 16*8, 16*5, 16*6, 16*8,
32*5, 32*6, 32*8 32*5, 32*6, 32*8
}; };
for (unsigned i = 1; i < ARRAYLEN(ftab); i++) for (unsigned i = 1; i < ARRAYLEN(ftab); i++) {
if (ftab[i] > ticks) if (ftab[i] > ticks) {
return i - 1; return i - 1;
}
}
return 0x0f; return 0x0f;
} }
@ -633,8 +650,9 @@ static unsigned getFilter(unsigned ticks)
void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
{ {
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if (timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT) {
return; return;
}
TMR_IC_InitTypeDef TIM_ICInitStructure; TMR_IC_InitTypeDef TIM_ICInitStructure;
@ -650,8 +668,9 @@ void timerChConfigIC(const timerHardware_t *timHw, bool polarityRising, unsigned
void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks) void timerChConfigICDual(const timerHardware_t *timHw, bool polarityRising, unsigned inputFilterTicks)
{ {
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if (timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT) {
return; return;
}
TMR_IC_InitTypeDef TIM_ICInitStructure; TMR_IC_InitTypeDef TIM_ICInitStructure;
bool directRising = (timHw->channel & TMR_CHANNEL_2) ? !polarityRising : polarityRising; bool directRising = (timHw->channel & TMR_CHANNEL_2) ? !polarityRising : polarityRising;
@ -695,8 +714,9 @@ volatile timCCR_t* timerChCCR(const timerHardware_t *timHw)
void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh) void timerChConfigOC(const timerHardware_t* timHw, bool outEnable, bool stateHigh)
{ {
unsigned timer = lookupTimerIndex(timHw->tim); unsigned timer = lookupTimerIndex(timHw->tim);
if (timer >= USED_TIMER_COUNT) if (timer >= USED_TIMER_COUNT) {
return; return;
}
TMR_OC_InitTypeDef TIM_OCInitStructure; TMR_OC_InitTypeDef TIM_OCInitStructure;
@ -1000,7 +1020,9 @@ void timerInit(void)
void timerStart(TMR_TypeDef *tim) void timerStart(TMR_TypeDef *tim)
{ {
TMR_HandleTypeDef* handle = timerFindTimerHandle(tim); TMR_HandleTypeDef* handle = timerFindTimerHandle(tim);
if (handle == NULL) return; if (handle == NULL) {
return;
}
__DAL_TMR_ENABLE(handle); __DAL_TMR_ENABLE(handle);
} }
@ -1135,13 +1157,16 @@ DAL_StatusTypeDef DMA_SetCurrDataCounter(TMR_HandleTypeDef *htim, uint32_t Chann
{ {
if ((htim->State == DAL_TMR_STATE_BUSY)) { if ((htim->State == DAL_TMR_STATE_BUSY)) {
return DAL_BUSY; return DAL_BUSY;
} else if ((htim->State == DAL_TMR_STATE_READY)) { } else {
if (((uint32_t) pData == 0) && (Length > 0)) { if ((htim->State == DAL_TMR_STATE_READY)) {
return DAL_ERROR; if (((uint32_t) pData == 0) && (Length > 0)) {
} else { return DAL_ERROR;
htim->State = DAL_TMR_STATE_BUSY; } else {
htim->State = DAL_TMR_STATE_BUSY;
}
} }
} }
switch (Channel) { switch (Channel) {
case TMR_CHANNEL_1: { case TMR_CHANNEL_1: {
/* Set the DMA Period elapsed callback */ /* Set the DMA Period elapsed callback */

View file

@ -165,6 +165,7 @@ void transponderIrHardwareInit(ioTag_t ioTag, transponder_t *transponder)
/* Configuration Error */ /* Configuration Error */
return; return;
} }
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) { if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
if (DAL_TMREx_PWMN_Start(&TmrHandle, timerChannel) != DAL_OK) { if (DAL_TMREx_PWMN_Start(&TmrHandle, timerChannel) != DAL_OK) {
/* Starting PWM generation Error */ /* Starting PWM generation Error */

View file

@ -122,7 +122,7 @@ MCU_COMMON_SRC = \
drivers/usb_msc_common.c \ drivers/usb_msc_common.c \
drivers/adc.c \ drivers/adc.c \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_config.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
common/stm32/bus_spi_hw.c \ common/stm32/bus_spi_hw.c \
common/stm32/serial_uart_hw.c \ common/stm32/serial_uart_hw.c \
@ -145,7 +145,7 @@ SIZE_OPTIMISED_SRC += \
drivers/bus_i2c_timing.c \ drivers/bus_i2c_timing.c \
drivers/inverter.c \ drivers/inverter.c \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_config.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
drivers/serial_escserial.c \ drivers/serial_escserial.c \
drivers/serial_pinconfig.c \ drivers/serial_pinconfig.c \

View file

@ -6,7 +6,7 @@ MCU_COMMON_SRC += \
common/stm32/config_flash.c \ common/stm32/config_flash.c \
common/stm32/bus_spi_pinconfig.c \ common/stm32/bus_spi_pinconfig.c \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_config.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/bus_spi_hw.c \ common/stm32/bus_spi_hw.c \
common/stm32/io_impl.c \ common/stm32/io_impl.c \
common/stm32/serial_uart_hw.c \ common/stm32/serial_uart_hw.c \
@ -17,7 +17,7 @@ MCU_COMMON_SRC += \
SIZE_OPTIMISED_SRC += \ SIZE_OPTIMISED_SRC += \
drivers/bus_spi_config.c \ drivers/bus_spi_config.c \
common/stm32/bus_i2c_config.c \ common/stm32/bus_i2c_pinconfig.c \
common/stm32/config_flash.c \ common/stm32/config_flash.c \
common/stm32/bus_spi_pinconfig.c common/stm32/bus_spi_pinconfig.c

View file

@ -39,7 +39,7 @@
#include "pg/bus_i2c.h" #include "pg/bus_i2c.h"
void i2cHardwareConfigure(const i2cConfig_t *i2cConfig) void i2cPinConfigure(const i2cConfig_t *i2cConfig)
{ {
for (int index = 0 ; index < I2CDEV_COUNT ; index++) { for (int index = 0 ; index < I2CDEV_COUNT ; index++) {
const i2cHardware_t *hardware = &i2cHardware[index]; const i2cHardware_t *hardware = &i2cHardware[index];
@ -56,13 +56,13 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig)
for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) { for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) {
if (i2cConfig[device].ioTagScl == hardware->sclPins[pindex].ioTag) { if (i2cConfig[device].ioTagScl == hardware->sclPins[pindex].ioTag) {
pDev->scl = IOGetByTag(i2cConfig[device].ioTagScl); pDev->scl = IOGetByTag(i2cConfig[device].ioTagScl);
#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4) #if I2C_TRAIT_AF_PIN
pDev->sclAF = hardware->sclPins[pindex].af; pDev->sclAF = hardware->sclPins[pindex].af;
#endif #endif
} }
if (i2cConfig[device].ioTagSda == hardware->sdaPins[pindex].ioTag) { if (i2cConfig[device].ioTagSda == hardware->sdaPins[pindex].ioTag) {
pDev->sda = IOGetByTag(i2cConfig[device].ioTagSda); pDev->sda = IOGetByTag(i2cConfig[device].ioTagSda);
#if defined(STM32F4) || defined(STM32H7) || defined(STM32G4) || defined(AT32F4) || defined(APM32F4) #if I2C_TRAIT_AF_PIN
pDev->sdaAF = hardware->sdaPins[pindex].af; pDev->sdaAF = hardware->sdaPins[pindex].af;
#endif #endif
} }

View file

@ -1,5 +1,5 @@
#define I2CDEV_2 (1) #define I2CDEV_2 (1)
int i2cConfig(int); int i2cConfig(int);
void i2cHardwareConfigure(int); void i2cPinConfigure(int);
void i2cInit(int); void i2cInit(int);