mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
Removed trailing spaces from device drivers
This commit is contained in:
parent
c00e49ed72
commit
19901730fc
37 changed files with 226 additions and 226 deletions
|
@ -270,7 +270,7 @@ void mpuIntExtiInit(void)
|
||||||
EXTIEnable(mpuIntIO, true);
|
EXTIEnable(mpuIntIO, true);
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
mpuExtiInitDone = true;
|
mpuExtiInitDone = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -120,7 +120,7 @@
|
||||||
|
|
||||||
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
|
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
|
||||||
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
|
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
|
||||||
typedef void(*mpuResetFuncPtr)(void);
|
typedef void(*mpuResetFuncPtr)(void);
|
||||||
|
|
||||||
typedef struct mpuConfiguration_s {
|
typedef struct mpuConfiguration_s {
|
||||||
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
|
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
|
||||||
|
|
|
@ -97,7 +97,7 @@ void mpu6500GyroInit(uint8_t lpf)
|
||||||
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
|
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
|
||||||
#endif
|
#endif
|
||||||
delay(15);
|
delay(15);
|
||||||
|
|
||||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||||
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
|
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -82,9 +82,9 @@ bool icm20689SpiDetect(void)
|
||||||
{
|
{
|
||||||
uint8_t tmp;
|
uint8_t tmp;
|
||||||
uint8_t attemptsRemaining = 20;
|
uint8_t attemptsRemaining = 20;
|
||||||
|
|
||||||
icm20689SpiInit();
|
icm20689SpiInit();
|
||||||
|
|
||||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
||||||
|
|
||||||
icm20689WriteRegister(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
icm20689WriteRegister(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||||
|
@ -145,7 +145,7 @@ void icm20689AccInit(acc_t *acc)
|
||||||
void icm20689GyroInit(uint8_t lpf)
|
void icm20689GyroInit(uint8_t lpf)
|
||||||
{
|
{
|
||||||
mpuIntExtiInit();
|
mpuIntExtiInit();
|
||||||
|
|
||||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||||
|
|
||||||
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||||
|
@ -170,7 +170,7 @@ void icm20689GyroInit(uint8_t lpf)
|
||||||
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||||
|
|
||||||
delay(15);
|
delay(15);
|
||||||
|
|
||||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||||
mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -204,7 +204,7 @@ bool mpu6000SpiDetect(void)
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mpu6000AccAndGyroInit(void)
|
static void mpu6000AccAndGyroInit(void)
|
||||||
{
|
{
|
||||||
if (mpuSpi6000InitDone) {
|
if (mpuSpi6000InitDone) {
|
||||||
return;
|
return;
|
||||||
|
|
|
@ -84,9 +84,9 @@ bool mpu6500SpiDetect(void)
|
||||||
|
|
||||||
mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
|
mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
|
||||||
|
|
||||||
if (tmp == MPU6500_WHO_AM_I_CONST ||
|
if (tmp == MPU6500_WHO_AM_I_CONST ||
|
||||||
tmp == MPU9250_WHO_AM_I_CONST ||
|
tmp == MPU9250_WHO_AM_I_CONST ||
|
||||||
tmp == ICM20608G_WHO_AM_I_CONST ||
|
tmp == ICM20608G_WHO_AM_I_CONST ||
|
||||||
tmp == ICM20602_WHO_AM_I_CONST) {
|
tmp == ICM20602_WHO_AM_I_CONST) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -103,13 +103,13 @@ void mpu6500SpiGyroInit(uint8_t lpf)
|
||||||
{
|
{
|
||||||
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
|
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
|
|
||||||
mpu6500GyroInit(lpf);
|
mpu6500GyroInit(lpf);
|
||||||
|
|
||||||
// Disable Primary I2C Interface
|
// Disable Primary I2C Interface
|
||||||
mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
|
mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
}
|
}
|
||||||
|
|
|
@ -19,7 +19,7 @@
|
||||||
|
|
||||||
bool mpu6500SpiDetect(void);
|
bool mpu6500SpiDetect(void);
|
||||||
|
|
||||||
void mpu6500SpiAccInit(acc_t *acc);
|
void mpu6500SpiAccInit(acc_t *acc);
|
||||||
void mpu6500SpiGyroInit(uint8_t lpf);
|
void mpu6500SpiGyroInit(uint8_t lpf);
|
||||||
|
|
||||||
bool mpu6500SpiAccDetect(acc_t *acc);
|
bool mpu6500SpiAccDetect(acc_t *acc);
|
||||||
|
|
|
@ -39,7 +39,7 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const adcDevice_t adcHardware[] = {
|
const adcDevice_t adcHardware[] = {
|
||||||
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Channelx = DMA1_Channel1 }
|
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Channelx = DMA1_Channel1 }
|
||||||
};
|
};
|
||||||
|
|
||||||
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
|
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
|
||||||
|
@ -64,7 +64,7 @@ const adcTagMap_t adcTagMap[] = {
|
||||||
{ DEFIO_TAG_E__PA6, ADC_Channel_6 }, // ADC12
|
{ DEFIO_TAG_E__PA6, ADC_Channel_6 }, // ADC12
|
||||||
{ DEFIO_TAG_E__PA7, ADC_Channel_7 }, // ADC12
|
{ DEFIO_TAG_E__PA7, ADC_Channel_7 }, // ADC12
|
||||||
{ DEFIO_TAG_E__PB0, ADC_Channel_8 }, // ADC12
|
{ DEFIO_TAG_E__PB0, ADC_Channel_8 }, // ADC12
|
||||||
{ DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12
|
{ DEFIO_TAG_E__PB1, ADC_Channel_9 }, // ADC12
|
||||||
};
|
};
|
||||||
|
|
||||||
// Driver for STM32F103CB onboard ADC
|
// Driver for STM32F103CB onboard ADC
|
||||||
|
|
|
@ -39,11 +39,11 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const adcDevice_t adcHardware[] = {
|
const adcDevice_t adcHardware[] = {
|
||||||
{ .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 },
|
{ .ADCx = ADC1, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA1_Channel1 },
|
||||||
#ifdef ADC24_DMA_REMAP
|
#ifdef ADC24_DMA_REMAP
|
||||||
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 }
|
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel3 }
|
||||||
#else
|
#else
|
||||||
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 }
|
{ .ADCx = ADC2, .rccADC = RCC_AHB(ADC12), .DMAy_Channelx = DMA2_Channel1 }
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -38,8 +38,8 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const adcDevice_t adcHardware[] = {
|
const adcDevice_t adcHardware[] = {
|
||||||
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
|
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
|
||||||
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
|
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
|
||||||
};
|
};
|
||||||
|
|
||||||
/* note these could be packed up for saving space */
|
/* note these could be packed up for saving space */
|
||||||
|
@ -127,7 +127,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
if (device == ADCINVALID)
|
if (device == ADCINVALID)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
adcDevice_t adc = adcHardware[device];
|
adcDevice_t adc = adcHardware[device];
|
||||||
|
|
||||||
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
|
for (uint8_t i = 0; i < ADC_CHANNEL_COUNT; i++) {
|
||||||
if (!adcConfig[i].tag)
|
if (!adcConfig[i].tag)
|
||||||
|
|
|
@ -37,13 +37,13 @@
|
||||||
#define ADC_INSTANCE ADC1
|
#define ADC_INSTANCE ADC1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const adcDevice_t adcHardware[] = {
|
const adcDevice_t adcHardware[] = {
|
||||||
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0 },
|
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_CHANNEL_0 },
|
||||||
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
|
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
|
||||||
};
|
};
|
||||||
|
|
||||||
/* note these could be packed up for saving space */
|
/* note these could be packed up for saving space */
|
||||||
const adcTagMap_t adcTagMap[] = {
|
const adcTagMap_t adcTagMap[] = {
|
||||||
/*
|
/*
|
||||||
{ DEFIO_TAG_E__PF3, ADC_Channel_9 },
|
{ DEFIO_TAG_E__PF3, ADC_Channel_9 },
|
||||||
{ DEFIO_TAG_E__PF4, ADC_Channel_14 },
|
{ DEFIO_TAG_E__PF4, ADC_Channel_14 },
|
||||||
|
|
|
@ -142,15 +142,15 @@ static bool i2cHandleHardwareFailure(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)
|
||||||
{
|
{
|
||||||
HAL_StatusTypeDef status;
|
HAL_StatusTypeDef status;
|
||||||
|
|
||||||
if(reg_ == 0xFF)
|
if(reg_ == 0xFF)
|
||||||
status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT);
|
status = HAL_I2C_Master_Transmit(&i2cHandle[device].Handle,addr_ << 1,data, len_, I2C_DEFAULT_TIMEOUT);
|
||||||
else
|
else
|
||||||
status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT);
|
status = HAL_I2C_Mem_Write(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,data, len_, I2C_DEFAULT_TIMEOUT);
|
||||||
|
|
||||||
if(status != HAL_OK)
|
if(status != HAL_OK)
|
||||||
return i2cHandleHardwareFailure(device);
|
return i2cHandleHardwareFailure(device);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -162,15 +162,15 @@ bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
|
||||||
bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
|
bool i2cRead(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
|
||||||
{
|
{
|
||||||
HAL_StatusTypeDef status;
|
HAL_StatusTypeDef status;
|
||||||
|
|
||||||
if(reg_ == 0xFF)
|
if(reg_ == 0xFF)
|
||||||
status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT);
|
status = HAL_I2C_Master_Receive(&i2cHandle[device].Handle,addr_ << 1,buf, len, I2C_DEFAULT_TIMEOUT);
|
||||||
else
|
else
|
||||||
status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT);
|
status = HAL_I2C_Mem_Read(&i2cHandle[device].Handle,addr_ << 1, reg_, I2C_MEMADD_SIZE_8BIT,buf, len, I2C_DEFAULT_TIMEOUT);
|
||||||
|
|
||||||
if(status != HAL_OK)
|
if(status != HAL_OK)
|
||||||
return i2cHandleHardwareFailure(device);
|
return i2cHandleHardwareFailure(device);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -228,7 +228,7 @@ void i2cInit(I2CDevice device)
|
||||||
#endif
|
#endif
|
||||||
// Init I2C peripheral
|
// Init I2C peripheral
|
||||||
HAL_I2C_DeInit(&i2cHandle[device].Handle);
|
HAL_I2C_DeInit(&i2cHandle[device].Handle);
|
||||||
|
|
||||||
i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev;
|
i2cHandle[device].Handle.Instance = i2cHardwareMap[device].dev;
|
||||||
/// TODO: HAL check if I2C timing is correct
|
/// TODO: HAL check if I2C timing is correct
|
||||||
i2cHandle[device].Handle.Init.Timing = 0x00B01B59;
|
i2cHandle[device].Handle.Init.Timing = 0x00B01B59;
|
||||||
|
@ -239,12 +239,12 @@ void i2cInit(I2CDevice device)
|
||||||
i2cHandle[device].Handle.Init.OwnAddress2 = 0x0;
|
i2cHandle[device].Handle.Init.OwnAddress2 = 0x0;
|
||||||
i2cHandle[device].Handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
|
i2cHandle[device].Handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
|
||||||
i2cHandle[device].Handle.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
|
i2cHandle[device].Handle.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
|
||||||
|
|
||||||
|
|
||||||
HAL_I2C_Init(&i2cHandle[device].Handle);
|
HAL_I2C_Init(&i2cHandle[device].Handle);
|
||||||
/* Enable the Analog I2C Filter */
|
/* Enable the Analog I2C Filter */
|
||||||
HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
|
HAL_I2CEx_ConfigAnalogFilter(&i2cHandle[device].Handle,I2C_ANALOGFILTER_ENABLE);
|
||||||
|
|
||||||
HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
|
HAL_NVIC_SetPriority(i2cHardwareMap[device].er_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER));
|
||||||
HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
|
HAL_NVIC_EnableIRQ(i2cHardwareMap[device].er_irq);
|
||||||
HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
|
HAL_NVIC_SetPriority(i2cHardwareMap[device].ev_irq, NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV), NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV));
|
||||||
|
|
|
@ -83,7 +83,7 @@ void i2cInit(I2CDevice device)
|
||||||
|
|
||||||
I2C_TypeDef *I2Cx;
|
I2C_TypeDef *I2Cx;
|
||||||
I2Cx = i2c->dev;
|
I2Cx = i2c->dev;
|
||||||
|
|
||||||
IO_t scl = IOGetByTag(i2c->scl);
|
IO_t scl = IOGetByTag(i2c->scl);
|
||||||
IO_t sda = IOGetByTag(i2c->sda);
|
IO_t sda = IOGetByTag(i2c->sda);
|
||||||
|
|
||||||
|
@ -109,7 +109,7 @@ void i2cInit(I2CDevice device)
|
||||||
I2C_Init(I2Cx, &i2cInit);
|
I2C_Init(I2Cx, &i2cInit);
|
||||||
|
|
||||||
I2C_StretchClockCmd(I2Cx, ENABLE);
|
I2C_StretchClockCmd(I2Cx, ENABLE);
|
||||||
|
|
||||||
I2C_Cmd(I2Cx, ENABLE);
|
I2C_Cmd(I2Cx, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -286,11 +286,11 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
|
||||||
HAL_StatusTypeDef status;
|
HAL_StatusTypeDef status;
|
||||||
|
|
||||||
#define SPI_DEFAULT_TIMEOUT 10
|
#define SPI_DEFAULT_TIMEOUT 10
|
||||||
|
|
||||||
if(!out) // Tx only
|
if(!out) // Tx only
|
||||||
{
|
{
|
||||||
status = HAL_SPI_Transmit(&spiHandle[spiDeviceByInstance(instance)].Handle, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
|
status = HAL_SPI_Transmit(&spiHandle[spiDeviceByInstance(instance)].Handle, (uint8_t *)in, len, SPI_DEFAULT_TIMEOUT);
|
||||||
}
|
}
|
||||||
else if(!in) // Rx only
|
else if(!in) // Rx only
|
||||||
{
|
{
|
||||||
status = HAL_SPI_Receive(&spiHandle[spiDeviceByInstance(instance)].Handle, out, len, SPI_DEFAULT_TIMEOUT);
|
status = HAL_SPI_Receive(&spiHandle[spiDeviceByInstance(instance)].Handle, out, len, SPI_DEFAULT_TIMEOUT);
|
||||||
|
@ -299,10 +299,10 @@ bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len
|
||||||
{
|
{
|
||||||
status = HAL_SPI_TransmitReceive(&spiHandle[spiDeviceByInstance(instance)].Handle, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT);
|
status = HAL_SPI_TransmitReceive(&spiHandle[spiDeviceByInstance(instance)].Handle, (uint8_t *)in, out, len, SPI_DEFAULT_TIMEOUT);
|
||||||
}
|
}
|
||||||
|
|
||||||
if( status != HAL_OK)
|
if( status != HAL_OK)
|
||||||
spiTimeoutUserCallback(instance);
|
spiTimeoutUserCallback(instance);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -93,7 +93,7 @@ resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier)
|
||||||
|
|
||||||
uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier)
|
uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier)
|
||||||
{
|
{
|
||||||
return dmaDescriptors[identifier].resourceIndex;
|
return dmaDescriptors[identifier].resourceIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel)
|
dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel)
|
||||||
|
@ -103,5 +103,5 @@ dmaIdentifier_e dmaGetIdentifier(const DMA_Channel_TypeDef* channel)
|
||||||
return i;
|
return i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
|
@ -84,7 +84,7 @@ typedef enum {
|
||||||
#define DMA_IT_FEIF ((uint32_t)0x00000001)
|
#define DMA_IT_FEIF ((uint32_t)0x00000001)
|
||||||
|
|
||||||
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream);
|
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream);
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -95,7 +95,7 @@ typedef enum {
|
||||||
DMA1_CH5_HANDLER,
|
DMA1_CH5_HANDLER,
|
||||||
DMA1_CH6_HANDLER,
|
DMA1_CH6_HANDLER,
|
||||||
DMA1_CH7_HANDLER,
|
DMA1_CH7_HANDLER,
|
||||||
#if defined(STM32F3) || defined(STM32F10X_CL)
|
#if defined(STM32F3) || defined(STM32F10X_CL)
|
||||||
DMA2_CH1_HANDLER,
|
DMA2_CH1_HANDLER,
|
||||||
DMA2_CH2_HANDLER,
|
DMA2_CH2_HANDLER,
|
||||||
DMA2_CH3_HANDLER,
|
DMA2_CH3_HANDLER,
|
||||||
|
|
|
@ -112,7 +112,7 @@ resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier)
|
||||||
|
|
||||||
uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier)
|
uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier)
|
||||||
{
|
{
|
||||||
return dmaDescriptors[identifier].resourceIndex;
|
return dmaDescriptors[identifier].resourceIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream)
|
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream)
|
||||||
|
|
|
@ -104,7 +104,7 @@ resourceOwner_e dmaGetOwner(dmaIdentifier_e identifier)
|
||||||
|
|
||||||
uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier)
|
uint8_t dmaGetResourceIndex(dmaIdentifier_e identifier)
|
||||||
{
|
{
|
||||||
return dmaDescriptors[identifier].resourceIndex;
|
return dmaDescriptors[identifier].resourceIndex;
|
||||||
}
|
}
|
||||||
|
|
||||||
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream)
|
dmaIdentifier_e dmaGetIdentifier(const DMA_Stream_TypeDef* stream)
|
||||||
|
|
|
@ -205,16 +205,16 @@ static bool m25p16_readIdentification()
|
||||||
*/
|
*/
|
||||||
bool m25p16_init(ioTag_t csTag)
|
bool m25p16_init(ioTag_t csTag)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
if we have already detected a flash device we can simply exit
|
if we have already detected a flash device we can simply exit
|
||||||
|
|
||||||
TODO: change the init param in favour of flash CFG when ParamGroups work is done
|
TODO: change the init param in favour of flash CFG when ParamGroups work is done
|
||||||
then cs pin can be specified in hardware_revision.c or config.c (dependent on revision).
|
then cs pin can be specified in hardware_revision.c or config.c (dependent on revision).
|
||||||
*/
|
*/
|
||||||
if (geometry.sectors) {
|
if (geometry.sectors) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (csTag) {
|
if (csTag) {
|
||||||
m25p16CsPin = IOGetByTag(csTag);
|
m25p16CsPin = IOGetByTag(csTag);
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -20,14 +20,14 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#ifdef INVERTER
|
#ifdef INVERTER
|
||||||
|
|
||||||
#include "io.h"
|
#include "io.h"
|
||||||
#include "io_impl.h"
|
#include "io_impl.h"
|
||||||
|
|
||||||
#include "inverter.h"
|
#include "inverter.h"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
TODO: move this to support multiple inverters on different UARTs etc
|
TODO: move this to support multiple inverters on different UARTs etc
|
||||||
possibly move to put it in the UART driver itself.
|
possibly move to put it in the UART driver itself.
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -22,31 +22,31 @@
|
||||||
#define WS2811_LED_STRIP_LENGTH 32
|
#define WS2811_LED_STRIP_LENGTH 32
|
||||||
#define WS2811_BITS_PER_LED 24
|
#define WS2811_BITS_PER_LED 24
|
||||||
// for 50us delay
|
// for 50us delay
|
||||||
#define WS2811_DELAY_BUFFER_LENGTH 42
|
#define WS2811_DELAY_BUFFER_LENGTH 42
|
||||||
|
|
||||||
#define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH)
|
#define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH)
|
||||||
// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
|
// number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes)
|
||||||
#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)
|
#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH)
|
||||||
|
|
||||||
#if defined(STM32F40_41xxx)
|
#if defined(STM32F40_41xxx)
|
||||||
#define WS2811_TIMER_HZ 84000000
|
#define WS2811_TIMER_HZ 84000000
|
||||||
#define WS2811_TIMER_PERIOD 104
|
#define WS2811_TIMER_PERIOD 104
|
||||||
// timer compare value for logical 1
|
// timer compare value for logical 1
|
||||||
#define BIT_COMPARE_1 67
|
#define BIT_COMPARE_1 67
|
||||||
// timer compare value for logical 0
|
// timer compare value for logical 0
|
||||||
#define BIT_COMPARE_0 33
|
#define BIT_COMPARE_0 33
|
||||||
#elif defined(STM32F7)
|
#elif defined(STM32F7)
|
||||||
// timer compare value for logical 1
|
// timer compare value for logical 1
|
||||||
#define BIT_COMPARE_1 76
|
#define BIT_COMPARE_1 76
|
||||||
// timer compare value for logical 0
|
// timer compare value for logical 0
|
||||||
#define BIT_COMPARE_0 38
|
#define BIT_COMPARE_0 38
|
||||||
#else
|
#else
|
||||||
#define WS2811_TIMER_HZ 24000000
|
#define WS2811_TIMER_HZ 24000000
|
||||||
#define WS2811_TIMER_PERIOD 29
|
#define WS2811_TIMER_PERIOD 29
|
||||||
// timer compare value for logical 1
|
// timer compare value for logical 1
|
||||||
#define BIT_COMPARE_1 17
|
#define BIT_COMPARE_1 17
|
||||||
// timer compare value for logical 0
|
// timer compare value for logical 0
|
||||||
#define BIT_COMPARE_0 9
|
#define BIT_COMPARE_0 9
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void ws2811LedStripInit(ioTag_t ioTag);
|
void ws2811LedStripInit(ioTag_t ioTag);
|
||||||
|
|
|
@ -72,7 +72,7 @@ void ws2811LedStripHardwareInit(ioTag_t ioTag)
|
||||||
|
|
||||||
/* Compute the prescaler value */
|
/* Compute the prescaler value */
|
||||||
uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1;
|
uint16_t prescalerValue = (uint16_t) (SystemCoreClock / WS2811_TIMER_HZ) - 1;
|
||||||
|
|
||||||
/* Time base configuration */
|
/* Time base configuration */
|
||||||
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
||||||
TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz
|
TIM_TimeBaseStructure.TIM_Period = WS2811_TIMER_PERIOD; // 800kHz
|
||||||
|
|
|
@ -43,7 +43,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
|
||||||
|
|
||||||
TIM_OCStructInit(&TIM_OCInitStructure);
|
TIM_OCStructInit(&TIM_OCInitStructure);
|
||||||
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
||||||
|
|
||||||
if (output & TIMER_OUTPUT_N_CHANNEL) {
|
if (output & TIMER_OUTPUT_N_CHANNEL) {
|
||||||
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
|
||||||
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
|
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
|
||||||
|
@ -162,7 +162,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
pwmWriteFuncPtr pwmWritePtr;
|
pwmWriteFuncPtr pwmWritePtr;
|
||||||
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
|
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
|
||||||
bool isDigital = false;
|
bool isDigital = false;
|
||||||
|
|
||||||
switch (motorConfig->motorPwmProtocol) {
|
switch (motorConfig->motorPwmProtocol) {
|
||||||
default:
|
default:
|
||||||
case PWM_TYPE_ONESHOT125:
|
case PWM_TYPE_ONESHOT125:
|
||||||
|
@ -202,16 +202,16 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
if (!useUnsyncedPwm && !isDigital) {
|
if (!useUnsyncedPwm && !isDigital) {
|
||||||
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
|
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||||
const ioTag_t tag = motorConfig->ioTags[motorIndex];
|
const ioTag_t tag = motorConfig->ioTags[motorIndex];
|
||||||
|
|
||||||
if (!tag) {
|
if (!tag) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
|
const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
|
||||||
|
|
||||||
if (timerHardware == NULL) {
|
if (timerHardware == NULL) {
|
||||||
/* flag failure and disable ability to arm */
|
/* flag failure and disable ability to arm */
|
||||||
break;
|
break;
|
||||||
|
@ -227,10 +227,10 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
||||||
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
|
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
|
||||||
|
|
||||||
motors[motorIndex].pwmWritePtr = pwmWritePtr;
|
motors[motorIndex].pwmWritePtr = pwmWritePtr;
|
||||||
if (useUnsyncedPwm) {
|
if (useUnsyncedPwm) {
|
||||||
const uint32_t hz = timerMhzCounter * 1000000;
|
const uint32_t hz = timerMhzCounter * 1000000;
|
||||||
|
@ -242,7 +242,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool pwmIsSynced(void)
|
bool pwmIsSynced(void)
|
||||||
{
|
{
|
||||||
return pwmCompleteWritePtr != NULL;
|
return pwmCompleteWritePtr != NULL;
|
||||||
}
|
}
|
||||||
|
@ -264,23 +264,23 @@ void servoInit(const servoConfig_t *servoConfig)
|
||||||
{
|
{
|
||||||
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
||||||
const ioTag_t tag = servoConfig->ioTags[servoIndex];
|
const ioTag_t tag = servoConfig->ioTags[servoIndex];
|
||||||
|
|
||||||
if (!tag) {
|
if (!tag) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
servos[servoIndex].io = IOGetByTag(tag);
|
servos[servoIndex].io = IOGetByTag(tag);
|
||||||
|
|
||||||
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
|
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
|
||||||
IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
|
IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
|
||||||
|
|
||||||
const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
|
const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
|
||||||
|
|
||||||
if (timer == NULL) {
|
if (timer == NULL) {
|
||||||
/* flag failure and disable ability to arm */
|
/* flag failure and disable ability to arm */
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
|
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
|
||||||
servos[servoIndex].enabled = true;
|
servos[servoIndex].enabled = true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -41,7 +41,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
|
||||||
{
|
{
|
||||||
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
|
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
|
||||||
if(Handle == NULL) return;
|
if(Handle == NULL) return;
|
||||||
|
|
||||||
TIM_OC_InitTypeDef TIM_OCInitStructure;
|
TIM_OC_InitTypeDef TIM_OCInitStructure;
|
||||||
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM2;
|
TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM2;
|
||||||
TIM_OCInitStructure.Pulse = value;
|
TIM_OCInitStructure.Pulse = value;
|
||||||
|
@ -50,7 +50,7 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8
|
||||||
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
|
TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
|
||||||
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
|
TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
|
||||||
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
|
TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
|
||||||
|
|
||||||
HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
|
HAL_TIM_PWM_ConfigChannel(Handle, &TIM_OCInitStructure, channel);
|
||||||
//HAL_TIM_PWM_Start(Handle, channel);
|
//HAL_TIM_PWM_Start(Handle, channel);
|
||||||
}
|
}
|
||||||
|
@ -173,7 +173,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
pwmWriteFuncPtr pwmWritePtr;
|
pwmWriteFuncPtr pwmWritePtr;
|
||||||
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
|
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
|
||||||
bool isDigital = false;
|
bool isDigital = false;
|
||||||
|
|
||||||
switch (motorConfig->motorPwmProtocol) {
|
switch (motorConfig->motorPwmProtocol) {
|
||||||
default:
|
default:
|
||||||
case PWM_TYPE_ONESHOT125:
|
case PWM_TYPE_ONESHOT125:
|
||||||
|
@ -213,16 +213,16 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
if (!useUnsyncedPwm && !isDigital) {
|
if (!useUnsyncedPwm && !isDigital) {
|
||||||
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
|
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||||
const ioTag_t tag = motorConfig->ioTags[motorIndex];
|
const ioTag_t tag = motorConfig->ioTags[motorIndex];
|
||||||
|
|
||||||
if (!tag) {
|
if (!tag) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
|
const timerHardware_t *timerHardware = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
|
||||||
|
|
||||||
if (timerHardware == NULL) {
|
if (timerHardware == NULL) {
|
||||||
/* flag failure and disable ability to arm */
|
/* flag failure and disable ability to arm */
|
||||||
break;
|
break;
|
||||||
|
@ -237,11 +237,11 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
motors[motorIndex].io = IOGetByTag(tag);
|
motors[motorIndex].io = IOGetByTag(tag);
|
||||||
|
|
||||||
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
||||||
//IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
|
//IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
|
||||||
IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
|
IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timerHardware->alternateFunction);
|
||||||
|
|
||||||
motors[motorIndex].pwmWritePtr = pwmWritePtr;
|
motors[motorIndex].pwmWritePtr = pwmWritePtr;
|
||||||
if (useUnsyncedPwm) {
|
if (useUnsyncedPwm) {
|
||||||
const uint32_t hz = timerMhzCounter * 1000000;
|
const uint32_t hz = timerMhzCounter * 1000000;
|
||||||
|
@ -253,7 +253,7 @@ void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t mot
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool pwmIsSynced(void)
|
bool pwmIsSynced(void)
|
||||||
{
|
{
|
||||||
return pwmCompleteWritePtr != NULL;
|
return pwmCompleteWritePtr != NULL;
|
||||||
}
|
}
|
||||||
|
@ -275,24 +275,24 @@ void servoInit(const servoConfig_t *servoConfig)
|
||||||
{
|
{
|
||||||
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
|
||||||
const ioTag_t tag = servoConfig->ioTags[servoIndex];
|
const ioTag_t tag = servoConfig->ioTags[servoIndex];
|
||||||
|
|
||||||
if (!tag) {
|
if (!tag) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
servos[servoIndex].io = IOGetByTag(tag);
|
servos[servoIndex].io = IOGetByTag(tag);
|
||||||
|
|
||||||
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
|
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
|
||||||
//IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
|
//IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
|
||||||
|
|
||||||
const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
|
const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
|
||||||
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
|
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
|
||||||
|
|
||||||
if (timer == NULL) {
|
if (timer == NULL) {
|
||||||
/* flag failure and disable ability to arm */
|
/* flag failure and disable ability to arm */
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
|
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
|
||||||
servos[servoIndex].enabled = true;
|
servos[servoIndex].enabled = true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -83,7 +83,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
||||||
packet <<= 1;
|
packet <<= 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
DMA_SetCurrDataCounter(motor->timerHardware->dmaChannel, MOTOR_DMA_BUFFER_SIZE);
|
DMA_SetCurrDataCounter(motor->timerHardware->dmaChannel, MOTOR_DMA_BUFFER_SIZE);
|
||||||
DMA_Cmd(motor->timerHardware->dmaChannel, ENABLE);
|
DMA_Cmd(motor->timerHardware->dmaChannel, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -94,10 +94,10 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
|
||||||
if (!pwmMotorsEnabled) {
|
if (!pwmMotorsEnabled) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int i = 0; i < dmaMotorTimerCount; i++) {
|
for (int i = 0; i < dmaMotorTimerCount; i++) {
|
||||||
TIM_SetCounter(dmaMotorTimers[i].timer, 0);
|
TIM_SetCounter(dmaMotorTimers[i].timer, 0);
|
||||||
TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
|
TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -118,18 +118,18 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
|
|
||||||
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
||||||
motor->timerHardware = timerHardware;
|
motor->timerHardware = timerHardware;
|
||||||
|
|
||||||
TIM_TypeDef *timer = timerHardware->tim;
|
TIM_TypeDef *timer = timerHardware->tim;
|
||||||
const IO_t motorIO = IOGetByTag(timerHardware->tag);
|
const IO_t motorIO = IOGetByTag(timerHardware->tag);
|
||||||
|
|
||||||
const uint8_t timerIndex = getTimerIndex(timer);
|
const uint8_t timerIndex = getTimerIndex(timer);
|
||||||
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
|
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
|
||||||
|
|
||||||
IOInit(motorIO, OWNER_MOTOR, 0);
|
IOInit(motorIO, OWNER_MOTOR, 0);
|
||||||
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction);
|
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction);
|
||||||
|
|
||||||
if (configureTimer) {
|
if (configureTimer) {
|
||||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||||
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
||||||
|
|
||||||
RCC_ClockCmd(timerRCC(timer), ENABLE);
|
RCC_ClockCmd(timerRCC(timer), ENABLE);
|
||||||
|
@ -155,7 +155,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||||
TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
|
||||||
}
|
}
|
||||||
|
|
||||||
TIM_OCStructInit(&TIM_OCInitStructure);
|
TIM_OCStructInit(&TIM_OCInitStructure);
|
||||||
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
||||||
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
|
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
|
||||||
|
@ -177,16 +177,16 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
|
TIM_CCxCmd(timer, timerHardware->channel, TIM_CCx_Enable);
|
||||||
|
|
||||||
if (configureTimer) {
|
if (configureTimer) {
|
||||||
TIM_CtrlPWMOutputs(timer, ENABLE);
|
TIM_CtrlPWMOutputs(timer, ENABLE);
|
||||||
TIM_ARRPreloadConfig(timer, ENABLE);
|
TIM_ARRPreloadConfig(timer, ENABLE);
|
||||||
TIM_Cmd(timer, ENABLE);
|
TIM_Cmd(timer, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
DMA_Channel_TypeDef *channel = timerHardware->dmaChannel;
|
DMA_Channel_TypeDef *channel = timerHardware->dmaChannel;
|
||||||
|
|
||||||
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
||||||
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
|
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
|
||||||
|
|
||||||
DMA_Cmd(channel, DISABLE);
|
DMA_Cmd(channel, DISABLE);
|
||||||
DMA_DeInit(channel);
|
DMA_DeInit(channel);
|
||||||
DMA_StructInit(&DMA_InitStructure);
|
DMA_StructInit(&DMA_InitStructure);
|
||||||
|
|
|
@ -81,7 +81,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
||||||
packet <<= 1;
|
packet <<= 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
DMA_SetCurrDataCounter(motor->timerHardware->dmaStream, MOTOR_DMA_BUFFER_SIZE);
|
DMA_SetCurrDataCounter(motor->timerHardware->dmaStream, MOTOR_DMA_BUFFER_SIZE);
|
||||||
DMA_Cmd(motor->timerHardware->dmaStream, ENABLE);
|
DMA_Cmd(motor->timerHardware->dmaStream, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -92,10 +92,10 @@ void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
|
||||||
if (!pwmMotorsEnabled) {
|
if (!pwmMotorsEnabled) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (int i = 0; i < dmaMotorTimerCount; i++) {
|
for (int i = 0; i < dmaMotorTimerCount; i++) {
|
||||||
TIM_SetCounter(dmaMotorTimers[i].timer, 0);
|
TIM_SetCounter(dmaMotorTimers[i].timer, 0);
|
||||||
TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
|
TIM_DMACmd(dmaMotorTimers[i].timer, dmaMotorTimers[i].timerDmaSources, ENABLE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -116,23 +116,23 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
|
|
||||||
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
||||||
motor->timerHardware = timerHardware;
|
motor->timerHardware = timerHardware;
|
||||||
|
|
||||||
TIM_TypeDef *timer = timerHardware->tim;
|
TIM_TypeDef *timer = timerHardware->tim;
|
||||||
const IO_t motorIO = IOGetByTag(timerHardware->tag);
|
const IO_t motorIO = IOGetByTag(timerHardware->tag);
|
||||||
|
|
||||||
const uint8_t timerIndex = getTimerIndex(timer);
|
const uint8_t timerIndex = getTimerIndex(timer);
|
||||||
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
|
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
|
||||||
|
|
||||||
IOInit(motorIO, OWNER_MOTOR, 0);
|
IOInit(motorIO, OWNER_MOTOR, 0);
|
||||||
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction);
|
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerHardware->alternateFunction);
|
||||||
|
|
||||||
if (configureTimer) {
|
if (configureTimer) {
|
||||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||||
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
|
||||||
|
|
||||||
RCC_ClockCmd(timerRCC(timer), ENABLE);
|
RCC_ClockCmd(timerRCC(timer), ENABLE);
|
||||||
TIM_Cmd(timer, DISABLE);
|
TIM_Cmd(timer, DISABLE);
|
||||||
|
|
||||||
uint32_t hz;
|
uint32_t hz;
|
||||||
switch (pwmProtocolType) {
|
switch (pwmProtocolType) {
|
||||||
case(PWM_TYPE_DSHOT600):
|
case(PWM_TYPE_DSHOT600):
|
||||||
|
@ -149,11 +149,11 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;
|
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / timerClockDivisor(timer) / hz) - 1;
|
||||||
TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH;
|
TIM_TimeBaseStructure.TIM_Period = MOTOR_BITLENGTH;
|
||||||
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
|
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
|
||||||
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
|
||||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||||
TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure);
|
||||||
}
|
}
|
||||||
|
|
||||||
TIM_OCStructInit(&TIM_OCInitStructure);
|
TIM_OCStructInit(&TIM_OCInitStructure);
|
||||||
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
|
||||||
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
|
if (timerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
|
||||||
|
@ -171,20 +171,20 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
|
timerOCPreloadConfig(timer, timerHardware->channel, TIM_OCPreload_Enable);
|
||||||
motor->timerDmaSource = timerDmaSource(timerHardware->channel);
|
motor->timerDmaSource = timerDmaSource(timerHardware->channel);
|
||||||
dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
|
dmaMotorTimers[timerIndex].timerDmaSources |= motor->timerDmaSource;
|
||||||
|
|
||||||
TIM_CCxCmd(timer, motor->timerHardware->channel, TIM_CCx_Enable);
|
TIM_CCxCmd(timer, motor->timerHardware->channel, TIM_CCx_Enable);
|
||||||
|
|
||||||
if (configureTimer) {
|
if (configureTimer) {
|
||||||
TIM_CtrlPWMOutputs(timer, ENABLE);
|
TIM_CtrlPWMOutputs(timer, ENABLE);
|
||||||
TIM_ARRPreloadConfig(timer, ENABLE);
|
TIM_ARRPreloadConfig(timer, ENABLE);
|
||||||
TIM_Cmd(timer, ENABLE);
|
TIM_Cmd(timer, ENABLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
DMA_Stream_TypeDef *stream = timerHardware->dmaStream;
|
DMA_Stream_TypeDef *stream = timerHardware->dmaStream;
|
||||||
|
|
||||||
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
dmaInit(timerHardware->dmaIrqHandler, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
|
||||||
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
|
dmaSetHandler(timerHardware->dmaIrqHandler, motor_DMA_IRQHandler, NVIC_BUILD_PRIORITY(1, 2), motorIndex);
|
||||||
|
|
||||||
DMA_Cmd(stream, DISABLE);
|
DMA_Cmd(stream, DISABLE);
|
||||||
DMA_DeInit(stream);
|
DMA_DeInit(stream);
|
||||||
|
|
||||||
|
|
|
@ -91,7 +91,7 @@ void pwmWriteDigital(uint8_t index, uint16_t value)
|
||||||
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
|
void pwmCompleteDigitalMotorUpdate(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
UNUSED(motorCount);
|
UNUSED(motorCount);
|
||||||
|
|
||||||
if (!pwmMotorsEnabled) {
|
if (!pwmMotorsEnabled) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -123,13 +123,13 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
{
|
{
|
||||||
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
motorDmaOutput_t * const motor = &dmaMotors[motorIndex];
|
||||||
motor->timerHardware = timerHardware;
|
motor->timerHardware = timerHardware;
|
||||||
|
|
||||||
TIM_TypeDef *timer = timerHardware->tim;
|
TIM_TypeDef *timer = timerHardware->tim;
|
||||||
const IO_t motorIO = IOGetByTag(timerHardware->tag);
|
const IO_t motorIO = IOGetByTag(timerHardware->tag);
|
||||||
|
|
||||||
const uint8_t timerIndex = getTimerIndex(timer);
|
const uint8_t timerIndex = getTimerIndex(timer);
|
||||||
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
|
const bool configureTimer = (timerIndex == dmaMotorTimerCount-1);
|
||||||
|
|
||||||
IOInit(motorIO, OWNER_MOTOR, 0);
|
IOInit(motorIO, OWNER_MOTOR, 0);
|
||||||
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction);
|
IOConfigGPIOAF(motorIO, IO_CONFIG(GPIO_MODE_AF_PP, GPIO_SPEED_FREQ_VERY_HIGH, GPIO_PULLUP), timerHardware->alternateFunction);
|
||||||
|
|
||||||
|
@ -137,7 +137,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
|
|
||||||
if (configureTimer) {
|
if (configureTimer) {
|
||||||
RCC_ClockCmd(timerRCC(timer), ENABLE);
|
RCC_ClockCmd(timerRCC(timer), ENABLE);
|
||||||
|
|
||||||
uint32_t hz;
|
uint32_t hz;
|
||||||
switch (pwmProtocolType) {
|
switch (pwmProtocolType) {
|
||||||
case(PWM_TYPE_DSHOT600):
|
case(PWM_TYPE_DSHOT600):
|
||||||
|
@ -168,7 +168,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
{
|
{
|
||||||
motor->TimHandle = dmaMotors[timerIndex].TimHandle;
|
motor->TimHandle = dmaMotors[timerIndex].TimHandle;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (timerHardware->channel) {
|
switch (timerHardware->channel) {
|
||||||
case TIM_CHANNEL_1:
|
case TIM_CHANNEL_1:
|
||||||
motor->timerDmaSource = TIM_DMA_ID_CC1;
|
motor->timerDmaSource = TIM_DMA_ID_CC1;
|
||||||
|
@ -220,7 +220,7 @@ void pwmDigitalMotorHardwareConfig(const timerHardware_t *timerHardware, uint8_t
|
||||||
/* Initialization Error */
|
/* Initialization Error */
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
TIM_OC_InitTypeDef TIM_OCInitStructure;
|
TIM_OC_InitTypeDef TIM_OCInitStructure;
|
||||||
|
|
||||||
/* PWM1 Mode configuration: Channel1 */
|
/* PWM1 Mode configuration: Channel1 */
|
||||||
|
|
|
@ -349,7 +349,7 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
{
|
{
|
||||||
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
|
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
|
||||||
if(Handle == NULL) return;
|
if(Handle == NULL) return;
|
||||||
|
|
||||||
TIM_IC_InitTypeDef TIM_ICInitStructure;
|
TIM_IC_InitTypeDef TIM_ICInitStructure;
|
||||||
|
|
||||||
TIM_ICInitStructure.ICPolarity = polarity;
|
TIM_ICInitStructure.ICPolarity = polarity;
|
||||||
|
@ -390,16 +390,16 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
void pwmRxInit(const pwmConfig_t *pwmConfig)
|
void pwmRxInit(const pwmConfig_t *pwmConfig)
|
||||||
{
|
{
|
||||||
for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) {
|
for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) {
|
||||||
|
|
||||||
pwmInputPort_t *port = &pwmInputPorts[channel];
|
pwmInputPort_t *port = &pwmInputPorts[channel];
|
||||||
|
|
||||||
const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM);
|
const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_PWM);
|
||||||
|
|
||||||
if (!timer) {
|
if (!timer) {
|
||||||
/* TODO: maybe fail here if not enough channels? */
|
/* TODO: maybe fail here if not enough channels? */
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
port->state = 0;
|
port->state = 0;
|
||||||
port->missedEvents = 0;
|
port->missedEvents = 0;
|
||||||
port->channel = channel;
|
port->channel = channel;
|
||||||
|
@ -433,7 +433,7 @@ void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol)
|
||||||
if (!motors[motorIndex].enabled || motors[motorIndex].tim != pwmTimer) {
|
if (!motors[motorIndex].enabled || motors[motorIndex].tim != pwmTimer) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (pwmProtocol)
|
switch (pwmProtocol)
|
||||||
{
|
{
|
||||||
case PWM_TYPE_ONESHOT125:
|
case PWM_TYPE_ONESHOT125:
|
||||||
|
@ -464,9 +464,9 @@ void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol)
|
||||||
/* TODO: fail here? */
|
/* TODO: fail here? */
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
ppmAvoidPWMTimerClash(timer->tim, pwmProtocol);
|
ppmAvoidPWMTimerClash(timer->tim, pwmProtocol);
|
||||||
|
|
||||||
port->mode = INPUT_MODE_PPM;
|
port->mode = INPUT_MODE_PPM;
|
||||||
port->timerHardware = timer;
|
port->timerHardware = timer;
|
||||||
|
|
||||||
|
|
|
@ -68,16 +68,16 @@ typedef struct uartDevice_s {
|
||||||
|
|
||||||
//static uartPort_t uartPort[MAX_UARTS];
|
//static uartPort_t uartPort[MAX_UARTS];
|
||||||
#ifdef USE_UART1
|
#ifdef USE_UART1
|
||||||
static uartDevice_t uart1 =
|
static uartDevice_t uart1 =
|
||||||
{
|
{
|
||||||
.DMAChannel = DMA_CHANNEL_4,
|
.DMAChannel = DMA_CHANNEL_4,
|
||||||
#ifdef USE_UART1_RX_DMA
|
#ifdef USE_UART1_RX_DMA
|
||||||
.rxDMAStream = DMA2_Stream5,
|
.rxDMAStream = DMA2_Stream5,
|
||||||
#endif
|
#endif
|
||||||
.txDMAStream = DMA2_Stream7,
|
.txDMAStream = DMA2_Stream7,
|
||||||
.dev = USART1,
|
.dev = USART1,
|
||||||
.rx = IO_TAG(UART1_RX_PIN),
|
.rx = IO_TAG(UART1_RX_PIN),
|
||||||
.tx = IO_TAG(UART1_TX_PIN),
|
.tx = IO_TAG(UART1_TX_PIN),
|
||||||
.af = GPIO_AF7_USART1,
|
.af = GPIO_AF7_USART1,
|
||||||
#ifdef UART1_AHB1_PERIPHERALS
|
#ifdef UART1_AHB1_PERIPHERALS
|
||||||
.rcc_ahb1 = UART1_AHB1_PERIPHERALS,
|
.rcc_ahb1 = UART1_AHB1_PERIPHERALS,
|
||||||
|
@ -91,16 +91,16 @@ static uartDevice_t uart1 =
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_UART2
|
#ifdef USE_UART2
|
||||||
static uartDevice_t uart2 =
|
static uartDevice_t uart2 =
|
||||||
{
|
{
|
||||||
.DMAChannel = DMA_CHANNEL_4,
|
.DMAChannel = DMA_CHANNEL_4,
|
||||||
#ifdef USE_UART2_RX_DMA
|
#ifdef USE_UART2_RX_DMA
|
||||||
.rxDMAStream = DMA1_Stream5,
|
.rxDMAStream = DMA1_Stream5,
|
||||||
#endif
|
#endif
|
||||||
.txDMAStream = DMA1_Stream6,
|
.txDMAStream = DMA1_Stream6,
|
||||||
.dev = USART2,
|
.dev = USART2,
|
||||||
.rx = IO_TAG(UART2_RX_PIN),
|
.rx = IO_TAG(UART2_RX_PIN),
|
||||||
.tx = IO_TAG(UART2_TX_PIN),
|
.tx = IO_TAG(UART2_TX_PIN),
|
||||||
.af = GPIO_AF7_USART2,
|
.af = GPIO_AF7_USART2,
|
||||||
#ifdef UART2_AHB1_PERIPHERALS
|
#ifdef UART2_AHB1_PERIPHERALS
|
||||||
.rcc_ahb1 = UART2_AHB1_PERIPHERALS,
|
.rcc_ahb1 = UART2_AHB1_PERIPHERALS,
|
||||||
|
@ -114,16 +114,16 @@ static uartDevice_t uart2 =
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_UART3
|
#ifdef USE_UART3
|
||||||
static uartDevice_t uart3 =
|
static uartDevice_t uart3 =
|
||||||
{
|
{
|
||||||
.DMAChannel = DMA_CHANNEL_4,
|
.DMAChannel = DMA_CHANNEL_4,
|
||||||
#ifdef USE_UART3_RX_DMA
|
#ifdef USE_UART3_RX_DMA
|
||||||
.rxDMAStream = DMA1_Stream1,
|
.rxDMAStream = DMA1_Stream1,
|
||||||
#endif
|
#endif
|
||||||
.txDMAStream = DMA1_Stream3,
|
.txDMAStream = DMA1_Stream3,
|
||||||
.dev = USART3,
|
.dev = USART3,
|
||||||
.rx = IO_TAG(UART3_RX_PIN),
|
.rx = IO_TAG(UART3_RX_PIN),
|
||||||
.tx = IO_TAG(UART3_TX_PIN),
|
.tx = IO_TAG(UART3_TX_PIN),
|
||||||
.af = GPIO_AF7_USART3,
|
.af = GPIO_AF7_USART3,
|
||||||
#ifdef UART3_AHB1_PERIPHERALS
|
#ifdef UART3_AHB1_PERIPHERALS
|
||||||
.rcc_ahb1 = UART3_AHB1_PERIPHERALS,
|
.rcc_ahb1 = UART3_AHB1_PERIPHERALS,
|
||||||
|
@ -137,16 +137,16 @@ static uartDevice_t uart3 =
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_UART4
|
#ifdef USE_UART4
|
||||||
static uartDevice_t uart4 =
|
static uartDevice_t uart4 =
|
||||||
{
|
{
|
||||||
.DMAChannel = DMA_CHANNEL_4,
|
.DMAChannel = DMA_CHANNEL_4,
|
||||||
#ifdef USE_UART4_RX_DMA
|
#ifdef USE_UART4_RX_DMA
|
||||||
.rxDMAStream = DMA1_Stream2,
|
.rxDMAStream = DMA1_Stream2,
|
||||||
#endif
|
#endif
|
||||||
.txDMAStream = DMA1_Stream4,
|
.txDMAStream = DMA1_Stream4,
|
||||||
.dev = UART4,
|
.dev = UART4,
|
||||||
.rx = IO_TAG(UART4_RX_PIN),
|
.rx = IO_TAG(UART4_RX_PIN),
|
||||||
.tx = IO_TAG(UART4_TX_PIN),
|
.tx = IO_TAG(UART4_TX_PIN),
|
||||||
.af = GPIO_AF8_UART4,
|
.af = GPIO_AF8_UART4,
|
||||||
#ifdef UART4_AHB1_PERIPHERALS
|
#ifdef UART4_AHB1_PERIPHERALS
|
||||||
.rcc_ahb1 = UART4_AHB1_PERIPHERALS,
|
.rcc_ahb1 = UART4_AHB1_PERIPHERALS,
|
||||||
|
@ -160,16 +160,16 @@ static uartDevice_t uart4 =
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_UART5
|
#ifdef USE_UART5
|
||||||
static uartDevice_t uart5 =
|
static uartDevice_t uart5 =
|
||||||
{
|
{
|
||||||
.DMAChannel = DMA_CHANNEL_4,
|
.DMAChannel = DMA_CHANNEL_4,
|
||||||
#ifdef USE_UART5_RX_DMA
|
#ifdef USE_UART5_RX_DMA
|
||||||
.rxDMAStream = DMA1_Stream0,
|
.rxDMAStream = DMA1_Stream0,
|
||||||
#endif
|
#endif
|
||||||
.txDMAStream = DMA1_Stream7,
|
.txDMAStream = DMA1_Stream7,
|
||||||
.dev = UART5,
|
.dev = UART5,
|
||||||
.rx = IO_TAG(UART5_RX_PIN),
|
.rx = IO_TAG(UART5_RX_PIN),
|
||||||
.tx = IO_TAG(UART5_TX_PIN),
|
.tx = IO_TAG(UART5_TX_PIN),
|
||||||
.af = GPIO_AF8_UART5,
|
.af = GPIO_AF8_UART5,
|
||||||
#ifdef UART5_AHB1_PERIPHERALS
|
#ifdef UART5_AHB1_PERIPHERALS
|
||||||
.rcc_ahb1 = UART5_AHB1_PERIPHERALS,
|
.rcc_ahb1 = UART5_AHB1_PERIPHERALS,
|
||||||
|
@ -183,16 +183,16 @@ static uartDevice_t uart5 =
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_UART6
|
#ifdef USE_UART6
|
||||||
static uartDevice_t uart6 =
|
static uartDevice_t uart6 =
|
||||||
{
|
{
|
||||||
.DMAChannel = DMA_CHANNEL_5,
|
.DMAChannel = DMA_CHANNEL_5,
|
||||||
#ifdef USE_UART6_RX_DMA
|
#ifdef USE_UART6_RX_DMA
|
||||||
.rxDMAStream = DMA2_Stream1,
|
.rxDMAStream = DMA2_Stream1,
|
||||||
#endif
|
#endif
|
||||||
.txDMAStream = DMA2_Stream6,
|
.txDMAStream = DMA2_Stream6,
|
||||||
.dev = USART6,
|
.dev = USART6,
|
||||||
.rx = IO_TAG(UART6_RX_PIN),
|
.rx = IO_TAG(UART6_RX_PIN),
|
||||||
.tx = IO_TAG(UART6_TX_PIN),
|
.tx = IO_TAG(UART6_TX_PIN),
|
||||||
.af = GPIO_AF8_USART6,
|
.af = GPIO_AF8_USART6,
|
||||||
#ifdef UART6_AHB1_PERIPHERALS
|
#ifdef UART6_AHB1_PERIPHERALS
|
||||||
.rcc_ahb1 = UART6_AHB1_PERIPHERALS,
|
.rcc_ahb1 = UART6_AHB1_PERIPHERALS,
|
||||||
|
@ -414,7 +414,7 @@ uartPort_t *serialUART(UARTDevice device, uint32_t baudRate, portMode_t mode, po
|
||||||
IOInit(tx, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
|
IOInit(tx, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
|
||||||
IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
|
IOConfigGPIOAF(tx, IOCFG_AF_PP, uart->af);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mode & MODE_RX) {
|
if (mode & MODE_RX) {
|
||||||
IOInit(rx, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
|
IOInit(rx, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
|
||||||
IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
|
IOConfigGPIOAF(rx, IOCFG_AF_PP, uart->af);
|
||||||
|
|
|
@ -167,16 +167,16 @@ static const struct serialPortVTable usbVTable[] = {
|
||||||
serialPort_t *usbVcpOpen(void)
|
serialPort_t *usbVcpOpen(void)
|
||||||
{
|
{
|
||||||
vcpPort_t *s;
|
vcpPort_t *s;
|
||||||
|
|
||||||
/* Init Device Library */
|
/* Init Device Library */
|
||||||
USBD_Init(&USBD_Device, &VCP_Desc, 0);
|
USBD_Init(&USBD_Device, &VCP_Desc, 0);
|
||||||
|
|
||||||
/* Add Supported Class */
|
/* Add Supported Class */
|
||||||
USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
|
USBD_RegisterClass(&USBD_Device, USBD_CDC_CLASS);
|
||||||
|
|
||||||
/* Add CDC Interface Class */
|
/* Add CDC Interface Class */
|
||||||
USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
|
USBD_CDC_RegisterInterface(&USBD_Device, &USBD_CDC_fops);
|
||||||
|
|
||||||
/* Start Device Process */
|
/* Start Device Process */
|
||||||
USBD_Start(&USBD_Device);
|
USBD_Start(&USBD_Device);
|
||||||
|
|
||||||
|
|
|
@ -32,7 +32,7 @@
|
||||||
typedef struct beeperConfig_s {
|
typedef struct beeperConfig_s {
|
||||||
ioTag_t ioTag;
|
ioTag_t ioTag;
|
||||||
uint8_t isInverted;
|
uint8_t isInverted;
|
||||||
uint8_t isOD;
|
uint8_t isOD;
|
||||||
} beeperConfig_t;
|
} beeperConfig_t;
|
||||||
|
|
||||||
void systemBeep(bool on);
|
void systemBeep(bool on);
|
||||||
|
|
|
@ -815,7 +815,7 @@ void timerOCPreloadConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t preload)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
volatile timCCR_t* timerCCR(TIM_TypeDef *tim, uint8_t channel)
|
volatile timCCR_t* timerCCR(TIM_TypeDef *tim, uint8_t channel)
|
||||||
{
|
{
|
||||||
|
|
|
@ -95,13 +95,13 @@ typedef struct timerHardware_s {
|
||||||
#endif
|
#endif
|
||||||
#if defined(USE_DSHOT) || defined(LED_STRIP)
|
#if defined(USE_DSHOT) || defined(LED_STRIP)
|
||||||
#if defined(STM32F4) || defined(STM32F7)
|
#if defined(STM32F4) || defined(STM32F7)
|
||||||
DMA_Stream_TypeDef *dmaStream;
|
DMA_Stream_TypeDef *dmaStream;
|
||||||
uint32_t dmaChannel;
|
uint32_t dmaChannel;
|
||||||
#elif defined(STM32F3) || defined(STM32F1)
|
#elif defined(STM32F3) || defined(STM32F1)
|
||||||
DMA_Channel_TypeDef *dmaChannel;
|
DMA_Channel_TypeDef *dmaChannel;
|
||||||
#endif
|
#endif
|
||||||
uint8_t dmaIrqHandler;
|
uint8_t dmaIrqHandler;
|
||||||
#endif
|
#endif
|
||||||
} timerHardware_t;
|
} timerHardware_t;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -122,7 +122,7 @@ typedef enum {
|
||||||
#endif
|
#endif
|
||||||
#elif defined(STM32F3)
|
#elif defined(STM32F3)
|
||||||
#define HARDWARE_TIMER_DEFINITION_COUNT 10
|
#define HARDWARE_TIMER_DEFINITION_COUNT 10
|
||||||
#elif defined(STM32F411xE)
|
#elif defined(STM32F411xE)
|
||||||
#define HARDWARE_TIMER_DEFINITION_COUNT 10
|
#define HARDWARE_TIMER_DEFINITION_COUNT 10
|
||||||
#elif defined(STM32F4)
|
#elif defined(STM32F4)
|
||||||
#define HARDWARE_TIMER_DEFINITION_COUNT 14
|
#define HARDWARE_TIMER_DEFINITION_COUNT 14
|
||||||
|
|
|
@ -73,21 +73,21 @@
|
||||||
#define DEF_TIM_DMA__TIM15_COM DMA1_CH5
|
#define DEF_TIM_DMA__TIM15_COM DMA1_CH5
|
||||||
#define DEF_TIM_DMA__TIM15_CH1N DMA1_CH5
|
#define DEF_TIM_DMA__TIM15_CH1N DMA1_CH5
|
||||||
|
|
||||||
#ifdef REMAP_TIM16_DMA
|
#ifdef REMAP_TIM16_DMA
|
||||||
#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6
|
#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH6
|
||||||
#define DEF_TIM_DMA__TIM16_UP DMA1_CH6
|
#define DEF_TIM_DMA__TIM16_UP DMA1_CH6
|
||||||
#else
|
#else
|
||||||
#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH3
|
#define DEF_TIM_DMA__TIM16_CH1 DMA1_CH3
|
||||||
#define DEF_TIM_DMA__TIM16_UP DMA1_CH3
|
#define DEF_TIM_DMA__TIM16_UP DMA1_CH3
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef REMAP_TIM17_DMA
|
#ifdef REMAP_TIM17_DMA
|
||||||
#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH7
|
#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH7
|
||||||
#define DEF_TIM_DMA__TIM17_UP DMA1_CH7
|
#define DEF_TIM_DMA__TIM17_UP DMA1_CH7
|
||||||
#else
|
#else
|
||||||
#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH1
|
#define DEF_TIM_DMA__TIM17_CH1 DMA1_CH1
|
||||||
#define DEF_TIM_DMA__TIM17_UP DMA1_CH1
|
#define DEF_TIM_DMA__TIM17_UP DMA1_CH1
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define DEF_TIM_DMA__TIM8_CH3 DMA2_CH1
|
#define DEF_TIM_DMA__TIM8_CH3 DMA2_CH1
|
||||||
#define DEF_TIM_DMA__TIM8_UP DMA2_CH1
|
#define DEF_TIM_DMA__TIM8_UP DMA2_CH1
|
||||||
|
@ -134,9 +134,9 @@
|
||||||
#define GPIO_AF__PA15_TIM8_CH1 GPIO_AF_2
|
#define GPIO_AF__PA15_TIM8_CH1 GPIO_AF_2
|
||||||
|
|
||||||
#define GPIO_AF__PA7_TIM8_CH1N GPIO_AF_4
|
#define GPIO_AF__PA7_TIM8_CH1N GPIO_AF_4
|
||||||
|
|
||||||
#define GPIO_AF__PA14_TIM4_CH2 GPIO_AF_5
|
#define GPIO_AF__PA14_TIM4_CH2 GPIO_AF_5
|
||||||
|
|
||||||
#define GPIO_AF__PA7_TIM1_CH1N GPIO_AF_6
|
#define GPIO_AF__PA7_TIM1_CH1N GPIO_AF_6
|
||||||
#define GPIO_AF__PA8_TIM1_CH1 GPIO_AF_6
|
#define GPIO_AF__PA8_TIM1_CH1 GPIO_AF_6
|
||||||
#define GPIO_AF__PA9_TIM1_CH2 GPIO_AF_6
|
#define GPIO_AF__PA9_TIM1_CH2 GPIO_AF_6
|
||||||
|
@ -165,7 +165,7 @@
|
||||||
#define GPIO_AF__PB11_TIM2_CH4 GPIO_AF_1
|
#define GPIO_AF__PB11_TIM2_CH4 GPIO_AF_1
|
||||||
#define GPIO_AF__PB14_TIM15_CH1 GPIO_AF_1
|
#define GPIO_AF__PB14_TIM15_CH1 GPIO_AF_1
|
||||||
#define GPIO_AF__PB15_TIM15_CH2 GPIO_AF_1
|
#define GPIO_AF__PB15_TIM15_CH2 GPIO_AF_1
|
||||||
|
|
||||||
#define GPIO_AF__PB0_TIM3_CH3 GPIO_AF_2
|
#define GPIO_AF__PB0_TIM3_CH3 GPIO_AF_2
|
||||||
#define GPIO_AF__PB1_TIM3_CH4 GPIO_AF_2
|
#define GPIO_AF__PB1_TIM3_CH4 GPIO_AF_2
|
||||||
#define GPIO_AF__PB4_TIM3_CH1 GPIO_AF_2
|
#define GPIO_AF__PB4_TIM3_CH1 GPIO_AF_2
|
||||||
|
@ -175,35 +175,35 @@
|
||||||
#define GPIO_AF__PB8_TIM4_CH3 GPIO_AF_2
|
#define GPIO_AF__PB8_TIM4_CH3 GPIO_AF_2
|
||||||
#define GPIO_AF__PB9_TIM4_CH4 GPIO_AF_2
|
#define GPIO_AF__PB9_TIM4_CH4 GPIO_AF_2
|
||||||
#define GPIO_AF__PB15_TIM15_CH1N GPIO_AF_2
|
#define GPIO_AF__PB15_TIM15_CH1N GPIO_AF_2
|
||||||
|
|
||||||
#define GPIO_AF__PB0_TIM8_CH2N GPIO_AF_4
|
#define GPIO_AF__PB0_TIM8_CH2N GPIO_AF_4
|
||||||
#define GPIO_AF__PB1_TIM8_CH3N GPIO_AF_4
|
#define GPIO_AF__PB1_TIM8_CH3N GPIO_AF_4
|
||||||
#define GPIO_AF__PB3_TIM8_CH1N GPIO_AF_4
|
#define GPIO_AF__PB3_TIM8_CH1N GPIO_AF_4
|
||||||
#define GPIO_AF__PB4_TIM8_CH2N GPIO_AF_4
|
#define GPIO_AF__PB4_TIM8_CH2N GPIO_AF_4
|
||||||
#define GPIO_AF__PB15_TIM1_CH3N GPIO_AF_4
|
#define GPIO_AF__PB15_TIM1_CH3N GPIO_AF_4
|
||||||
|
|
||||||
#define GPIO_AF__PB6_TIM8_CH1 GPIO_AF_5
|
#define GPIO_AF__PB6_TIM8_CH1 GPIO_AF_5
|
||||||
|
|
||||||
#define GPIO_AF__PB0_TIM1_CH2N GPIO_AF_6
|
#define GPIO_AF__PB0_TIM1_CH2N GPIO_AF_6
|
||||||
#define GPIO_AF__PB1_TIM1_CH3N GPIO_AF_6
|
#define GPIO_AF__PB1_TIM1_CH3N GPIO_AF_6
|
||||||
#define GPIO_AF__PB13_TIM1_CH1N GPIO_AF_6
|
#define GPIO_AF__PB13_TIM1_CH1N GPIO_AF_6
|
||||||
#define GPIO_AF__PB14_TIM1_CH2N GPIO_AF_6
|
#define GPIO_AF__PB14_TIM1_CH2N GPIO_AF_6
|
||||||
|
|
||||||
#define GPIO_AF__PB5_TIM17_CH1 GPIO_AF_10
|
#define GPIO_AF__PB5_TIM17_CH1 GPIO_AF_10
|
||||||
#define GPIO_AF__PB7_TIM3_CH4 GPIO_AF_10
|
#define GPIO_AF__PB7_TIM3_CH4 GPIO_AF_10
|
||||||
#define GPIO_AF__PB8_TIM8_CH2 GPIO_AF_10
|
#define GPIO_AF__PB8_TIM8_CH2 GPIO_AF_10
|
||||||
#define GPIO_AF__PB9_TIM8_CH3 GPIO_AF_10
|
#define GPIO_AF__PB9_TIM8_CH3 GPIO_AF_10
|
||||||
|
|
||||||
#define GPIO_AF__PC6_TIM3_CH1 GPIO_AF_2
|
#define GPIO_AF__PC6_TIM3_CH1 GPIO_AF_2
|
||||||
#define GPIO_AF__PC7_TIM3_CH2 GPIO_AF_2
|
#define GPIO_AF__PC7_TIM3_CH2 GPIO_AF_2
|
||||||
#define GPIO_AF__PC8_TIM3_CH3 GPIO_AF_2
|
#define GPIO_AF__PC8_TIM3_CH3 GPIO_AF_2
|
||||||
#define GPIO_AF__PC9_TIM3_CH4 GPIO_AF_2
|
#define GPIO_AF__PC9_TIM3_CH4 GPIO_AF_2
|
||||||
|
|
||||||
#define GPIO_AF__PC6_TIM8_CH1 GPIO_AF_4
|
#define GPIO_AF__PC6_TIM8_CH1 GPIO_AF_4
|
||||||
#define GPIO_AF__PC7_TIM8_CH2 GPIO_AF_4
|
#define GPIO_AF__PC7_TIM8_CH2 GPIO_AF_4
|
||||||
#define GPIO_AF__PC8_TIM8_CH3 GPIO_AF_4
|
#define GPIO_AF__PC8_TIM8_CH3 GPIO_AF_4
|
||||||
#define GPIO_AF__PC9_TIM8_CH4 GPIO_AF_4
|
#define GPIO_AF__PC9_TIM8_CH4 GPIO_AF_4
|
||||||
|
|
||||||
#define GPIO_AF__PC10_TIM8_CH1N GPIO_AF_4
|
#define GPIO_AF__PC10_TIM8_CH1N GPIO_AF_4
|
||||||
#define GPIO_AF__PC11_TIM8_CH2N GPIO_AF_4
|
#define GPIO_AF__PC11_TIM8_CH2N GPIO_AF_4
|
||||||
#define GPIO_AF__PC12_TIM8_CH3N GPIO_AF_4
|
#define GPIO_AF__PC12_TIM8_CH3N GPIO_AF_4
|
||||||
|
@ -252,11 +252,11 @@
|
||||||
|
|
||||||
#define DEF_TIM_DMA_STR_0__TIM2_CH1 DMA1_ST5
|
#define DEF_TIM_DMA_STR_0__TIM2_CH1 DMA1_ST5
|
||||||
#define DEF_TIM_DMA_STR_0__TIM2_CH2 DMA1_ST6
|
#define DEF_TIM_DMA_STR_0__TIM2_CH2 DMA1_ST6
|
||||||
#define DEF_TIM_DMA_STR_0__TIM2_CH3 DMA1_ST1
|
#define DEF_TIM_DMA_STR_0__TIM2_CH3 DMA1_ST1
|
||||||
#define DEF_TIM_DMA_STR_0__TIM2_CH4 DMA1_ST7
|
#define DEF_TIM_DMA_STR_0__TIM2_CH4 DMA1_ST7
|
||||||
#define DEF_TIM_DMA_STR_1__TIM2_CH4 DMA1_ST6
|
#define DEF_TIM_DMA_STR_1__TIM2_CH4 DMA1_ST6
|
||||||
|
|
||||||
#define DEF_TIM_DMA_STR_0__TIM3_CH1 DMA1_ST4
|
#define DEF_TIM_DMA_STR_0__TIM3_CH1 DMA1_ST4
|
||||||
#define DEF_TIM_DMA_STR_0__TIM3_CH2 DMA1_ST5
|
#define DEF_TIM_DMA_STR_0__TIM3_CH2 DMA1_ST5
|
||||||
#define DEF_TIM_DMA_STR_0__TIM3_CH3 DMA1_ST7
|
#define DEF_TIM_DMA_STR_0__TIM3_CH3 DMA1_ST7
|
||||||
#define DEF_TIM_DMA_STR_0__TIM3_CH4 DMA1_ST2
|
#define DEF_TIM_DMA_STR_0__TIM3_CH4 DMA1_ST2
|
||||||
|
@ -306,11 +306,11 @@
|
||||||
|
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM2_CH1 DMA_Channel_3
|
#define DEF_TIM_DMA_CHN_0__TIM2_CH1 DMA_Channel_3
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM2_CH2 DMA_Channel_3
|
#define DEF_TIM_DMA_CHN_0__TIM2_CH2 DMA_Channel_3
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM2_CH3 DMA_Channel_3
|
#define DEF_TIM_DMA_CHN_0__TIM2_CH3 DMA_Channel_3
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM2_CH4 DMA_Channel_3
|
#define DEF_TIM_DMA_CHN_0__TIM2_CH4 DMA_Channel_3
|
||||||
#define DEF_TIM_DMA_CHN_1__TIM2_CH4 DMA_Channel_3
|
#define DEF_TIM_DMA_CHN_1__TIM2_CH4 DMA_Channel_3
|
||||||
|
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM3_CH1 DMA_Channel_5
|
#define DEF_TIM_DMA_CHN_0__TIM3_CH1 DMA_Channel_5
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM3_CH2 DMA_Channel_5
|
#define DEF_TIM_DMA_CHN_0__TIM3_CH2 DMA_Channel_5
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM3_CH3 DMA_Channel_5
|
#define DEF_TIM_DMA_CHN_0__TIM3_CH3 DMA_Channel_5
|
||||||
#define DEF_TIM_DMA_CHN_0__TIM3_CH4 DMA_Channel_5
|
#define DEF_TIM_DMA_CHN_0__TIM3_CH4 DMA_Channel_5
|
||||||
|
@ -371,7 +371,7 @@
|
||||||
#define DMA_NONE_STREAM NULL
|
#define DMA_NONE_STREAM NULL
|
||||||
|
|
||||||
|
|
||||||
#define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan
|
#define DEF_TIM_CHAN(chan) DEF_CHAN_ ## chan
|
||||||
#define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out )
|
#define DEF_TIM_OUTPUT(chan, out) ( DEF_CHAN_ ## chan ## _OUTPUT | out )
|
||||||
|
|
||||||
#define DMA_NONE_HANDLER 0
|
#define DMA_NONE_HANDLER 0
|
||||||
|
@ -384,8 +384,8 @@
|
||||||
#define DEF_CHAN_CH2N TIM_Channel_2
|
#define DEF_CHAN_CH2N TIM_Channel_2
|
||||||
#define DEF_CHAN_CH3N TIM_Channel_3
|
#define DEF_CHAN_CH3N TIM_Channel_3
|
||||||
#define DEF_CHAN_CH4N TIM_Channel_4
|
#define DEF_CHAN_CH4N TIM_Channel_4
|
||||||
|
|
||||||
#define DEF_CHAN_CH1_OUTPUT TIMER_OUTPUT_NONE
|
#define DEF_CHAN_CH1_OUTPUT TIMER_OUTPUT_NONE
|
||||||
#define DEF_CHAN_CH2_OUTPUT TIMER_OUTPUT_NONE
|
#define DEF_CHAN_CH2_OUTPUT TIMER_OUTPUT_NONE
|
||||||
#define DEF_CHAN_CH3_OUTPUT TIMER_OUTPUT_NONE
|
#define DEF_CHAN_CH3_OUTPUT TIMER_OUTPUT_NONE
|
||||||
#define DEF_CHAN_CH4_OUTPUT TIMER_OUTPUT_NONE
|
#define DEF_CHAN_CH4_OUTPUT TIMER_OUTPUT_NONE
|
||||||
|
|
|
@ -70,7 +70,7 @@ typedef struct {
|
||||||
} timerInfo_t;
|
} timerInfo_t;
|
||||||
timerInfo_t timerInfo[USED_TIMER_COUNT+1];
|
timerInfo_t timerInfo[USED_TIMER_COUNT+1];
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
{
|
{
|
||||||
TIM_HandleTypeDef Handle;
|
TIM_HandleTypeDef Handle;
|
||||||
} timerHandle_t;
|
} timerHandle_t;
|
||||||
|
@ -237,7 +237,7 @@ TIM_HandleTypeDef* timerFindTimerHandle(TIM_TypeDef *tim)
|
||||||
uint8_t timerIndex = lookupTimerIndex(tim);
|
uint8_t timerIndex = lookupTimerIndex(tim);
|
||||||
if (timerIndex >= USED_TIMER_COUNT)
|
if (timerIndex >= USED_TIMER_COUNT)
|
||||||
return NULL;
|
return NULL;
|
||||||
|
|
||||||
return &timerHandle[timerIndex].Handle;
|
return &timerHandle[timerIndex].Handle;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -252,16 +252,16 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
|
||||||
// already configured
|
// already configured
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
timerHandle[timerIndex].Handle.Instance = tim;
|
timerHandle[timerIndex].Handle.Instance = tim;
|
||||||
|
|
||||||
timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
|
timerHandle[timerIndex].Handle.Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR
|
||||||
timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
|
timerHandle[timerIndex].Handle.Init.Prescaler = (SystemCoreClock / timerClockDivisor(tim) / ((uint32_t)mhz * 1000000)) - 1;
|
||||||
|
|
||||||
timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
timerHandle[timerIndex].Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
|
||||||
timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
|
timerHandle[timerIndex].Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||||
timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000;
|
timerHandle[timerIndex].Handle.Init.RepetitionCounter = 0x0000;
|
||||||
|
|
||||||
HAL_TIM_Base_Init(&timerHandle[timerIndex].Handle);
|
HAL_TIM_Base_Init(&timerHandle[timerIndex].Handle);
|
||||||
if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9)
|
if(tim == TIM1 || tim == TIM2 || tim == TIM3 || tim == TIM4 || tim == TIM5 || tim == TIM8 || tim == TIM9)
|
||||||
{
|
{
|
||||||
|
@ -292,7 +292,7 @@ void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, ui
|
||||||
if (timerIndex >= USED_TIMER_COUNT) {
|
if (timerIndex >= USED_TIMER_COUNT) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
configTimeBase(timerHardwarePtr->tim, period, mhz);
|
configTimeBase(timerHardwarePtr->tim, period, mhz);
|
||||||
HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
|
HAL_TIM_Base_Start(&timerHandle[timerIndex].Handle);
|
||||||
|
|
||||||
|
@ -357,7 +357,7 @@ static void timerChConfig_UpdateOverflow(timerConfig_t *cfg, TIM_TypeDef *tim) {
|
||||||
if (timerIndex >= USED_TIMER_COUNT) {
|
if (timerIndex >= USED_TIMER_COUNT) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
|
timerOvrHandlerRec_t **chain = &cfg->overflowCallbackActive;
|
||||||
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
|
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
|
||||||
for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
|
for(int i = 0; i < CC_CHANNELS_PER_TIMER; i++)
|
||||||
|
@ -441,7 +441,7 @@ void timerChITConfigDualLo(const timerHardware_t *timHw, FunctionalState newStat
|
||||||
if (timerIndex >= USED_TIMER_COUNT) {
|
if (timerIndex >= USED_TIMER_COUNT) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(newState)
|
if(newState)
|
||||||
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
|
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel&~TIM_CHANNEL_2));
|
||||||
else
|
else
|
||||||
|
@ -460,7 +460,7 @@ void timerChITConfig(const timerHardware_t *timHw, FunctionalState newState)
|
||||||
if (timerIndex >= USED_TIMER_COUNT) {
|
if (timerIndex >= USED_TIMER_COUNT) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(newState)
|
if(newState)
|
||||||
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
|
__HAL_TIM_ENABLE_IT(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
|
||||||
else
|
else
|
||||||
|
@ -479,7 +479,7 @@ void timerChClearCCFlag(const timerHardware_t *timHw)
|
||||||
if (timerIndex >= USED_TIMER_COUNT) {
|
if (timerIndex >= USED_TIMER_COUNT) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
__HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
|
__HAL_TIM_CLEAR_FLAG(&timerHandle[timerIndex].Handle, TIM_IT_CCx(timHw->channel));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -744,8 +744,8 @@ _TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM17_IRQHandler, 17);
|
||||||
void timerInit(void)
|
void timerInit(void)
|
||||||
{
|
{
|
||||||
memset(timerConfig, 0, sizeof (timerConfig));
|
memset(timerConfig, 0, sizeof (timerConfig));
|
||||||
|
|
||||||
|
|
||||||
#if USED_TIMERS & TIM_N(1)
|
#if USED_TIMERS & TIM_N(1)
|
||||||
__HAL_RCC_TIM1_CLK_ENABLE();
|
__HAL_RCC_TIM1_CLK_ENABLE();
|
||||||
#endif
|
#endif
|
||||||
|
@ -874,7 +874,7 @@ const timerHardware_t *timerGetByTag(ioTag_t tag, timerUsageFlag_e flag)
|
||||||
if (timerHardware[i].tag == tag) {
|
if (timerHardware[i].tag == tag) {
|
||||||
if (timerHardware[i].output & flag) {
|
if (timerHardware[i].output & flag) {
|
||||||
return &timerHardware[i];
|
return &timerHardware[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return NULL;
|
return NULL;
|
||||||
|
|
|
@ -62,18 +62,18 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
||||||
#endif
|
#endif
|
||||||
};
|
};
|
||||||
|
|
||||||
/*
|
/*
|
||||||
need a mapping from dma and timers to pins, and the values should all be set here to the dmaMotors array.
|
need a mapping from dma and timers to pins, and the values should all be set here to the dmaMotors array.
|
||||||
this mapping could be used for both these motors and for led strip.
|
this mapping could be used for both these motors and for led strip.
|
||||||
|
|
||||||
only certain pins have OC output (already used in normal PWM et al) but then
|
only certain pins have OC output (already used in normal PWM et al) but then
|
||||||
there are only certain DMA streams/channels available for certain timers and channels.
|
there are only certain DMA streams/channels available for certain timers and channels.
|
||||||
*** (this may highlight some hardware limitations on some targets) ***
|
*** (this may highlight some hardware limitations on some targets) ***
|
||||||
|
|
||||||
DMA1
|
DMA1
|
||||||
|
|
||||||
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
||||||
0
|
0
|
||||||
1
|
1
|
||||||
2 TIM4_CH1 TIM4_CH2 TIM4_CH3
|
2 TIM4_CH1 TIM4_CH2 TIM4_CH3
|
||||||
3 TIM2_CH3 TIM2_CH1 TIM2_CH1 TIM2_CH4
|
3 TIM2_CH3 TIM2_CH1 TIM2_CH1 TIM2_CH4
|
||||||
|
@ -81,10 +81,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
||||||
4
|
4
|
||||||
5 TIM3_CH4 TIM3_CH1 TIM3_CH2 TIM3_CH3
|
5 TIM3_CH4 TIM3_CH1 TIM3_CH2 TIM3_CH3
|
||||||
6 TIM5_CH3 TIM5_CH4 TIM5_CH1 TIM5_CH4 TIM5_CH2
|
6 TIM5_CH3 TIM5_CH4 TIM5_CH1 TIM5_CH4 TIM5_CH2
|
||||||
7
|
7
|
||||||
|
|
||||||
DMA2
|
DMA2
|
||||||
|
|
||||||
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
||||||
0 TIM8_CH1 TIM1_CH1
|
0 TIM8_CH1 TIM1_CH1
|
||||||
TIM8_CH2 TIM1_CH2
|
TIM8_CH2 TIM1_CH2
|
||||||
|
@ -101,13 +101,13 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
||||||
uint8_t timerClockDivisor(TIM_TypeDef *tim)
|
uint8_t timerClockDivisor(TIM_TypeDef *tim)
|
||||||
{
|
{
|
||||||
#if defined (STM32F40_41xxx)
|
#if defined (STM32F40_41xxx)
|
||||||
if (tim == TIM8) return 1;
|
if (tim == TIM8) return 1;
|
||||||
#endif
|
#endif
|
||||||
if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
|
if (tim == TIM1 || tim == TIM9 || tim == TIM10 || tim == TIM11) {
|
||||||
return 1;
|
return 1;
|
||||||
} else {
|
} else {
|
||||||
return 2;
|
return 2;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
|
void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode)
|
||||||
|
|
|
@ -58,18 +58,18 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
||||||
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn},
|
{ .TIMx = TIM14, .rcc = RCC_APB1(TIM14), .inputIrq = TIM8_TRG_COM_TIM14_IRQn},
|
||||||
};
|
};
|
||||||
|
|
||||||
/*
|
/*
|
||||||
need a mapping from dma and timers to pins, and the values should all be set here to the dmaMotors array.
|
need a mapping from dma and timers to pins, and the values should all be set here to the dmaMotors array.
|
||||||
this mapping could be used for both these motors and for led strip.
|
this mapping could be used for both these motors and for led strip.
|
||||||
|
|
||||||
only certain pins have OC output (already used in normal PWM et al) but then
|
only certain pins have OC output (already used in normal PWM et al) but then
|
||||||
there are only certain DMA streams/channels available for certain timers and channels.
|
there are only certain DMA streams/channels available for certain timers and channels.
|
||||||
*** (this may highlight some hardware limitations on some targets) ***
|
*** (this may highlight some hardware limitations on some targets) ***
|
||||||
|
|
||||||
DMA1
|
DMA1
|
||||||
|
|
||||||
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
||||||
0
|
0
|
||||||
1
|
1
|
||||||
2 TIM4_CH1 TIM4_CH2 TIM4_CH3
|
2 TIM4_CH1 TIM4_CH2 TIM4_CH3
|
||||||
3 TIM2_CH3 TIM2_CH1 TIM2_CH1 TIM2_CH4
|
3 TIM2_CH3 TIM2_CH1 TIM2_CH1 TIM2_CH4
|
||||||
|
@ -77,10 +77,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = {
|
||||||
4
|
4
|
||||||
5 TIM3_CH4 TIM3_CH1 TIM3_CH2 TIM3_CH3
|
5 TIM3_CH4 TIM3_CH1 TIM3_CH2 TIM3_CH3
|
||||||
6 TIM5_CH3 TIM5_CH4 TIM5_CH1 TIM5_CH4 TIM5_CH2
|
6 TIM5_CH3 TIM5_CH4 TIM5_CH1 TIM5_CH4 TIM5_CH2
|
||||||
7
|
7
|
||||||
|
|
||||||
DMA2
|
DMA2
|
||||||
|
|
||||||
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
Channel Stream0 Stream1 Stream2 Stream3 Stream4 Stream5 Stream6 Stream7
|
||||||
0 TIM8_CH1 TIM1_CH1
|
0 TIM8_CH1 TIM1_CH1
|
||||||
TIM8_CH2 TIM1_CH2
|
TIM8_CH2 TIM1_CH2
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue