diff --git a/src/main/drivers/transponder_ir.c b/src/main/drivers/transponder_ir.c index 590f68e123..efe6a1b022 100644 --- a/src/main/drivers/transponder_ir.c +++ b/src/main/drivers/transponder_ir.c @@ -34,6 +34,17 @@ #define TRANSPONDER_TIMER_MHZ 24 #define TRANSPONDER_CARRIER_HZ 460750 +static uint8_t bitToggleOne = 0; +#define BIT_TOGGLE_0 0 + +#define TRANSPONDER_BITS_PER_BYTE 10 // start + 8 data + stop +#define TRANSPONDER_DATA_LENGTH 6 +#define TRANSPONDER_TOGGLES_PER_BIT 11 +#define TRANSPONDER_GAP_TOGGLES 1 +#define TRANSPONDER_TOGGLES (TRANSPONDER_TOGGLES_PER_BIT + TRANSPONDER_GAP_TOGGLES) + +#define TRANSPONDER_DMA_BUFFER_SIZE ((TRANSPONDER_TOGGLES_PER_BIT + 1) * TRANSPONDER_BITS_PER_BYTE * TRANSPONDER_DATA_LENGTH) + /* * Implementation note: * Using around over 700 bytes for a transponder DMA buffer is a little excessive, likely an alternative implementation that uses a fast @@ -42,12 +53,16 @@ * * On an STM32F303CC 720 bytes is currently fine and that is the target for which this code was designed for. */ +#if defined(STM32F3) uint8_t transponderIrDMABuffer[TRANSPONDER_DMA_BUFFER_SIZE]; +#elif defined(STM32F4) +uint32_t transponderIrDMABuffer[TRANSPONDER_DMA_BUFFER_SIZE]; +#else +#error "Transponder not supported on this MCU." +#endif volatile uint8_t transponderIrDataTransferInProgress = 0; -static uint8_t bitToggleOne = 0; -#define BIT_TOGGLE_0 0 static IO_t transponderIO = IO_NONE; static TIM_TypeDef *timer = NULL; @@ -104,7 +119,7 @@ void transponderIrHardwareInit(ioTag_t ioTag) TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = period; TIM_TimeBaseStructure.TIM_Prescaler = prescaler; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(timer, &TIM_TimeBaseStructure); @@ -145,8 +160,14 @@ void transponderIrHardwareInit(ioTag_t ioTag) DMA_InitStructure.DMA_BufferSize = TRANSPONDER_DMA_BUFFER_SIZE; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; +#if defined(STM32F3) DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; +#elif defined(STM32F4) + + DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; + DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; +#endif DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; DMA_InitStructure.DMA_Priority = DMA_Priority_High; @@ -270,4 +291,4 @@ void transponderIrTransmit(void) transponderIrDataTransferInProgress = 1; transponderIrDMAEnable(); } -#endif \ No newline at end of file +#endif diff --git a/src/main/drivers/transponder_ir.h b/src/main/drivers/transponder_ir.h index 0defc55cbd..fb0c276427 100644 --- a/src/main/drivers/transponder_ir.h +++ b/src/main/drivers/transponder_ir.h @@ -19,14 +19,6 @@ #include "io_types.h" -#define TRANSPONDER_BITS_PER_BYTE 10 // start + 8 data + stop -#define TRANSPONDER_DATA_LENGTH 6 -#define TRANSPONDER_TOGGLES_PER_BIT 11 -#define TRANSPONDER_GAP_TOGGLES 1 -#define TRANSPONDER_TOGGLES (TRANSPONDER_TOGGLES_PER_BIT + TRANSPONDER_GAP_TOGGLES) - -#define TRANSPONDER_DMA_BUFFER_SIZE ((TRANSPONDER_TOGGLES_PER_BIT + 1) * TRANSPONDER_BITS_PER_BYTE * TRANSPONDER_DATA_LENGTH) - bool transponderIrInit(); void transponderIrDisable(void); @@ -40,5 +32,4 @@ void transponderIrTransmit(void); bool isTransponderIrReady(void); -extern uint8_t transponderIrDMABuffer[TRANSPONDER_DMA_BUFFER_SIZE]; extern volatile uint8_t transponderIrDataTransferInProgress;