diff --git a/Makefile b/Makefile index b8c891ef62..e41d2d19e4 100644 --- a/Makefile +++ b/Makefile @@ -600,6 +600,7 @@ LDFLAGS = -lm \ -Wl,-gc-sections,-Map,$(TARGET_MAP) \ -Wl,-L$(LINKER_DIR) \ -Wl,--cref \ + -Wl,--no-wchar-size-warning \ -T$(LD_SCRIPT) ############################################################################### diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 21fd2948f8..d059fe882f 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -17,76 +17,81 @@ #include #include -#include #include -#include "common/axis.h" #include "common/filter.h" #include "common/maths.h" -#define M_LN2_FLOAT 0.69314718055994530942f -#define M_PI_FLOAT 3.14159265358979323846f +#define M_LN2_FLOAT 0.69314718055994530942f +#define M_PI_FLOAT 3.14159265358979323846f #define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ #define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ -// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime) -float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT) { +// PT1 Low Pass filter - // Pre calculate and store RC - if (!filter->RC) { - filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); - } +void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) +{ + filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->dT = dT; +} - filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); +float pt1FilterApply(pt1Filter_t *filter, float input) +{ + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); + return filter->state; +} + +float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) +{ + // Pre calculate and store RC + if (!filter->RC) { + filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->dT = dT; + } + + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); return filter->state; } /* sets up a biquad Filter */ -void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate) +void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate) { - float sampleRate; + const float sampleRate = 1 / ((float)refreshRate * 0.000001f); - sampleRate = 1 / ((float)refreshRate * 0.000001f); - - float omega, sn, cs, alpha; - float a0, a1, a2, b0, b1, b2; - - /* setup variables */ - omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; - sn = sinf(omega); - cs = cosf(omega); + // setup variables + const float omega = 2 * M_PI_FLOAT * filterCutFreq / sampleRate; + const float sn = sinf(omega); + const float cs = cosf(omega); //this is wrong, should be hyperbolic sine //alpha = sn * sinf(M_LN2_FLOAT /2 * BIQUAD_BANDWIDTH * omega /sn); - alpha = sn / (2 * BIQUAD_Q); + const float alpha = sn / (2 * BIQUAD_Q); - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; + const float b0 = (1 - cs) / 2; + const float b1 = 1 - cs; + const float b2 = (1 - cs) / 2; + const float a0 = 1 + alpha; + const float a1 = -2 * cs; + const float a2 = 1 - alpha; - /* precompute the coefficients */ - newState->b0 = b0 / a0; - newState->b1 = b1 / a0; - newState->b2 = b2 / a0; - newState->a1 = a1 / a0; - newState->a2 = a2 / a0; + // precompute the coefficients + filter->b0 = b0 / a0; + filter->b1 = b1 / a0; + filter->b2 = b2 / a0; + filter->a1 = a1 / a0; + filter->a2 = a2 / a0; - /* zero initial samples */ - newState->d1 = newState->d2 = 1; + // zero initial samples + filter->d1 = filter->d2 = 0; } /* Computes a biquad_t filter on a sample */ -float applyBiQuadFilter(float sample, biquad_t *state) //direct form 2 transposed +float biquadFilterApply(biquadFilter_t *filter, float input) { - float result; - - result = state->b0 * sample + state->d1; - state->d1 = state->b1 * sample - state->a1 * result + state->d2; - state->d2 = state->b2 * sample - state->a2 * result; + const float result = filter->b0 * input + filter->d1; + filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; + filter->d2 = filter->b2 * input - filter->a2 * result; return result; } diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 809063f4b0..66fcb53bb0 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -17,20 +17,26 @@ #define DELTA_MAX_SAMPLES 12 -typedef struct filterStatePt1_s { - float state; - float RC; - float constdT; -} filterStatePt1_t; +typedef struct pt1Filter_s { + float state; + float RC; + float dT; +} pt1Filter_t; /* this holds the data required to update samples thru a filter */ -typedef struct biquad_s { +typedef struct biquadFilter_s { float b0, b1, b2, a1, a2; float d1, d2; -} biquad_t; +} biquadFilter_t; + + +void biquadFilterInit(biquadFilter_t *filter, float filterCutFreq, uint32_t refreshRate); +float biquadFilterApply(biquadFilter_t *filter, float input); + +void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT); +float pt1FilterApply(pt1Filter_t *filter, float input); +float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT); -float applyBiQuadFilter(float sample, biquad_t *state); -float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt); int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]); float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]); -void BiQuadNewLpf(float filterCutFreq, biquad_t *newState, uint32_t refreshRate); + diff --git a/src/main/config/config.c b/src/main/config/config.c index 5bcd1e923b..bc8d830a59 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -645,7 +645,13 @@ static void resetConf(void) masterConfig.blackbox_rate_denom = 1; #endif // BLACKBOX - + +#ifdef SERIALRX_UART + if (featureConfigured(FEATURE_RX_SERIAL)) { + masterConfig.serialConfig.portConfigs[SERIALRX_UART].functionMask = FUNCTION_RX_SERIAL; + } +#endif + // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.escAndServoConfig.minthrottle = 1025; @@ -660,11 +666,6 @@ static void resetConf(void) #if defined(ALIENFLIGHT) featureClear(FEATURE_ONESHOT125); -#ifdef ALIENFLIGHTF1 - masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; -#else - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; -#endif #ifdef ALIENFLIGHTF3 masterConfig.mag_hardware = MAG_NONE; // disabled by default #endif @@ -688,11 +689,6 @@ static void resetConf(void) masterConfig.customMotorMixer[7] = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L#endif #endif -#if defined(SINGULARITY) - // alternative defaults settings for SINGULARITY target - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; -#endif - // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); @@ -848,10 +844,10 @@ void validateAndFixConfig(void) if (featureConfigured(FEATURE_SOFTSERIAL) && ( 0 #ifdef USE_SOFTSERIAL1 - || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) + || (WS2811_TIMER == SOFTSERIAL_1_TIMER) #endif #ifdef USE_SOFTSERIAL2 - || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) + || (WS2811_TIMER == SOFTSERIAL_2_TIMER) #endif )) { // led strip needs the same timer as softserial @@ -901,15 +897,11 @@ void validateAndFixConfig(void) #if defined(COLIBRI_RACE) masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP; - if(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) { + if (featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_MSP)) { featureClear(FEATURE_RX_PARALLEL_PWM); featureClear(FEATURE_RX_MSP); featureSet(FEATURE_RX_PPM); } - if(featureConfigured(FEATURE_RX_SERIAL)) { - masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; - //masterConfig.rxConfig.serialrx_provider = SERIALRX_SBUS; - } #endif useRxConfig(&masterConfig.rxConfig); diff --git a/src/main/drivers/io.h b/src/main/drivers/io.h index debc78d950..315560e430 100644 --- a/src/main/drivers/io.h +++ b/src/main/drivers/io.h @@ -52,6 +52,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) #define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO +#define IOCFG_OUT_PP_25 IO_CONFIG(GPIO_Mode_OUT, GPIO_Speed_25MHz, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) #define IOCFG_AF_PP_PD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_DOWN) @@ -60,6 +61,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration #define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN) #define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP) #define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL) +#define IOCFG_IPU_25 IO_CONFIG(GPIO_Mode_IN, GPIO_Speed_25MHz, 0, GPIO_PuPd_UP) #elif defined(UNIT_TEST) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 3821350f57..5bd5857b56 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -80,18 +80,9 @@ void setStripColors(const hsvColor_t *colors) } } -void ws2811DMAHandler(DMA_Channel_TypeDef *channel) { - if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) { - ws2811LedDataTransferInProgress = 0; - DMA_Cmd(channel, DISABLE); - DMA_ClearFlag(WS2811_DMA_TC_FLAG); - } -} - void ws2811LedStripInit(void) { memset(&ledStripDMABuffer, 0, WS2811_DMA_BUFFER_SIZE); - dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); ws2811LedStripHardwareInit(); ws2811UpdateStrip(); } @@ -141,12 +132,11 @@ STATIC_UNIT_TESTED void updateLEDDMABuffer(uint8_t componentValue) */ void ws2811UpdateStrip(void) { - static uint32_t waitCounter = 0; static rgbColor24bpp_t *rgb24; - // wait until previous transfer completes - while(ws2811LedDataTransferInProgress) { - waitCounter++; + // don't wait - risk of infinite block, just get an update next time round + if (ws2811LedDataTransferInProgress) { + return; } dmaBufferOffset = 0; // reset buffer memory index diff --git a/src/main/drivers/light_ws2811strip_stm32f10x.c b/src/main/drivers/light_ws2811strip_stm32f10x.c index b7f1bd4138..40003663d9 100644 --- a/src/main/drivers/light_ws2811strip_stm32f10x.c +++ b/src/main/drivers/light_ws2811strip_stm32f10x.c @@ -23,35 +23,41 @@ #include "common/color.h" #include "drivers/light_ws2811strip.h" #include "nvic.h" +#include "io.h" +#include "dma.h" +#include "timer.h" + +#ifdef LED_STRIP + +static IO_t ws2811IO = IO_NONE; +bool ws2811Initialised = false; + +void ws2811DMAHandler(DMA_Channel_TypeDef *channel) +{ + if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) { + ws2811LedDataTransferInProgress = 0; + DMA_Cmd(channel, DISABLE); + DMA_ClearFlag(WS2811_DMA_TC_FLAG); + } +} void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; - GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; -#ifdef CC3D - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOB, &GPIO_InitStructure); -#else - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - - /* GPIOA Configuration: TIM3 Channel 1 as alternate function push-pull */ - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); -#endif - - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); + + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); +/* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIO(ws2811IO, IO_CONFIG(GPIO_Speed_50MHz, GPIO_Mode_AF_PP)); + + RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; /* Time base configuration */ @@ -108,16 +114,20 @@ void ws2811LedStripHardwareInit(void) NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) { + if (!ws2811Initialised) + return; + DMA_SetCurrDataCounter(DMA1_Channel6, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred TIM_SetCounter(TIM3, 0); TIM_Cmd(TIM3, ENABLE); DMA_Cmd(DMA1_Channel6, ENABLE); } - +#endif \ No newline at end of file diff --git a/src/main/drivers/light_ws2811strip_stm32f30x.c b/src/main/drivers/light_ws2811strip_stm32f30x.c index fbcc8d3fb0..567fd0b8e5 100644 --- a/src/main/drivers/light_ws2811strip_stm32f30x.c +++ b/src/main/drivers/light_ws2811strip_stm32f30x.c @@ -20,51 +20,54 @@ #include -#include "gpio.h" +#include "io.h" #include "nvic.h" #include "common/color.h" #include "drivers/light_ws2811strip.h" +#include "dma.h" +#include "rcc.h" +#include "timer.h" -#ifndef WS2811_GPIO -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#ifdef LED_STRIP + +#ifndef WS2811_PIN +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER - #endif +static IO_t ws2811IO = IO_NONE; +bool ws2811Initialised = false; + +void ws2811DMAHandler(DMA_Channel_TypeDef *channel) +{ + if (DMA_GetFlagStatus(WS2811_DMA_TC_FLAG)) { + ws2811LedDataTransferInProgress = 0; + DMA_Cmd(channel, DISABLE); + DMA_ClearFlag(WS2811_DMA_TC_FLAG); + } +} + void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; - GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; - - RCC_AHBPeriphClockCmd(WS2811_GPIO_AHB_PERIPHERAL, ENABLE); - - GPIO_PinAFConfig(WS2811_GPIO, WS2811_PIN_SOURCE, WS2811_GPIO_AF); - - /* Configuration alternate function push-pull */ - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = WS2811_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(WS2811_GPIO, &GPIO_InitStructure); - - - RCC_APB2PeriphClockCmd(WS2811_TIMER_APB2_PERIPHERAL, ENABLE); + + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); + + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ + IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerGPIOAF(WS2811_TIMER)); + + RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); /* Compute the prescaler value */ prescalerValue = (uint16_t) (SystemCoreClock / 24000000) - 1; @@ -122,16 +125,20 @@ void ws2811LedStripHardwareInit(void) NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } void ws2811LedStripDMAEnable(void) { + if (!ws2811Initialised) + return; + DMA_SetCurrDataCounter(WS2811_DMA_CHANNEL, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred TIM_SetCounter(WS2811_TIMER, 0); TIM_Cmd(WS2811_TIMER, ENABLE); DMA_Cmd(WS2811_DMA_CHANNEL, ENABLE); } - +#endif \ No newline at end of file diff --git a/src/main/drivers/light_ws2811strip_stm32f4xx.c b/src/main/drivers/light_ws2811strip_stm32f4xx.c index d85f1c5757..f1d04dfbee 100644 --- a/src/main/drivers/light_ws2811strip_stm32f4xx.c +++ b/src/main/drivers/light_ws2811strip_stm32f4xx.c @@ -22,64 +22,121 @@ #include "common/color.h" #include "light_ws2811strip.h" +#include "dma.h" #include "nvic.h" +#include "io.h" +#include "system.h" +#include "rcc.h" +#include "timer.h" #ifdef LED_STRIP + +#if !defined(WS2811_PIN) +#define WS2811_PIN PA0 +#define WS2811_TIMER TIM5 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 +#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_DMA_CHANNEL DMA_Channel_6 +#define WS2811_DMA_IRQ DMA1_Stream2_IRQn +#define WS2811_TIMER_CHANNEL TIM_Channel_1 +#endif + +static IO_t ws2811IO = IO_NONE; +static uint16_t timDMASource = 0; +bool ws2811Initialised = false; + +void ws2811DMAHandler(DMA_Stream_TypeDef *stream) +{ + if (DMA_GetFlagStatus(stream, WS2811_DMA_FLAG)) { + ws2811LedDataTransferInProgress = 0; + DMA_ClearITPendingBit(stream, WS2811_DMA_IT); + DMA_Cmd(stream, DISABLE); + TIM_DMACmd(WS2811_TIMER, timDMASource, DISABLE); + } +} + void ws2811LedStripHardwareInit(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; - GPIO_InitTypeDef GPIO_InitStructure; DMA_InitTypeDef DMA_InitStructure; uint16_t prescalerValue; - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); - + RCC_ClockCmd(timerRCC(WS2811_TIMER), ENABLE); + ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); /* GPIOA Configuration: TIM5 Channel 1 as alternate function push-pull */ - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; - GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; - GPIO_Init(GPIOA, &GPIO_InitStructure); - - GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM5); - - + IOInit(ws2811IO, OWNER_SYSTEM, RESOURCE_OUTPUT); + IOConfigGPIOAF(ws2811IO, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_PP, GPIO_PuPd_UP), timerGPIOAF(WS2811_TIMER)); + // Stop timer - TIM_Cmd(TIM5, DISABLE); + TIM_Cmd(WS2811_TIMER, DISABLE); /* Compute the prescaler value */ - prescalerValue = (uint16_t) (SystemCoreClock / 2 / 84000000) - 1; + prescalerValue = (uint16_t)(SystemCoreClock / 2 / 84000000) - 1; + /* Time base configuration */ TIM_TimeBaseStructure.TIM_Period = 104; // 800kHz TIM_TimeBaseStructure.TIM_Prescaler = prescalerValue; - TIM_TimeBaseStructure.TIM_ClockDivision = 0; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(WS2811_TIMER, &TIM_TimeBaseStructure); /* PWM1 Mode configuration: Channel1 */ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset; + TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCNPolarity_High; + TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; TIM_OCInitStructure.TIM_Pulse = 0; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - TIM_OC1Init(TIM5, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable); + + uint32_t channelAddress = 0; + switch (WS2811_TIMER_CHANNEL) { + case TIM_Channel_1: + TIM_OC1Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC1; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR1); + TIM_OC1PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC2; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR2); + TIM_OC2PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC3; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR3); + TIM_OC3PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(WS2811_TIMER, &TIM_OCInitStructure); + timDMASource = TIM_DMA_CC4; + channelAddress = (uint32_t)(&WS2811_TIMER->CCR4); + TIM_OC4PreloadConfig(WS2811_TIMER, TIM_OCPreload_Enable); + break; + } + + TIM_CtrlPWMOutputs(WS2811_TIMER, ENABLE); + TIM_ARRPreloadConfig(WS2811_TIMER, ENABLE); - TIM_Cmd(TIM5, ENABLE); + TIM_CCxCmd(WS2811_TIMER, WS2811_TIMER_CHANNEL, TIM_CCx_Enable); + TIM_Cmd(WS2811_TIMER, ENABLE); + dmaSetHandler(WS2811_DMA_HANDLER_IDENTIFER, ws2811DMAHandler); /* configure DMA */ - /* DMA1 Channel Config */ - DMA_Cmd(DMA1_Stream2, DISABLE); // disable DMA channel 6 - DMA_DeInit(DMA1_Stream2); + DMA_Cmd(WS2811_DMA_STREAM, DISABLE); + DMA_DeInit(WS2811_DMA_STREAM); DMA_StructInit(&DMA_InitStructure); - DMA_InitStructure.DMA_Channel = DMA_Channel_6; - DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(TIM5->CCR1); + DMA_InitStructure.DMA_Channel = WS2811_DMA_CHANNEL; + DMA_InitStructure.DMA_PeripheralBaseAddr = channelAddress; DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)ledStripDMABuffer; DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral; DMA_InitStructure.DMA_BufferSize = WS2811_DMA_BUFFER_SIZE; @@ -93,38 +150,36 @@ void ws2811LedStripHardwareInit(void) DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull; DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single; DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single; - DMA_Init(DMA1_Stream2, &DMA_InitStructure); - DMA_ITConfig(DMA1_Stream2, DMA_IT_TC, ENABLE); - DMA_ClearITPendingBit(DMA1_Stream2, DMA_IT_TCIF2); // clear DMA1 Channel 6 transfer complete flag + + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE); + DMA_Init(WS2811_DMA_STREAM, &DMA_InitStructure); + DMA_ITConfig(WS2811_DMA_STREAM, DMA_IT_TC, ENABLE); + DMA_ClearITPendingBit(WS2811_DMA_STREAM, WS2811_DMA_IT); + NVIC_InitTypeDef NVIC_InitStructure; - NVIC_InitStructure.NVIC_IRQChannel = DMA1_Stream2_IRQn; + NVIC_InitStructure.NVIC_IRQChannel = WS2811_DMA_IRQ; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_WS2811_DMA); NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_WS2811_DMA); NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); + ws2811Initialised = true; setStripColor(&hsv_white); ws2811UpdateStrip(); } -void DMA1_Stream2_IRQHandler(void) -{ - if (DMA_GetFlagStatus(DMA1_Stream2, DMA_FLAG_TCIF2)) { - ws2811LedDataTransferInProgress = 0; - DMA_Cmd(DMA1_Stream2, DISABLE); - TIM_DMACmd(TIM5, TIM_DMA_CC1, DISABLE); - DMA_ClearITPendingBit(DMA1_Stream2, DMA_IT_TCIF2); - } -} - void ws2811LedStripDMAEnable(void) { - DMA_SetCurrDataCounter(DMA1_Stream2, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred - TIM_SetCounter(TIM5, 0); - DMA_Cmd(DMA1_Stream2, ENABLE); - TIM_DMACmd(TIM5, TIM_DMA_CC1, ENABLE); + if (!ws2811Initialised) + return; + + DMA_SetCurrDataCounter(WS2811_DMA_STREAM, WS2811_DMA_BUFFER_SIZE); // load number of bytes to be transferred + TIM_SetCounter(WS2811_TIMER, 0); + DMA_Cmd(WS2811_DMA_STREAM, ENABLE); + TIM_DMACmd(WS2811_TIMER, timDMASource, ENABLE); } -#endif + +#endif \ No newline at end of file diff --git a/src/main/drivers/max7456_symbols.h b/src/main/drivers/max7456_symbols.h index 2030cb956b..b2e8da72b5 100644 --- a/src/main/drivers/max7456_symbols.h +++ b/src/main/drivers/max7456_symbols.h @@ -176,7 +176,7 @@ #define SYM_FT 0x0F // Voltage and amperage -#define SYM_VOLT 0x00 +#define SYM_VOLT 0x06 #define SYM_AMP 0x9A #define SYM_MAH 0xA4 #define SYM_WATT 0x57 diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 8d332ba56e..5e797ea2f0 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -157,33 +157,33 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; #endif -#ifdef LED_STRIP_TIMER +#ifdef WS2811_TIMER // skip LED Strip output if (init->useLEDStrip) { - if (timerHardwarePtr->tim == LED_STRIP_TIMER) + if (timerHardwarePtr->tim == WS2811_TIMER) continue; -#if defined(STM32F303xC) && defined(WS2811_GPIO) && defined(WS2811_PIN_SOURCE) - if (CheckGPIOPinSource(timerHardwarePtr->tag, WS2811_GPIO, WS2811_PIN_SOURCE)) +#if defined(STM32F303xC) && defined(WS2811_PIN) + if (timerHardwarePtr->tag == IO_TAG(WS2811_PIN)) continue; #endif } #endif -#ifdef VBAT_ADC_GPIO - if (init->useVbat && CheckGPIOPin(timerHardwarePtr->tag, VBAT_ADC_GPIO, VBAT_ADC_GPIO_PIN)) { +#ifdef VBAT_ADC_PIN + if (init->useVbat && timerHardwarePtr->tag == IO_TAG(VBAT_ADC_PIN)) { continue; } #endif #ifdef RSSI_ADC_GPIO - if (init->useRSSIADC && CheckGPIOPin(timerHardwarePtr->tag, RSSI_ADC_GPIO, RSSI_ADC_GPIO_PIN)) { + if (init->useRSSIADC && timerHardwarePtr->tag == IO_TAG(RSSI_ADC_PIN)) { continue; } #endif #ifdef CURRENT_METER_ADC_GPIO - if (init->useCurrentMeterADC && CheckGPIOPin(timerHardwarePtr->tag, CURRENT_METER_ADC_GPIO, CURRENT_METER_ADC_GPIO_PIN)) { + if (init->useCurrentMeterADC && timerHardwarePtr->tag == IO_TAG(CURRENT_METER_ADC_PIN)) { continue; } #endif @@ -274,7 +274,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } if (init->useChannelForwarding && !init->airplane) { -#if defined(NAZE) && defined(LED_STRIP_TIMER) +#if defined(NAZE) && defined(WS2811_TIMER) // if LED strip is active, PWM5-8 are unavailable, so map AUX1+AUX2 to PWM13+PWM14 if (init->useLEDStrip) { if (timerIndex >= PWM13 && timerIndex <= PWM14) { diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index c124f05e20..f457288ac9 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -71,6 +71,24 @@ #define UART3_RX_PINSOURCE GPIO_PinSource11 #endif +#ifndef UART4_GPIO +#define UART4_TX_PIN GPIO_Pin_10 // PC10 (AF5) +#define UART4_RX_PIN GPIO_Pin_11 // PC11 (AF5) +#define UART4_GPIO_AF GPIO_AF_5 +#define UART4_GPIO GPIOC +#define UART4_TX_PINSOURCE GPIO_PinSource10 +#define UART4_RX_PINSOURCE GPIO_PinSource11 +#endif + +#ifndef UART5_GPIO // The real UART5_RX is on PD2, no board is using. +#define UART5_TX_PIN GPIO_Pin_12 // PC12 (AF5) +#define UART5_RX_PIN GPIO_Pin_12 // PC12 (AF5) +#define UART5_GPIO_AF GPIO_AF_5 +#define UART5_GPIO GPIOC +#define UART5_TX_PINSOURCE GPIO_PinSource12 +#define UART5_RX_PINSOURCE GPIO_PinSource12 +#endif + #ifdef USE_USART1 static uartPort_t uartPort1; #endif @@ -80,6 +98,12 @@ static uartPort_t uartPort2; #ifdef USE_USART3 static uartPort_t uartPort3; #endif +#ifdef USE_USART4 +static uartPort_t uartPort4; +#endif +#ifdef USE_USART5 +static uartPort_t uartPort5; +#endif #ifdef USE_USART1 uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t options) @@ -324,6 +348,124 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t optio } #endif +#ifdef USE_USART4 +uartPort_t *serialUSART4(uint32_t baudRate, portMode_t mode, portOptions_t options) +{ + uartPort_t *s; + static volatile uint8_t rx4Buffer[UART4_RX_BUFFER_SIZE]; + static volatile uint8_t tx4Buffer[UART4_TX_BUFFER_SIZE]; + NVIC_InitTypeDef NVIC_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + + s = &uartPort4; + s->port.vTable = uartVTable; + + s->port.baudRate = baudRate; + + s->port.rxBufferSize = UART4_RX_BUFFER_SIZE; + s->port.txBufferSize = UART4_TX_BUFFER_SIZE; + s->port.rxBuffer = rx4Buffer; + s->port.txBuffer = tx4Buffer; + + s->USARTx = UART4; + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART4, ENABLE); + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP; + + if (options & SERIAL_BIDIR) { + GPIO_InitStructure.GPIO_Pin = UART4_TX_PIN; + GPIO_InitStructure.GPIO_OType = (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD; + GPIO_PinAFConfig(UART4_GPIO, UART4_TX_PINSOURCE, UART4_GPIO_AF); + GPIO_Init(UART4_GPIO, &GPIO_InitStructure); + if(!(options & SERIAL_INVERTED)) + GPIO_SetBits(UART4_GPIO, UART4_TX_PIN); // OpenDrain output should be inactive + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART4_TX_PIN; + GPIO_PinAFConfig(UART4_GPIO, UART4_TX_PINSOURCE, UART4_GPIO_AF); + GPIO_Init(UART4_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART4_RX_PIN; + GPIO_PinAFConfig(UART4_GPIO, UART4_RX_PINSOURCE, UART4_GPIO_AF); + GPIO_Init(UART4_GPIO, &GPIO_InitStructure); + } + } + + NVIC_InitStructure.NVIC_IRQChannel = UART4_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART4); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART4); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + return s; +} +#endif + +#ifdef USE_USART5 +uartPort_t *serialUSART5(uint32_t baudRate, portMode_t mode, portOptions_t options) +{ + uartPort_t *s; + static volatile uint8_t rx5Buffer[UART5_RX_BUFFER_SIZE]; + static volatile uint8_t tx5Buffer[UART5_TX_BUFFER_SIZE]; + NVIC_InitTypeDef NVIC_InitStructure; + GPIO_InitTypeDef GPIO_InitStructure; + + s = &uartPort5; + s->port.vTable = uartVTable; + + s->port.baudRate = baudRate; + + s->port.rxBufferSize = UART5_RX_BUFFER_SIZE; + s->port.txBufferSize = UART5_TX_BUFFER_SIZE; + s->port.rxBuffer = rx5Buffer; + s->port.txBuffer = tx5Buffer; + + s->USARTx = UART5; + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE); + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; + GPIO_InitStructure.GPIO_PuPd = (options & SERIAL_INVERTED) ? GPIO_PuPd_DOWN : GPIO_PuPd_UP; + + if (options & SERIAL_BIDIR) { + GPIO_InitStructure.GPIO_Pin = UART5_TX_PIN; + GPIO_InitStructure.GPIO_OType = (options & SERIAL_INVERTED) ? GPIO_OType_PP : GPIO_OType_OD; + GPIO_PinAFConfig(UART5_GPIO, UART5_TX_PINSOURCE, UART5_GPIO_AF); + GPIO_Init(UART5_GPIO, &GPIO_InitStructure); + if(!(options & SERIAL_INVERTED)) + GPIO_SetBits(UART5_GPIO, UART5_TX_PIN); // OpenDrain output should be inactive + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART5_TX_PIN; + GPIO_PinAFConfig(UART5_GPIO, UART5_TX_PINSOURCE, UART5_GPIO_AF); + GPIO_Init(UART5_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART5_RX_PIN; + GPIO_PinAFConfig(UART5_GPIO, UART5_RX_PINSOURCE, UART5_GPIO_AF); + GPIO_Init(UART5_GPIO, &GPIO_InitStructure); + } + } + + NVIC_InitStructure.NVIC_IRQChannel = UART5_IRQn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SERIALUART5); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SERIALUART5); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + + return s; +} +#endif + static void handleUsartTxDma(uartPort_t *s) { DMA_Cmd(s->txDMAChannel, DISABLE); @@ -424,3 +566,21 @@ void USART3_IRQHandler(void) usartIrqHandler(s); } #endif + +#ifdef USE_USART4 +void UART4_IRQHandler(void) +{ + uartPort_t *s = &uartPort4; + + usartIrqHandler(s); +} +#endif + +#ifdef USE_USART5 +void UART5_IRQHandler(void) +{ + uartPort_t *s = &uartPort5; + + usartIrqHandler(s); +} +#endif diff --git a/src/main/drivers/system.h b/src/main/drivers/system.h index 92e17f8a82..c6d58aa744 100644 --- a/src/main/drivers/system.h +++ b/src/main/drivers/system.h @@ -42,6 +42,7 @@ void systemReset(void); void systemResetToBootloader(void); bool isMPUSoftReset(void); void cycleCounterInit(void); +void checkForBootLoaderRequest(void); void enableGPIOPowerUsageAndNoiseReductions(void); // current crystal frequency - 8 or 12MHz diff --git a/src/main/drivers/system_stm32f10x.c b/src/main/drivers/system_stm32f10x.c index 4ad526850a..3e59fe7342 100644 --- a/src/main/drivers/system_stm32f10x.c +++ b/src/main/drivers/system_stm32f10x.c @@ -37,7 +37,8 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) { +void systemResetToBootloader(void) +{ // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -68,6 +69,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + SetSysClock(false); #ifdef CC3D @@ -110,3 +113,6 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } +void checkForBootLoaderRequest(void) +{ +} \ No newline at end of file diff --git a/src/main/drivers/system_stm32f30x.c b/src/main/drivers/system_stm32f30x.c index 7e58ab061a..ee8aef1a0e 100644 --- a/src/main/drivers/system_stm32f30x.c +++ b/src/main/drivers/system_stm32f30x.c @@ -35,7 +35,8 @@ void systemReset(void) SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04; } -void systemResetToBootloader(void) { +void systemResetToBootloader(void) +{ // 1FFFF000 -> 20000200 -> SP // 1FFFF004 -> 1FFFF021 -> PC @@ -82,6 +83,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + // Enable FPU SCB->CPACR = (0x3 << (10 * 2)) | (0x3 << (11 * 2)); SetSysClock(); @@ -102,3 +105,7 @@ void systemInit(void) // SysTick SysTick_Config(SystemCoreClock / 1000); } + +void checkForBootLoaderRequest(void) +{ +} diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index f0042026f9..53f3767a5e 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -169,6 +169,8 @@ bool isMPUSoftReset(void) void systemInit(void) { + checkForBootLoaderRequest(); + SetSysClock(); // Configure NVIC preempt/priority groups @@ -194,3 +196,18 @@ void systemInit(void) SysTick_Config(SystemCoreClock / 1000); } +void(*bootJump)(void); +void checkForBootLoaderRequest(void) +{ + if (*((uint32_t *)0x2001FFFC) == 0xDEADBEEF) { + + *((uint32_t *)0x2001FFFC) = 0x0; + + __enable_irq(); + __set_MSP(0x20001000); + + bootJump = (void(*)(void))(*((uint32_t *) 0x1fff0004)); + bootJump(); + while (1); + } +} \ No newline at end of file diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 37067ae254..e168a46dfe 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -151,6 +151,18 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim) return 0; } +#if defined(STM32F3) || defined(STM32F4) +uint8_t timerGPIOAF(TIM_TypeDef *tim) +{ + for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) { + if (timerDefinitions[i].TIMx == tim) { + return timerDefinitions[i].alternateFunction; + } + } + return 0; +} +#endif + void timerNVICConfigure(uint8_t irq) { NVIC_InitTypeDef NVIC_InitStructure; diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index cdc461c690..26078d4517 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -69,6 +69,9 @@ typedef struct timerOvrHandlerRec_s { typedef struct timerDef_s { TIM_TypeDef *TIMx; rccPeriphTag_t rcc; +#if defined(STM32F3) || defined(STM32F4) + uint8_t alternateFunction; +#endif } timerDef_t; typedef struct timerHardware_s { @@ -82,8 +85,11 @@ typedef struct timerHardware_s { uint8_t alternateFunction; #endif } timerHardware_t; -enum {TIMER_OUTPUT_ENABLED = 0x01, TIMER_OUTPUT_INVERTED = 0x02}; +enum { + TIMER_OUTPUT_ENABLED = 0x01, + TIMER_OUTPUT_INVERTED = 0x02 +}; #ifdef STM32F1 #if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) @@ -149,3 +155,7 @@ void timerForceOverflow(TIM_TypeDef *tim); void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration rccPeriphTag_t timerRCC(TIM_TypeDef *tim); + +#if defined(STM32F3) || defined(STM32F4) +uint8_t timerGPIOAF(TIM_TypeDef *tim); +#endif diff --git a/src/main/drivers/timer_stm32f10x.c b/src/main/drivers/timer_stm32f10x.c index 27e9df381f..09ef0efc8c 100644 --- a/src/main/drivers/timer_stm32f10x.c +++ b/src/main/drivers/timer_stm32f10x.c @@ -57,34 +57,31 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) { - uint32_t tmp = 0; + uint32_t tmp = 0; - /* Check the parameters */ - assert_param(IS_TIM_LIST8_PERIPH(TIMx)); - assert_param(IS_TIM_CHANNEL(TIM_Channel)); - assert_param(IS_TIM_OCM(TIM_OCMode)); + /* Check the parameters */ + assert_param(IS_TIM_LIST8_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); - tmp = (uint32_t) TIMx; - tmp += CCMR_Offset; + tmp = (uint32_t) TIMx; + tmp += CCMR_Offset; - if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) - { - tmp += (TIM_Channel>>1); + if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) { + tmp += (TIM_Channel>>1); - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= TIM_OCMode; - } - else - { - tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint16_t)(TIM_Channel - (uint16_t)4) >> (uint16_t)1; - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); - } + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); + } } diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 4bce8a70f3..033aa316c4 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -10,16 +10,16 @@ #include "timer.h" const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM15, .rcc = RCC_APB2(TIM15) }, - { .TIMx = TIM16, .rcc = RCC_APB2(TIM16) }, - { .TIMx = TIM17, .rcc = RCC_APB2(TIM17) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_6 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_1 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_2 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_10 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_5 }, + { .TIMx = TIM15, .rcc = RCC_APB2(TIM15), GPIO_AF_9 }, + { .TIMx = TIM16, .rcc = RCC_APB2(TIM16), GPIO_AF_1 }, + { .TIMx = TIM17, .rcc = RCC_APB2(TIM17), GPIO_AF_1 }, }; @@ -58,31 +58,31 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint32_t TIM_OCMode) { - uint32_t tmp = 0; + uint32_t tmp = 0; - /* Check the parameters */ - assert_param(IS_TIM_LIST1_PERIPH(TIMx)); - assert_param(IS_TIM_CHANNEL(TIM_Channel)); - assert_param(IS_TIM_OCM(TIM_OCMode)); + /* Check the parameters */ + assert_param(IS_TIM_LIST1_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); - tmp = (uint32_t) TIMx; - tmp += CCMR_OFFSET; + tmp = (uint32_t) TIMx; + tmp += CCMR_OFFSET; - if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) { - tmp += (TIM_Channel>>1); + if ((TIM_Channel == TIM_Channel_1) || (TIM_Channel == TIM_Channel_3)) { + tmp += (TIM_Channel>>1); - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK; + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= CCMR_OC13M_MASK; - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= TIM_OCMode; - } else { - tmp += (uint32_t)(TIM_Channel - (uint32_t)4)>> (uint32_t)1; + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint32_t)(TIM_Channel - (uint32_t)4) >> (uint32_t)1; - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK; + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= CCMR_OC24M_MASK; - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8); - } + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint32_t)(TIM_OCMode << 8); + } } diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 8916a526c0..af09f85e1b 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -35,53 +35,50 @@ #define CCMR_Offset ((uint16_t)0x0018) const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { - { .TIMx = TIM1, .rcc = RCC_APB2(TIM1) }, - { .TIMx = TIM2, .rcc = RCC_APB1(TIM2) }, - { .TIMx = TIM3, .rcc = RCC_APB1(TIM3) }, - { .TIMx = TIM4, .rcc = RCC_APB1(TIM4) }, - { .TIMx = TIM5, .rcc = RCC_APB1(TIM5) }, - { .TIMx = TIM6, .rcc = RCC_APB1(TIM6) }, - { .TIMx = TIM7, .rcc = RCC_APB1(TIM7) }, - { .TIMx = TIM8, .rcc = RCC_APB2(TIM8) }, - { .TIMx = TIM9, .rcc = RCC_APB2(TIM9) }, - { .TIMx = TIM10, .rcc = RCC_APB2(TIM10) }, - { .TIMx = TIM11, .rcc = RCC_APB2(TIM11) }, - { .TIMx = TIM12, .rcc = RCC_APB1(TIM12) }, - { .TIMx = TIM13, .rcc = RCC_APB1(TIM13) }, - { .TIMx = TIM14, .rcc = RCC_APB1(TIM14) }, + { .TIMx = TIM1, .rcc = RCC_APB2(TIM1), GPIO_AF_TIM1 }, + { .TIMx = TIM2, .rcc = RCC_APB1(TIM2), GPIO_AF_TIM2 }, + { .TIMx = TIM3, .rcc = RCC_APB1(TIM3), GPIO_AF_TIM3 }, + { .TIMx = TIM4, .rcc = RCC_APB1(TIM4), GPIO_AF_TIM4 }, + { .TIMx = TIM5, .rcc = RCC_APB1(TIM5), GPIO_AF_TIM5 }, + { .TIMx = TIM6, .rcc = RCC_APB1(TIM6), 0 }, + { .TIMx = TIM7, .rcc = RCC_APB1(TIM7), 0 }, + { .TIMx = TIM8, .rcc = RCC_APB2(TIM8), GPIO_AF_TIM8 }, + { .TIMx = TIM9, .rcc = RCC_APB2(TIM9), GPIO_AF_TIM9 }, + { .TIMx = TIM10, .rcc = RCC_APB2(TIM10), GPIO_AF_TIM10 }, + { .TIMx = TIM11, .rcc = RCC_APB2(TIM11), GPIO_AF_TIM11 }, + { .TIMx = TIM12, .rcc = RCC_APB1(TIM12), GPIO_AF_TIM12 }, + { .TIMx = TIM13, .rcc = RCC_APB1(TIM13), GPIO_AF_TIM13 }, + { .TIMx = TIM14, .rcc = RCC_APB1(TIM14), GPIO_AF_TIM14 }, }; void TIM_SelectOCxM_NoDisable(TIM_TypeDef* TIMx, uint16_t TIM_Channel, uint16_t TIM_OCMode) { - uint32_t tmp = 0; + uint32_t tmp = 0; - /* Check the parameters */ - assert_param(IS_TIM_LIST8_PERIPH(TIMx)); - assert_param(IS_TIM_CHANNEL(TIM_Channel)); - assert_param(IS_TIM_OCM(TIM_OCMode)); + /* Check the parameters */ + assert_param(IS_TIM_LIST8_PERIPH(TIMx)); + assert_param(IS_TIM_CHANNEL(TIM_Channel)); + assert_param(IS_TIM_OCM(TIM_OCMode)); - tmp = (uint32_t) TIMx; - tmp += CCMR_Offset; + tmp = (uint32_t) TIMx; + tmp += CCMR_Offset; - if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) - { - tmp += (TIM_Channel>>1); + if((TIM_Channel == TIM_Channel_1) ||(TIM_Channel == TIM_Channel_3)) { + tmp += (TIM_Channel>>1); - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC1M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= TIM_OCMode; - } - else - { - tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= TIM_OCMode; + } else { + tmp += (uint16_t)(TIM_Channel - (uint16_t)4)>> (uint16_t)1; - /* Reset the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); + /* Reset the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp &= (uint32_t)~((uint32_t)TIM_CCMR1_OC2M); - /* Configure the OCxM bits in the CCMRx register */ - *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); - } + /* Configure the OCxM bits in the CCMRx register */ + *(__IO uint32_t *) tmp |= (uint16_t)(TIM_OCMode << 8); + } } diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 2e200cff1f..d885d30eaa 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -28,7 +28,6 @@ #include "debug.h" #include "common/axis.h" -#include "common/filter.h" #include "drivers/system.h" #include "drivers/sensor.h" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index adc0145ba9..b5da4a0170 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -946,7 +946,7 @@ void filterServos(void) #ifdef USE_SERVOS static int16_t servoIdx; static bool servoFilterIsSet; - static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS]; + static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; #if defined(MIXER_DEBUG) uint32_t startTime = micros(); @@ -955,11 +955,11 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime); + biquadFilterInit(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); servoFilterIsSet = true; } - servo[servoIdx] = lrintf(applyBiQuadFilter((float) servo[servoIdx], &servoFilterState[servoIdx])); + servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 91ccbc2d84..b0dee87496 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -112,8 +112,8 @@ float getdT (void) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static filterStatePt1_t deltaFilterState[3]; -static filterStatePt1_t yawFilterState; +static pt1Filter_t deltaFilter[3]; +static pt1Filter_t yawFilter; #ifndef SKIP_PID_LUXFLOAT static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, @@ -205,7 +205,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat //-----calculate D-term if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = lrintf(PTerm + ITerm); @@ -230,7 +230,7 @@ static void pidFloat(const pidProfile_t *pidProfile, uint16_t max_angle_inclinat delta *= (1.0f / getdT()); // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1(delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT()); DTerm = constrainf(luxDTermScale * delta * (float)pidProfile->D8[axis] * tpaFactor, -300.0f, 300.0f); @@ -344,7 +344,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin //-----calculate D-term if (axis == YAW) { - if (pidProfile->yaw_lpf_hz) PTerm = filterApplyPt1(PTerm, &yawFilterState, pidProfile->yaw_lpf_hz, getdT()); + if (pidProfile->yaw_lpf_hz) PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT()); axisPID[axis] = PTerm + ITerm; @@ -369,7 +369,7 @@ static void pidInteger(const pidProfile_t *pidProfile, uint16_t max_angle_inclin delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5; // Filter delta - if (pidProfile->dterm_lpf_hz) delta = filterApplyPt1((float)delta, &deltaFilterState[axis], pidProfile->dterm_lpf_hz, getdT()); + if (pidProfile->dterm_lpf_hz) delta = pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()); DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 061f0d798a..863f3a8b6b 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -440,7 +440,7 @@ void updateLedCount(void) } } -void reevalulateLedConfig(void) +void reevaluateLedConfig(void) { updateLedCount(); determineLedStripDimensions(); @@ -534,7 +534,7 @@ bool parseLedStripConfig(uint8_t ledIndex, const char *config) memset(ledConfig, 0, sizeof(ledConfig_t)); } - reevalulateLedConfig(); + reevaluateLedConfig(); return ok; } @@ -1095,7 +1095,7 @@ void applyDefaultLedStripConfig(ledConfig_t *ledConfigs) memset(ledConfigs, 0, MAX_LED_STRIP_LENGTH * sizeof(ledConfig_t)); memcpy(ledConfigs, &defaultLedStripConfig, sizeof(defaultLedStripConfig)); - reevalulateLedConfig(); + reevaluateLedConfig(); } void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) @@ -1107,7 +1107,7 @@ void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse) void ledStripEnable(void) { - reevalulateLedConfig(); + reevaluateLedConfig(); ledStripInitialised = true; ws2811LedStripInit(); diff --git a/src/main/io/ledstrip.h b/src/main/io/ledstrip.h index 5f901e46c8..18915229b7 100644 --- a/src/main/io/ledstrip.h +++ b/src/main/io/ledstrip.h @@ -94,4 +94,4 @@ bool parseColor(uint8_t index, const char *colorConfig); void applyDefaultColors(hsvColor_t *colors, uint8_t colorCount); void ledStripEnable(void); -void reevalulateLedConfig(void); +void reevaluateLedConfig(void); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index b2effbb4b7..6fbdad7c0c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -316,7 +316,7 @@ serialPort_t *openSerialPort( #endif #ifdef USE_USART5 case SERIAL_PORT_USART5: - serialPort = uartOpen(USART5, callback, baudRate, mode, options); + serialPort = uartOpen(UART5, callback, baudRate, mode, options); break; #endif #ifdef USE_USART6 diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 6d96a36ec1..bcea2f8a1e 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -44,15 +44,15 @@ #define USE_TXRX_LED #if defined(USE_TXRX_LED) && defined(LED0) -# define RX_LED_OFF LED0_OFF -# define RX_LED_ON LED0_ON -# ifdef LED1 -# define TX_LED_OFF LED1_OFF -# define TX_LED_ON LED1_ON -# else -# define TX_LED_OFF LED0_OFF -# define TX_LED_ON LED0_ON -# endif +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON +#ifdef LED1 +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON +#else +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON +#endif #else # define RX_LED_OFF do {} while(0) # define RX_LED_ON do {} while(0) @@ -330,6 +330,7 @@ void esc4wayProcess(serialPort_t *serial) while(!esc4wayExitRequested) { // restart looking for new sequence from host crcIn = 0; + uint8_t esc = readByteCrc(); if(esc != cmd_Local_Escape) continue; // wait for sync character @@ -348,7 +349,6 @@ void esc4wayProcess(serialPort_t *serial) paramBuf[i] = readByteCrc(); readByteCrc(); readByteCrc(); // update input CRC - RX_LED_OFF; outLen = 0; // output handling code will send single zero byte if necessary replyAck = esc4wayAck_OK; @@ -356,8 +356,10 @@ void esc4wayProcess(serialPort_t *serial) if(crcIn != 0) // CRC of correct message == 0 replyAck = esc4wayAck_I_INVALID_CRC; + TX_LED_ON; if (replyAck == esc4wayAck_OK) replyAck = esc4wayProcessCmd(command, addr, paramBuf, inLen, &outLen); + RX_LED_OFF; // send single '\0' byte is output when length is zero (len ==0 -> 256 bytes) if(!outLen) { @@ -366,14 +368,13 @@ void esc4wayProcess(serialPort_t *serial) } crcOut = 0; - TX_LED_ON; serialBeginWrite(port); writeByteCrc(cmd_Remote_Escape); writeByteCrc(command); writeByteCrc(addr >> 8); writeByteCrc(addr & 0xff); writeByteCrc(outLen & 0xff); // only low byte is send, 0x00 -> 256B - for(int i = 0; i < outLen; i++) + for(int i = 0; i < outLen % 0x100; i++) writeByteCrc(paramBuf[i]); writeByteCrc(replyAck); writeByte(crcOut >> 8); diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index efb4610268..01a598e8b2 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -238,7 +238,7 @@ void BL_SendCMDRunRestartBootloader(void) static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr { // skip if adr == 0xFFFF - if((pMem->addr == 0xffff)) + if(pMem->addr == 0xffff) return 1; uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->addr >> 8, pMem->addr & 0xff }; BL_SendBuf(sCMD, sizeof(sCMD), true); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index a51f828eda..e160869df0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -112,6 +112,7 @@ static void cliRxFail(char *cmdline); static void cliAdjustmentRange(char *cmdline); static void cliMotorMix(char *cmdline); static void cliDefaults(char *cmdline); +void cliDfu(char *cmdLine); static void cliDump(char *cmdLine); void cliDumpProfile(uint8_t profileIndex); void cliDumpRateProfile(uint8_t rateProfileIndex) ; @@ -122,6 +123,7 @@ static void cliPlaySound(char *cmdline); static void cliProfile(char *cmdline); static void cliRateProfile(char *cmdline); static void cliReboot(void); +static void cliRebootEx(bool bootLoader); static void cliSave(char *cmdline); static void cliSerial(char *cmdline); #ifndef SKIP_SERIAL_PASSTHROUGH @@ -263,8 +265,8 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("color", "configure colors", NULL, cliColor), #endif CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), - CLI_COMMAND_DEF("dump", "dump configuration", - "[master|profile]", cliDump), + CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), + CLI_COMMAND_DEF("dump", "dump configuration", "[master|profile]", cliDump), CLI_COMMAND_DEF("exit", NULL, NULL, cliExit), CLI_COMMAND_DEF("feature", "configure features", "list\r\n" @@ -2564,10 +2566,19 @@ static void cliRateProfile(char *cmdline) { static void cliReboot(void) { - cliPrint("\r\nRebooting"); + cliRebootEx(false); +} + +static void cliRebootEx(bool bootLoader) +{ + cliPrint("\r\nRebooting"); bufWriterFlush(cliWriter); waitForSerialPortToFinishTransmitting(cliPort); stopMotors(); + if (bootLoader) { + systemResetToBootloader(); + return; + } systemReset(); } @@ -3107,6 +3118,13 @@ static void cliResource(char *cmdline) } } +void cliDfu(char *cmdLine) +{ + UNUSED(cmdLine); + cliPrint("\r\nRestarting in DFU mode"); + cliRebootEx(true); +} + void cliInit(serialConfig_t *serialConfig) { UNUSED(serialConfig); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9f2609c0a0..b616605ac3 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1807,7 +1807,7 @@ static bool processInCommand(void) ledConfig->color = read8(); - reevalulateLedConfig(); + reevaluateLedConfig(); } break; #endif diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 721ad27eb8..18299e4904 100644 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -168,8 +168,8 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_LEDSTRIP] = { .taskName = "LEDSTRIP", .taskFunc = taskLedStrip, - .desiredPeriod = 1000000 / 100, // 100 Hz - .staticPriority = TASK_PRIORITY_IDLE, + .desiredPeriod = 1000000 / 10, // 10 Hz + .staticPriority = TASK_PRIORITY_LOW, }, #endif diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d79eb2e14b..da86d37eb4 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -47,7 +47,6 @@ uint32_t accTargetLooptime; static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. extern uint16_t InflightcalibratingA; -extern bool AccInflightCalibrationArmed; extern bool AccInflightCalibrationMeasurementDone; extern bool AccInflightCalibrationSavetoEEProm; extern bool AccInflightCalibrationActive; @@ -55,7 +54,7 @@ extern bool AccInflightCalibrationActive; static flightDynamicsTrims_t *accelerationTrims; static float accLpfCutHz = 0; -static biquad_t accFilterState[XYZ_AXIS_COUNT]; +static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; static bool accFilterInitialised = false; void accSetCalibrationCycles(uint16_t calibrationCyclesRequired) @@ -87,9 +86,8 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; - int axis; - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { // Reset a[axis] at start of calibration if (isOnFirstAccelerationCalibrationCycle()) @@ -179,14 +177,13 @@ void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims) void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) { - int16_t accADCRaw[XYZ_AXIS_COUNT]; - int axis; + int16_t accADCRaw[XYZ_AXIS_COUNT]; if (!acc.read(accADCRaw)) { return; } - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis]; accSmooth[axis] = accADCRaw[axis]; } @@ -194,13 +191,17 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims) if (accLpfCutHz) { if (!accFilterInitialised) { if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */ - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(accLpfCutHz, &accFilterState[axis], accTargetLooptime); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + } accFilterInitialised = true; } } if (accFilterInitialised) { - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) accSmooth[axis] = lrintf(applyBiQuadFilter((float) accSmooth[axis], &accFilterState[axis])); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis])); + } } } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 5bcc265f16..23159d87cd 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -63,20 +63,19 @@ uint16_t batteryAdcToVoltage(uint16_t src) static void updateBatteryVoltage(void) { - uint16_t vbatSample; - static biquad_t vbatFilterState; - static bool vbatFilterStateIsSet; + static biquadFilter_t vbatFilter; + static bool vbatFilterIsInitialised; // store the battery voltage with some other recent battery voltage readings - vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); + uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY); if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample; - if (!vbatFilterStateIsSet) { - BiQuadNewLpf(VBATT_LPF_FREQ, &vbatFilterState, 50000); //50HZ Update - vbatFilterStateIsSet = true; + if (!vbatFilterIsInitialised) { + biquadFilterInit(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update + vbatFilterIsInitialised = true; } - vbatSample = applyBiQuadFilter(vbatSample, &vbatFilterState); + vbatSample = biquadFilterApply(&vbatFilter, vbatSample); vbat = batteryAdcToVoltage(vbatSample); if (debugMode == DEBUG_BATTERY) debug[1] = vbat; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c79d29013f..4f425dc1d2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -43,7 +43,7 @@ float gyroADCf[XYZ_AXIS_COUNT]; static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 }; static const gyroConfig_t *gyroConfig; -static biquad_t gyroFilterState[XYZ_AXIS_COUNT]; +static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT]; static uint8_t gyroSoftLpfHz; static uint16_t calibratingG = 0; @@ -57,7 +57,7 @@ void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime); + biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime); } } } @@ -157,7 +157,7 @@ void gyroUpdate(void) if (gyroSoftLpfHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]); + gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]); gyroADC[axis] = lrintf(gyroADCf[axis]); } } else { diff --git a/src/main/target/ALIENFLIGHTF3/target.c b/src/main/target/ALIENFLIGHTF3/target.c index ad85d7b9b9..57fc417638 100644 --- a/src/main/target/ALIENFLIGHTF3/target.c +++ b/src/main/target/ALIENFLIGHTF3/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include @@ -9,16 +24,16 @@ const uint16_t multiPPM[] = { PWM11 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM15 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM1 + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM17 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // TIM2 0xFFFF }; @@ -48,10 +63,7 @@ const uint16_t airPWM[] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - // // 6 x 3 pin headers - // - { TIM15, IO_TAG(PB15), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM1 - PB15 - TIM1_CH3N, TIM15_CH1N, *TIM15_CH2 { TIM15, IO_TAG(PB14), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM2 - PB14 - TIM1_CH2N, *TIM15_CH1 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // PWM3 - PA8 - *TIM1_CH1, TIM4_ETR @@ -59,20 +71,14 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM3, IO_TAG(PA6), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - PA6 - *TIM3_CH1, TIM8_BKIN, TIM1_BKIN, TIM16_CH1 { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM6 - PA2 - *TIM2_CH3, !TIM15_CH1 - // // 6 pin header - // - // PWM7-10 { TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM7 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N { TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM8 - PA7 - !TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 { TIM3, IO_TAG(PA4), TIM_Channel_2, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2 }, // PWM9 - PA4 - *TIM3_CH2 { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1 }, // PWM10 - PA1 - *TIM2_CH2, TIM15_CH1N - // // PPM PORT - Also USART2 RX (AF5) - // - { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_IPD, GPIO_AF_1 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 //{ TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 0, IOCFG_IPD, GPIO_PinSource3, GPIO_AF_9 } // PPM - PA3 - TIM2_CH4, TIM15_CH2 - PWM13 diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index a5d1b042f3..4cd3d955cb 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -117,7 +117,6 @@ #define USE_ADC #define ADC_INSTANCE ADC2 -//#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PA4 #define VBAT_SCALE_DEFAULT 20 @@ -133,6 +132,7 @@ #define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/ALIENFLIGHTF4/target.c b/src/main/target/ALIENFLIGHTF4/target.c index e6024280f5..efedb56689 100644 --- a/src/main/target/ALIENFLIGHTF4/target.c +++ b/src/main/target/ALIENFLIGHTF4/target.c @@ -1,74 +1,91 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include -#include #include #include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { - PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 - PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 - PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 - PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 - PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 - PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 + PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 + PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 + PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 or #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #2 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #5 or #3 + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #6 or #4 + PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #7 or #5 + PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #8 or #6 0xFFFF }; const uint16_t airPPM[] = { - PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 - PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 - PWM2 | (MAP_TO_SERVO_OUTPUT << 8), // servo #7 - PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 - PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #9 - PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 + PWM2 | (MAP_TO_SERVO_OUTPUT << 8), // servo #7 + PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #9 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 0xFFFF }; const uint16_t airPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 - PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 - PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 - PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 - PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 - PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 - PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 - PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 + PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1 + PWM2 | (MAP_TO_PWM_INPUT << 8), // input #2 + PWM3 | (MAP_TO_PWM_INPUT << 8), // input #3 + PWM4 | (MAP_TO_PWM_INPUT << 8), // input #4 + PWM5 | (MAP_TO_PWM_INPUT << 8), // input #5 + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2 + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM13 | (MAP_TO_SERVO_OUTPUT << 8), // servo #6 0xFFFF }; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index d5122a486c..98387ee23c 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -146,16 +146,10 @@ // LED strip configuration using RC5 pin. //#define LED_STRIP -//#define LED_STRIP_TIMER TIM8 //#define USE_LED_STRIP_ON_DMA1_CHANNEL3 -//#define WS2811_GPIO GPIOB -//#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -//#define WS2811_GPIO_AF GPIO_AF_3 -//#define WS2811_PIN GPIO_Pin_15 // TIM8_CH3 -//#define WS2811_PIN_SOURCE GPIO_PinSource15 +//#define WS2811_PIN PB15 // TIM8_CH3 //#define WS2811_TIMER TIM8 -//#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM8 //#define WS2811_DMA_CHANNEL DMA1_Channel3 //#define WS2811_IRQ DMA1_Channel3_IRQn @@ -176,6 +170,7 @@ #define DEFAULT_FEATURES (FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 24df5a7393..f005e5d1a0 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 7160da98f7..8d9d1c633a 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -134,6 +134,19 @@ #define USE_ADC #define VBAT_ADC_PIN PC3 +#define LED_STRIP +// LED Strip can run off Pin 6 (PB1) of the ESC outputs. +#define WS2811_PIN PB1 +#define WS2811_TIMER TIM3 +#define WS2811_TIMER_CHANNEL TIM_Channel_4 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST2_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream2 +#define WS2811_DMA_FLAG DMA_FLAG_TCIF2 +#define WS2811_DMA_IT DMA_IT_TCIF2 +#define WS2811_DMA_CHANNEL DMA_Channel_5 +#define WS2811_DMA_IRQ DMA1_Stream2_IRQn + + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define DEFAULT_RX_FEATURE FEATURE_RX_PPM diff --git a/src/main/target/BLUEJAYF4/target.mk b/src/main/target/BLUEJAYF4/target.mk index 9f23086666..f523965621 100644 --- a/src/main/target/BLUEJAYF4/target.mk +++ b/src/main/target/BLUEJAYF4/target.mk @@ -4,5 +4,7 @@ FEATURES += SDCARD VCP TARGET_SRC = \ drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_mpu6500.c \ - drivers/barometer_ms5611.c + drivers/barometer_ms5611.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_stm32f4xx.c diff --git a/src/main/target/CC3D/target.c b/src/main/target/CC3D/target.c index c685f7e35a..a870de9e24 100644 --- a/src/main/target/CC3D/target.c +++ b/src/main/target/CC3D/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include @@ -8,16 +23,16 @@ const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index a0415670d0..8d6e6afbc3 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -92,12 +92,13 @@ #define I2C_DEVICE (I2CDEV_2) // Flex port - SCL/PB10, SDA/PB11 #define USE_ADC -#define CURRENT_METER_ADC_PIN PB1 -#define VBAT_ADC_PIN PA0 -#define RSSI_ADC_PIN PB0 +#define CURRENT_METER_ADC_PIN PB1 +#define VBAT_ADC_PIN PA0 +#define RSSI_ADC_PIN PB0 #define LED_STRIP -#define LED_STRIP_TIMER TIM3 +#define WS2811_PIN PB4 +#define WS2811_TIMER TIM3 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER diff --git a/src/main/target/CHEBUZZF3/target.c b/src/main/target/CHEBUZZF3/target.c index 132ff783f9..631d83e6aa 100644 --- a/src/main/target/CHEBUZZF3/target.c +++ b/src/main/target/CHEBUZZF3/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/CJMCU/target.c b/src/main/target/CJMCU/target.c index 926f3efa03..0f8228559e 100644 --- a/src/main/target/CJMCU/target.c +++ b/src/main/target/CJMCU/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 10c99ccc01..be0764a03b 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1403,7 +1403,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) ledConfig->color = bstRead8(); - reevalulateLedConfig(); + reevaluateLedConfig(); } break; #endif diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 675067e068..91b21c8b36 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 976b287a05..2139ebb37d 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -113,7 +113,6 @@ #define USE_ADC #define ADC_INSTANCE ADC1 -#define BOARD_HAS_VOLTAGE_DIVIDER #define VBAT_ADC_PIN PC0 #define CURRENT_METER_ADC_PIN PC1 #define RSSI_ADC_PIN PC2 @@ -122,15 +121,8 @@ #define LED_STRIP #define USE_COLIBTI_RACE_LED_DEFAULT_CONFIG -#define LED_STRIP_TIMER TIM16 - -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 @@ -144,7 +136,10 @@ #define USE_MPU_DATA_READY_SIGNAL #define ENSURE_MPU_DATA_READY_IS_LOW -#define DEFAULT_RX_FEATURE FEATURE_RX_PPM +#define DEFAULT_FEATURES FEATURE_VBAT +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART3 #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/DOGE/target.c b/src/main/target/DOGE/target.c index ebd5e6f1f0..c07a925414 100644 --- a/src/main/target/DOGE/target.c +++ b/src/main/target/DOGE/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/DOGE/target.h b/src/main/target/DOGE/target.h index 44a16782b1..7ca509ea28 100644 --- a/src/main/target/DOGE/target.h +++ b/src/main/target/DOGE/target.h @@ -140,15 +140,9 @@ #define LED_STRIP // tqfp48 pin 16 -#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/EUSTM32F103RC/target.c b/src/main/target/EUSTM32F103RC/target.c index 6e1b4ae6b6..b2a0db9a16 100644 --- a/src/main/target/EUSTM32F103RC/target.c +++ b/src/main/target/EUSTM32F103RC/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index d6d916bb28..0869dd46b1 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -102,9 +102,6 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -//#define LED_STRIP -#define LED_STRIP_TIMER TIM3 - #define SPEKTRUM_BIND // USART2, PA3 #define BIND_PIN PA3 diff --git a/src/main/target/FURYF3/target.c b/src/main/target/FURYF3/target.c index 4dc593bb3f..e52da0ab27 100644 --- a/src/main/target/FURYF3/target.c +++ b/src/main/target/FURYF3/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -58,7 +74,7 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM4, IO_TAG(PB7), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM4 - S1 { TIM4, IO_TAG(PB6), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2 }, // PWM5 - S2 - { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM6 - S3 + { TIM17, IO_TAG(PB5), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10}, // PWM6 - S3 { TIM16, IO_TAG(PB4), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1 }, // PWM7 - S4 { TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6 }, // GPIO TIMER - LED_STRIP diff --git a/src/main/target/FURYF3/target.h b/src/main/target/FURYF3/target.h index 9075271c1c..db4ddd7457 100644 --- a/src/main/target/FURYF3/target.h +++ b/src/main/target/FURYF3/target.h @@ -156,16 +156,10 @@ #define CURRENT_METER_ADC_PIN PA2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/FURYF4/target.c b/src/main/target/FURYF4/target.c index c5bb0486c0..ac600ea439 100644 --- a/src/main/target/FURYF4/target.c +++ b/src/main/target/FURYF4/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/IRCFUSIONF3/target.c b/src/main/target/IRCFUSIONF3/target.c index 7207468fae..e31aa3a7b6 100644 --- a/src/main/target/IRCFUSIONF3/target.c +++ b/src/main/target/IRCFUSIONF3/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/KISSFC/target.c b/src/main/target/KISSFC/target.c index 60f2a07363..07450ad505 100644 --- a/src/main/target/KISSFC/target.c +++ b/src/main/target/KISSFC/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/KISSFC/target.h b/src/main/target/KISSFC/target.h index 9095ef7013..260a2bc57f 100644 --- a/src/main/target/KISSFC/target.h +++ b/src/main/target/KISSFC/target.h @@ -73,7 +73,7 @@ #define I2C_DEVICE (I2CDEV_1) // PB6/SCL, PB7/SDA #define USE_ADC -#define VBAT_SCALE_DEFAULT 164 +#define VBAT_SCALE_DEFAULT 160 #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PA0 //#define CURRENT_METER_ADC_PIN PA5 @@ -81,7 +81,8 @@ #define DEFAULT_FEATURES FEATURE_VBAT #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS; +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 #define SPEKTRUM_BIND #define BIND_PIN PB4 diff --git a/src/main/target/LUX_RACE/target.c b/src/main/target/LUX_RACE/target.c index e26bb29500..91b21c8b36 100644 --- a/src/main/target/LUX_RACE/target.c +++ b/src/main/target/LUX_RACE/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { @@ -56,12 +72,12 @@ const uint16_t airPWM[] = { PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/LUX_RACE/target.h b/src/main/target/LUX_RACE/target.h index 6292d2435f..cef9b31b0a 100644 --- a/src/main/target/LUX_RACE/target.h +++ b/src/main/target/LUX_RACE/target.h @@ -95,15 +95,8 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define LED_STRIP_TIMER TIM16 - -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/MOTOLAB/target.c b/src/main/target/MOTOLAB/target.c index b624db7ccf..a89063cc9a 100644 --- a/src/main/target/MOTOLAB/target.c +++ b/src/main/target/MOTOLAB/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 27bf98691b..183e1326d5 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -126,16 +126,10 @@ #define LED_STRIP #if 1 -#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/NAZE/target.c b/src/main/target/NAZE/target.c index 110ce3140a..7116b53551 100644 --- a/src/main/target/NAZE/target.c +++ b/src/main/target/NAZE/target.c @@ -1,22 +1,38 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index df73f8e4f2..cb2f575392 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -155,7 +155,8 @@ #define LED_STRIP -#define LED_STRIP_TIMER TIM3 +#define WS2811_TIMER TIM3 +#define WS2811_PIN PA6 #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC6 #define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH6_HANDLER @@ -180,6 +181,7 @@ #define DEFAULT_FEATURES FEATURE_MOTOR_STOP #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL #define SERIALRX_PROVIDER SERIALRX_SPEKTRUM2048 +#define SERIALRX_UART SERIAL_PORT_USART2 #define HARDWARE_BIND_PLUG // Hardware bind plug at PB5 (Pin 41) diff --git a/src/main/target/OLIMEXINO/target.c b/src/main/target/OLIMEXINO/target.c index 1b75af4224..ea20267b3c 100644 --- a/src/main/target/OLIMEXINO/target.c +++ b/src/main/target/OLIMEXINO/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index b7972ad777..3ff23fd8d3 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -78,18 +78,12 @@ #define USE_I2C #define I2C_DEVICE (I2CDEV_2) -// #define SOFT_I2C // enable to test software i2c -// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) -// #define SOFT_I2C_PB67 - #define USE_ADC #define CURRENT_METER_ADC_PIN PB1 #define VBAT_ADC_PIN PA4 #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM3 // IO - assuming all IOs on smt32f103rb LQFP64 package #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/OMNIBUS/target.c b/src/main/target/OMNIBUS/target.c index 94250435ab..d895b571ce 100644 --- a/src/main/target/OMNIBUS/target.c +++ b/src/main/target/OMNIBUS/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index c9eb4a59da..76d995fce3 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -166,15 +166,9 @@ #define LED_STRIP -#define LED_STRIP_TIMER TIM1 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/PIKOBLX/target.c b/src/main/target/PIKOBLX/target.c index b624db7ccf..a89063cc9a 100644 --- a/src/main/target/PIKOBLX/target.c +++ b/src/main/target/PIKOBLX/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index 8b14570c91..36ee217725 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -94,16 +94,10 @@ #define LED_STRIP #if 1 -#define LED_STRIP_TIMER TIM16 #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/PORT103R/target.c b/src/main/target/PORT103R/target.c index 42ce07beca..8f21b7cbb7 100644 --- a/src/main/target/PORT103R/target.c +++ b/src/main/target/PORT103R/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 64e0b6455c..d5d0850d93 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -115,9 +115,6 @@ #define RSSI_ADC_PIN PA1 #define EXTERNAL1_ADC_PIN PA5 -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM3 - #define USE_SERIAL_4WAY_BLHELI_INTERFACE // IO - stm32f103RCT6 in 64pin package diff --git a/src/main/target/REVO/target.c b/src/main/target/REVO/target.c index b32c5175f0..03f60516d4 100644 --- a/src/main/target/REVO/target.c +++ b/src/main/target/REVO/target.c @@ -1,23 +1,39 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM1 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index d2124e5b28..3a67944b91 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -110,8 +110,6 @@ #define SENSORS_SET (SENSOR_ACC) -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM5 #define DEFAULT_RX_FEATURE FEATURE_RX_PPM #define DEFAULT_FEATURES (FEATURE_BLACKBOX | FEATURE_ONESHOT125 | FEATURE_RX_SERIAL) diff --git a/src/main/target/REVONANO/target.c b/src/main/target/REVONANO/target.c index 53d4042172..9a579f2bc9 100644 --- a/src/main/target/REVONANO/target.c +++ b/src/main/target/REVONANO/target.c @@ -15,10 +15,10 @@ * along with Cleanflight. If not, see . */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/REVONANO/target.h b/src/main/target/REVONANO/target.h index 6f4a2c4315..14912fffc1 100644 --- a/src/main/target/REVONANO/target.h +++ b/src/main/target/REVONANO/target.h @@ -85,12 +85,6 @@ #define VBAT_ADC_PIN PA6 #define RSSI_ADC_PIN PA5 - -//#define SENSORS_SET (SENSOR_ACC|SENSOR_MAG) - -//#define LED_STRIP -//#define LED_STRIP_TIMER TIM5 - #define GPS #define BLACKBOX #define TELEMETRY diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c index eaeb18fca4..1bea16f50c 100644 --- a/src/main/target/RMDO/target.c +++ b/src/main/target/RMDO/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h index 779b8ab6ce..602609862e 100644 --- a/src/main/target/RMDO/target.h +++ b/src/main/target/RMDO/target.h @@ -112,16 +112,10 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SINGULARITY/target.c b/src/main/target/SINGULARITY/target.c index 645bd323f5..8131579519 100644 --- a/src/main/target/SINGULARITY/target.c +++ b/src/main/target/SINGULARITY/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SINGULARITY/target.h b/src/main/target/SINGULARITY/target.h index f58dcbe969..0d89f4226a 100644 --- a/src/main/target/SINGULARITY/target.h +++ b/src/main/target/SINGULARITY/target.h @@ -91,23 +91,16 @@ #define M25P16_SPI_INSTANCE SPI2 #define USE_ADC -#define BOARD_HAS_VOLTAGE_DIVIDER #define ADC_INSTANCE ADC2 #define VBAT_ADC_PIN PB2 #define VBAT_SCALE_DEFAULT 77 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 @@ -115,8 +108,9 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES FEATURE_BLACKBOX +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART3 #define SPEKTRUM_BIND // USART2, PA15 diff --git a/src/main/target/SIRINFPV/target.c b/src/main/target/SIRINFPV/target.c index dfe1517fb4..0630b527db 100644 --- a/src/main/target/SIRINFPV/target.c +++ b/src/main/target/SIRINFPV/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SPARKY/target.c b/src/main/target/SPARKY/target.c index d5ecdcc96f..389617278b 100644 --- a/src/main/target/SPARKY/target.c +++ b/src/main/target/SPARKY/target.c @@ -1,8 +1,24 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include +#include "drivers/io.h" #include "drivers/pwm_mapping.h" const uint16_t multiPPM[] = { diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 58101f0b85..0913d0ec16 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -97,16 +97,9 @@ #define LED_STRIP #if 1 // LED strip configuration using PWM motor output pin 5. -#define LED_STRIP_TIMER TIM16 - #define USE_LED_STRIP_ON_DMA1_CHANNEL3 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_6 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource6 +#define WS2811_PIN PA6 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/SPRACINGF3/target.c b/src/main/target/SPRACINGF3/target.c index 122f07bb0a..bf2a06da44 100644 --- a/src/main/target/SPRACINGF3/target.c +++ b/src/main/target/SPRACINGF3/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 7b2a365a55..840dda225a 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -121,16 +121,10 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SPRACINGF3EVO/target.c b/src/main/target/SPRACINGF3EVO/target.c index 1ac9743d3e..171f6be27a 100644 --- a/src/main/target/SPRACINGF3EVO/target.c +++ b/src/main/target/SPRACINGF3EVO/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index c4a9a04845..4adbe0d563 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -142,16 +142,9 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 - #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/SPRACINGF3MINI/target.c b/src/main/target/SPRACINGF3MINI/target.c index bc0d06cee6..214dab6564 100644 --- a/src/main/target/SPRACINGF3MINI/target.c +++ b/src/main/target/SPRACINGF3MINI/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 93e2204203..61b4bd9b5e 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -150,15 +150,8 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 - -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/target/STM32F3DISCOVERY/target.c b/src/main/target/STM32F3DISCOVERY/target.c index dcf2426cee..1712bebae2 100644 --- a/src/main/target/STM32F3DISCOVERY/target.c +++ b/src/main/target/STM32F3DISCOVERY/target.c @@ -1,5 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ -#include #include #include @@ -8,16 +23,16 @@ const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed - PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed 0xFFFF }; @@ -30,27 +45,27 @@ const uint16_t multiPWM[] = { PWM6 | (MAP_TO_PWM_INPUT << 8), PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) - PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed) + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed) + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3 PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 + PWM14 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6 0xFFFF }; const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), - PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 - PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), // servo #5 PWM6 | (MAP_TO_SERVO_OUTPUT << 8), PWM7 | (MAP_TO_SERVO_OUTPUT << 8), - PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 0xFFFF }; @@ -63,12 +78,12 @@ const uint16_t airPWM[] = { PWM6 | (MAP_TO_PWM_INPUT << 8), PWM7 | (MAP_TO_PWM_INPUT << 8), PWM8 | (MAP_TO_PWM_INPUT << 8), // input #8 - PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 PWM12 | (MAP_TO_SERVO_OUTPUT << 8), PWM13 | (MAP_TO_SERVO_OUTPUT << 8), - PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 + PWM14 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4 0xFFFF }; diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h index 947ad4913a..7664b47e0c 100644 --- a/src/main/target/STM32F3DISCOVERY/target.h +++ b/src/main/target/STM32F3DISCOVERY/target.h @@ -146,14 +146,8 @@ #define EXTERNAL1_ADC_PIN PC3 #define LED_STRIP -#define LED_STRIP_TIMER TIM16 -#define WS2811_GPIO GPIOB -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOB -#define WS2811_GPIO_AF GPIO_AF_1 -#define WS2811_PIN GPIO_Pin_8 // TIM16_CH1 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PB8 // TIM16_CH1 #define WS2811_TIMER TIM16 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM16 #define WS2811_DMA_CHANNEL DMA1_Channel3 #define WS2811_IRQ DMA1_Channel3_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC3 diff --git a/src/main/target/X_RACERSPI/target.h b/src/main/target/X_RACERSPI/target.h index 39c2e6c209..217765adcf 100644 --- a/src/main/target/X_RACERSPI/target.h +++ b/src/main/target/X_RACERSPI/target.h @@ -124,16 +124,10 @@ #define RSSI_ADC_PIN PB2 #define LED_STRIP -#define LED_STRIP_TIMER TIM1 #define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_GPIO GPIOA -#define WS2811_GPIO_AHB_PERIPHERAL RCC_AHBPeriph_GPIOA -#define WS2811_GPIO_AF GPIO_AF_6 -#define WS2811_PIN GPIO_Pin_8 -#define WS2811_PIN_SOURCE GPIO_PinSource8 +#define WS2811_PIN PA8 #define WS2811_TIMER TIM1 -#define WS2811_TIMER_APB2_PERIPHERAL RCC_APB2Periph_TIM1 #define WS2811_DMA_CHANNEL DMA1_Channel2 #define WS2811_IRQ DMA1_Channel2_IRQn #define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2 diff --git a/src/main/vcpf4/usbd_cdc_vcp.c b/src/main/vcpf4/usbd_cdc_vcp.c index 93ac1aaa5c..8b789fa48f 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.c +++ b/src/main/vcpf4/usbd_cdc_vcp.c @@ -170,20 +170,16 @@ uint32_t CDC_Send_DATA(uint8_t *ptrBuffer, uint8_t sendLength) */ static uint16_t VCP_DataTx(uint8_t* Buf, uint32_t Len) { - uint16_t ptr = APP_Rx_ptr_in; - uint32_t i; - - for (i = 0; i < Len; i++) - APP_Rx_Buffer[ptr++ & (APP_RX_DATA_SIZE-1)] = Buf[i]; - - APP_Rx_ptr_in = ptr % APP_RX_DATA_SIZE; - + for (uint32_t i = 0; i < Len; i++) { + APP_Rx_Buffer[APP_Rx_ptr_in] = Buf[i]; + APP_Rx_ptr_in = (APP_Rx_ptr_in + 1) % APP_RX_DATA_SIZE; + } return USBD_OK; } uint8_t usbAvailable(void) { - return (usbData.rxBufHead != usbData.rxBufTail); + return (usbData.bufferInPosition != usbData.bufferOutPosition); } /******************************************************************************* @@ -198,8 +194,8 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) uint32_t ch = 0; while (usbAvailable() && ch < len) { - recvBuf[ch] = usbData.rxBuf[usbData.rxBufTail]; - usbData.rxBufTail = (usbData.rxBufTail + 1) % USB_RX_BUFSIZE; + recvBuf[ch] = usbData.buffer[usbData.bufferOutPosition]; + usbData.bufferOutPosition = (usbData.bufferOutPosition + 1) % USB_RX_BUFSIZE; ch++; receiveLength--; } @@ -222,20 +218,20 @@ uint32_t CDC_Receive_DATA(uint8_t* recvBuf, uint32_t len) * @param Len: Number of data received (in bytes) * @retval Result of the opeartion: USBD_OK if all operations are OK else VCP_FAIL */ +static uint32_t rxTotalBytes = 0; +static uint32_t rxPackets = 0; + static uint16_t VCP_DataRx(uint8_t* Buf, uint32_t Len) { - uint16_t ptr = usbData.rxBufHead; - uint32_t i; + rxPackets++; - for (i = 0; i < Len; i++) - usbData.rxBuf[ptr++ & (USB_RX_BUFSIZE-1)] = Buf[i]; + for (uint32_t i = 0; i < Len; i++) { + usbData.buffer[usbData.bufferInPosition] = Buf[i]; + usbData.bufferInPosition = (usbData.bufferInPosition + 1) % USB_RX_BUFSIZE; + receiveLength++; + rxTotalBytes++; + } - usbData.rxBufHead = ptr % USB_RX_BUFSIZE; - - receiveLength = ((usbData.rxBufHead - usbData.rxBufTail) > 0 ? - (usbData.rxBufHead - usbData.rxBufTail) : - (usbData.rxBufHead + USB_RX_BUFSIZE - usbData.rxBufTail)) % USB_RX_BUFSIZE; - if(receiveLength > (USB_RX_BUFSIZE-1)) return USBD_FAIL; diff --git a/src/main/vcpf4/usbd_cdc_vcp.h b/src/main/vcpf4/usbd_cdc_vcp.h index 3ca22aa4ef..df1e29001c 100644 --- a/src/main/vcpf4/usbd_cdc_vcp.h +++ b/src/main/vcpf4/usbd_cdc_vcp.h @@ -34,7 +34,7 @@ #include "usbd_usr.h" #include "usbd_desc.h" -#define USB_RX_BUFSIZE 1024 +#define USB_RX_BUFSIZE 2048 __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; @@ -71,9 +71,9 @@ typedef struct } LINE_CODING; typedef struct { - uint8_t rxBuf[USB_RX_BUFSIZE]; - uint16_t rxBufHead; - uint16_t rxBufTail; + uint8_t buffer[USB_RX_BUFSIZE]; + uint16_t bufferInPosition; + uint16_t bufferOutPosition; } usbStruct_t;