From 6721587566cf850ddb138cda259e1236e4945ffd Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Tue, 6 May 2014 07:48:11 +0100 Subject: [PATCH] Split PWM code into appropriate separate files - mapping, rx and output. By decoupling everything the structures now only contain members they need. The mapping code is simplified. The calculation of timer periods is now where it belongs (with the output code, not with the mapping code). Also, since each motor output has it's own callback method it is technically possible to mix brushed and brushless motors if the brushed motors and brushless motors use different timers. Additional code would be required to fully support that. --- Makefile | 12 +- src/build_config.c | 2 +- src/drivers/pwm_common.c | 448 -------------------- src/drivers/pwm_mapping.c | 225 ++++++++++ src/drivers/{pwm_common.h => pwm_mapping.h} | 28 +- src/drivers/pwm_output.c | 153 +++++++ src/drivers/pwm_output.h | 8 + src/drivers/pwm_rx.c | 170 ++++++++ src/drivers/pwm_rx.h | 7 + src/flight_mixer.c | 4 +- src/main.c | 9 +- src/rx_common.c | 4 +- src/rx_pwm.c | 4 +- 13 files changed, 601 insertions(+), 473 deletions(-) delete mode 100755 src/drivers/pwm_common.c create mode 100755 src/drivers/pwm_mapping.c rename src/drivers/{pwm_common.h => pwm_mapping.h} (77%) create mode 100644 src/drivers/pwm_output.c create mode 100644 src/drivers/pwm_output.h create mode 100644 src/drivers/pwm_rx.c create mode 100644 src/drivers/pwm_rx.h diff --git a/Makefile b/Makefile index c0ce12cbd..73f5ebb05 100644 --- a/Makefile +++ b/Makefile @@ -142,7 +142,9 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/gpio_stm32f10x.c \ drivers/light_ledring.c \ drivers/sonar_hcsr04.c \ - drivers/pwm_common.c \ + drivers/pwm_mapping.c \ + drivers/pwm_output.c \ + drivers/pwm_rx.c \ drivers/serial_softserial.c \ drivers/serial_uart_common.c \ drivers/serial_uart_stm32f10x.c \ @@ -171,7 +173,9 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \ drivers/bus_i2c_stm32f10x.c \ drivers/bus_spi.c \ drivers/gpio_stm32f10x.c \ - drivers/pwm_common.c \ + drivers/pwm_mapping.c \ + drivers/pwm_output.c \ + drivers/pwm_rx.c \ drivers/serial_softserial.c \ drivers/serial_uart_common.c \ drivers/serial_uart_stm32f10x.c \ @@ -190,7 +194,9 @@ STM32F3DISCOVERY_SRC = startup_stm32f30x_md_gcc.S \ drivers/bus_i2c_stm32f30x.c \ drivers/bus_spi.c \ drivers/gpio_stm32f30x.c \ - drivers/pwm_common.c \ + drivers/pwm_mapping.c \ + drivers/pwm_output.c \ + drivers/pwm_rx.c \ drivers/serial_uart_common.c \ drivers/serial_uart_stm32f30x.c \ drivers/serial_softserial.c \ diff --git a/src/build_config.c b/src/build_config.c index a9c11a471..5a868705a 100644 --- a/src/build_config.c +++ b/src/build_config.c @@ -10,7 +10,7 @@ #include "drivers/gpio_common.h" #include "drivers/timer_common.h" -#include "drivers/pwm_common.h" +#include "drivers/pwm_mapping.h" #include "flight_mixer.h" #include "drivers/serial_common.h" diff --git a/src/drivers/pwm_common.c b/src/drivers/pwm_common.c deleted file mode 100755 index cb783a060..000000000 --- a/src/drivers/pwm_common.c +++ /dev/null @@ -1,448 +0,0 @@ - -#include -#include - -#include - -#include "platform.h" - -#include "gpio_common.h" -#include "timer_common.h" - -#include "failsafe.h" // FIXME dependency into the main code from a driver - -#include "pwm_common.h" -/* - Configuration maps - - Note: this documentation is only valid for STM32F10x, for STM32F30x please read the code itself. - - 1) multirotor PPM input - PWM1 used for PPM - PWM5..8 used for motors - PWM9..10 used for servo or else motors - PWM11..14 used for motors - - 2) multirotor PPM input with more servos - PWM1 used for PPM - PWM5..8 used for motors - PWM9..10 used for servo or else motors - PWM11..14 used for servos - - 2) multirotor PWM input - PWM1..8 used for input - PWM9..10 used for servo or else motors - PWM11..14 used for motors - - 3) airplane / flying wing w/PWM - PWM1..8 used for input - PWM9 used for motor throttle +PWM10 for 2nd motor - PWM11.14 used for servos - - 4) airplane / flying wing with PPM - PWM1 used for PPM - PWM5..8 used for servos - PWM9 used for motor throttle +PWM10 for 2nd motor - PWM11.14 used for servos -*/ - -typedef struct { -#ifdef STM32F303xC - volatile uint32_t *ccr; -#endif - -#ifdef STM32F10X_MD - volatile uint16_t *ccr; -#endif - uint16_t period; - - // for input only - uint8_t channel; - uint8_t state; - captureCompare_t rise; - captureCompare_t fall; - captureCompare_t capture; -} pwmPortData_t; - -enum { - TYPE_IP = 0x10, - TYPE_IW = 0x20, - TYPE_M = 0x40, - TYPE_S = 0x80 -}; - -typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors - -static pwmPortData_t pwmPorts[USABLE_TIMER_CHANNEL_COUNT]; -static uint16_t captures[MAX_PPM_AND_PWM_INPUTS]; -static pwmPortData_t *motors[MAX_PWM_MOTORS]; -static pwmPortData_t *servos[MAX_PWM_SERVOS]; -static pwmWriteFuncPtr pwmWritePtr = NULL; -static uint8_t numMotors = 0; -static uint8_t numServos = 0; -static uint8_t numInputs = 0; -static uint16_t failsafeThreshold = 985; - -static const uint8_t multiPPM[] = { - PWM1 | TYPE_IP, // PPM input - PWM9 | TYPE_M, // Swap to servo if needed - PWM10 | TYPE_M, // Swap to servo if needed - PWM11 | TYPE_M, - PWM12 | TYPE_M, - PWM13 | TYPE_M, - PWM14 | TYPE_M, - PWM5 | TYPE_M, // Swap to servo if needed - PWM6 | TYPE_M, // Swap to servo if needed - PWM7 | TYPE_M, // Swap to servo if needed - PWM8 | TYPE_M, // Swap to servo if needed - 0xFF -}; - -static const uint8_t multiPWM[] = { - PWM1 | TYPE_IW, // input #1 - PWM2 | TYPE_IW, - PWM3 | TYPE_IW, - PWM4 | TYPE_IW, - PWM5 | TYPE_IW, - PWM6 | TYPE_IW, - PWM7 | TYPE_IW, - PWM8 | TYPE_IW, // input #8 - PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed) - PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed) - PWM11 | TYPE_M, // motor #1 or #3 - PWM12 | TYPE_M, - PWM13 | TYPE_M, - PWM14 | TYPE_M, // motor #4 or #6 - 0xFF -}; - -static const uint8_t airPPM[] = { - PWM1 | TYPE_IP, // PPM input - PWM9 | TYPE_M, // motor #1 - PWM10 | TYPE_M, // motor #2 - PWM11 | TYPE_S, // servo #1 - PWM12 | TYPE_S, - PWM13 | TYPE_S, - PWM14 | TYPE_S, // servo #4 - PWM5 | TYPE_S, // servo #5 - PWM6 | TYPE_S, - PWM7 | TYPE_S, - PWM8 | TYPE_S, // servo #8 - 0xFF -}; - -static const uint8_t airPWM[] = { - PWM1 | TYPE_IW, // input #1 - PWM2 | TYPE_IW, - PWM3 | TYPE_IW, - PWM4 | TYPE_IW, - PWM5 | TYPE_IW, - PWM6 | TYPE_IW, - PWM7 | TYPE_IW, - PWM8 | TYPE_IW, // input #8 - PWM9 | TYPE_M, // motor #1 - PWM10 | TYPE_M, // motor #2 - PWM11 | TYPE_S, // servo #1 - PWM12 | TYPE_S, - PWM13 | TYPE_S, - PWM14 | TYPE_S, // servo #4 - 0xFF -}; - -static const uint8_t * const hardwareMaps[] = { - multiPWM, - multiPPM, - airPWM, - airPPM, -}; - -#define PWM_TIMER_MHZ 1 -#define PWM_BRUSHED_TIMER_MHZ 8 - -failsafe_t *failsafe; - -static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) -{ - TIM_OCInitTypeDef TIM_OCInitStructure; - - TIM_OCStructInit(&TIM_OCInitStructure); - TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; - TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; - TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; - TIM_OCInitStructure.TIM_Pulse = value; - TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; - TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; - - switch (channel) { - case TIM_Channel_1: - TIM_OC1Init(tim, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_2: - TIM_OC2Init(tim, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_3: - TIM_OC3Init(tim, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); - break; - case TIM_Channel_4: - TIM_OC4Init(tim, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); - break; - } -} - -void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) -{ - TIM_ICInitTypeDef TIM_ICInitStructure; - - TIM_ICStructInit(&TIM_ICInitStructure); - TIM_ICInitStructure.TIM_Channel = channel; - TIM_ICInitStructure.TIM_ICPolarity = polarity; - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; - - TIM_ICInit(tim, &TIM_ICInitStructure); -} - -static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) -{ - gpio_config_t cfg; - - cfg.pin = pin; - cfg.mode = mode; - cfg.speed = Speed_2MHz; - gpioInit(gpio, &cfg); -} - -static pwmPortData_t *pwmOutConfig(uint8_t port, uint8_t mhz, uint16_t period, uint16_t value) -{ - pwmPortData_t *p = &pwmPorts[port]; - configTimeBase(timerHardware[port].tim, period, mhz); - pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP); - pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value); - if (timerHardware[port].outputEnable) - TIM_CtrlPWMOutputs(timerHardware[port].tim, ENABLE); - TIM_Cmd(timerHardware[port].tim, ENABLE); - - switch (timerHardware[port].channel) { - case TIM_Channel_1: - p->ccr = &timerHardware[port].tim->CCR1; - break; - case TIM_Channel_2: - p->ccr = &timerHardware[port].tim->CCR2; - break; - case TIM_Channel_3: - p->ccr = &timerHardware[port].tim->CCR3; - break; - case TIM_Channel_4: - p->ccr = &timerHardware[port].tim->CCR4; - break; - } - p->period = period; - return p; -} - -static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel) -{ - pwmPortData_t *p = &pwmPorts[port]; - const timerHardware_t *timerHardwarePtr = &(timerHardware[port]); - - p->channel = channel; - - pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); - pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); - - timerConfigure(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ); - configureTimerCaptureCompareInterrupt(timerHardwarePtr, port, callback); - - return p; -} - -static void failsafeCheck(uint8_t channel, uint16_t pulse) -{ - static uint8_t goodPulses; - - if (channel < 4 && pulse > failsafeThreshold) - goodPulses |= (1 << channel); // if signal is valid - mark channel as OK - if (goodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter - goodPulses = 0; - failsafe->vTable->validDataReceived(); - } -} - -static void ppmCallback(uint8_t port, captureCompare_t capture) -{ - captureCompare_t diff; - static captureCompare_t now; - static captureCompare_t last = 0; - - static uint8_t chan = 0; - - last = now; - now = capture; - diff = now - last; - - if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe." - chan = 0; - } else { - if (diff > PULSE_MIN && diff < PULSE_MAX && chan < MAX_PPM_AND_PWM_INPUTS) { // 750 to 2250 ms is our 'valid' channel range - captures[chan] = diff; - failsafeCheck(chan, diff); - } - chan++; - } -} - -static void pwmCallback(uint8_t port, captureCompare_t capture) -{ - if (pwmPorts[port].state == 0) { - pwmPorts[port].rise = capture; - pwmPorts[port].state = 1; - pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Falling); - } else { - pwmPorts[port].fall = capture; - // compute capture - pwmPorts[port].capture = pwmPorts[port].fall - pwmPorts[port].rise; - if (pwmPorts[port].capture > PULSE_MIN && pwmPorts[port].capture < PULSE_MAX) { // valid pulse width - captures[pwmPorts[port].channel] = pwmPorts[port].capture; - failsafeCheck(pwmPorts[port].channel, pwmPorts[port].capture); - } - // switch state - pwmPorts[port].state = 0; - pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising); - } -} - -static void pwmWriteBrushed(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000; -} - -static void pwmWriteStandard(uint8_t index, uint16_t value) -{ - *motors[index]->ccr = value; -} - -void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe) -{ - int i = 0; - const uint8_t *setup; - - failsafe = initialFailsafe; - - // to avoid importing cfg/mcfg - failsafeThreshold = init->failsafeThreshold; - - // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] - if (init->airplane) - i = 2; // switch to air hardware config - if (init->usePPM) - i++; // next index is for PPM - - setup = hardwareMaps[i]; - - for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { - uint8_t port = setup[i] & 0x0F; - uint8_t mask = setup[i] & 0xF0; - - if (setup[i] == 0xFF) // terminator - break; - -#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER - // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E - if (port == PWM2) - continue; -#endif - -#ifdef STM32F10X_MD - // skip UART ports for GPS - if (init->useUART && (port == PWM3 || port == PWM4)) - continue; - - // skip softSerial ports - if (init->useSoftSerial && (port == PWM5 || port == PWM6 || port == PWM7 || port == PWM8)) - continue; -#endif -#ifdef STM32F303xC - // skip softSerial ports - if (init->useSoftSerial && (port == PWM9 || port == PWM10 || port == PWM11 || port == PWM12)) - continue; -#endif - - // skip ADC for powerMeter if configured - if (init->adcChannel && (init->adcChannel == port)) - continue; - - // hacks to allow current functionality - if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput) - mask = 0; - - if (init->useServos && !init->airplane) { -#ifdef STM32F10X_MD - // remap PWM9+10 as servos - if (port == PWM9 || port == PWM10) - mask = TYPE_S; -#endif -#ifdef STM32F303xC - // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer - if (init->useSoftSerial) { - if (port == PWM5 || port == PWM6) - mask = TYPE_S; - } else { - if (port == PWM9 || port == PWM10) - mask = TYPE_S; - } -#endif - } - - if (init->extraServos && !init->airplane) { - // remap PWM5..8 as servos when used in extended servo mode - if (port >= PWM5 && port <= PWM8) - mask = TYPE_S; - } - - if (mask & TYPE_IP) { - pwmInConfig(port, ppmCallback, 0); - numInputs = 8; - } else if (mask & TYPE_IW) { - pwmInConfig(port, pwmCallback, numInputs); - numInputs++; - } else if (mask & TYPE_M) { - uint32_t hz, mhz; - if (init->motorPwmRate > 500) - mhz = PWM_BRUSHED_TIMER_MHZ; - else - mhz = PWM_TIMER_MHZ; - hz = mhz * 1000000; - - motors[numMotors++] = pwmOutConfig(port, mhz, hz / init->motorPwmRate, init->idlePulse); - } else if (mask & TYPE_S) { - servos[numServos++] = pwmOutConfig(port, PWM_TIMER_MHZ, 1000000 / init->servoPwmRate, init->servoCenterPulse); - } - } - - // determine motor writer function - pwmWritePtr = pwmWriteStandard; - if (init->motorPwmRate > 500) - pwmWritePtr = pwmWriteBrushed; -} - -void pwmWriteMotor(uint8_t index, uint16_t value) -{ - if (index < numMotors) - pwmWritePtr(index, value); -} - -void pwmWriteServo(uint8_t index, uint16_t value) -{ - if (index < numServos) - *servos[index]->ccr = value; -} - -uint16_t pwmRead(uint8_t channel) -{ - return captures[channel]; -} diff --git a/src/drivers/pwm_mapping.c b/src/drivers/pwm_mapping.c new file mode 100755 index 000000000..332358783 --- /dev/null +++ b/src/drivers/pwm_mapping.c @@ -0,0 +1,225 @@ + +#include +#include + +#include + +#include "platform.h" + +#include "gpio_common.h" +#include "timer_common.h" + +#include "pwm_output.h" +#include "pwm_rx.h" +#include "pwm_mapping.h" +/* + Configuration maps + + Note: this documentation is only valid for STM32F10x, for STM32F30x please read the code itself. + + 1) multirotor PPM input + PWM1 used for PPM + PWM5..8 used for motors + PWM9..10 used for servo or else motors + PWM11..14 used for motors + + 2) multirotor PPM input with more servos + PWM1 used for PPM + PWM5..8 used for motors + PWM9..10 used for servo or else motors + PWM11..14 used for servos + + 2) multirotor PWM input + PWM1..8 used for input + PWM9..10 used for servo or else motors + PWM11..14 used for motors + + 3) airplane / flying wing w/PWM + PWM1..8 used for input + PWM9 used for motor throttle +PWM10 for 2nd motor + PWM11.14 used for servos + + 4) airplane / flying wing with PPM + PWM1 used for PPM + PWM5..8 used for servos + PWM9 used for motor throttle +PWM10 for 2nd motor + PWM11.14 used for servos +*/ + +enum { + TYPE_IP = 0x10, + TYPE_IW = 0x20, + TYPE_M = 0x40, + TYPE_S = 0x80 +}; + +static const uint8_t multiPPM[] = { + PWM1 | TYPE_IP, // PPM input + PWM9 | TYPE_M, // Swap to servo if needed + PWM10 | TYPE_M, // Swap to servo if needed + PWM11 | TYPE_M, + PWM12 | TYPE_M, + PWM13 | TYPE_M, + PWM14 | TYPE_M, + PWM5 | TYPE_M, // Swap to servo if needed + PWM6 | TYPE_M, // Swap to servo if needed + PWM7 | TYPE_M, // Swap to servo if needed + PWM8 | TYPE_M, // Swap to servo if needed + 0xFF +}; + +static const uint8_t multiPWM[] = { + PWM1 | TYPE_IW, // input #1 + PWM2 | TYPE_IW, + PWM3 | TYPE_IW, + PWM4 | TYPE_IW, + PWM5 | TYPE_IW, + PWM6 | TYPE_IW, + PWM7 | TYPE_IW, + PWM8 | TYPE_IW, // input #8 + PWM9 | TYPE_M, // motor #1 or servo #1 (swap to servo if needed) + PWM10 | TYPE_M, // motor #2 or servo #2 (swap to servo if needed) + PWM11 | TYPE_M, // motor #1 or #3 + PWM12 | TYPE_M, + PWM13 | TYPE_M, + PWM14 | TYPE_M, // motor #4 or #6 + 0xFF +}; + +static const uint8_t airPPM[] = { + PWM1 | TYPE_IP, // PPM input + PWM9 | TYPE_M, // motor #1 + PWM10 | TYPE_M, // motor #2 + PWM11 | TYPE_S, // servo #1 + PWM12 | TYPE_S, + PWM13 | TYPE_S, + PWM14 | TYPE_S, // servo #4 + PWM5 | TYPE_S, // servo #5 + PWM6 | TYPE_S, + PWM7 | TYPE_S, + PWM8 | TYPE_S, // servo #8 + 0xFF +}; + +static const uint8_t airPWM[] = { + PWM1 | TYPE_IW, // input #1 + PWM2 | TYPE_IW, + PWM3 | TYPE_IW, + PWM4 | TYPE_IW, + PWM5 | TYPE_IW, + PWM6 | TYPE_IW, + PWM7 | TYPE_IW, + PWM8 | TYPE_IW, // input #8 + PWM9 | TYPE_M, // motor #1 + PWM10 | TYPE_M, // motor #2 + PWM11 | TYPE_S, // servo #1 + PWM12 | TYPE_S, + PWM13 | TYPE_S, + PWM14 | TYPE_S, // servo #4 + 0xFF +}; + +static const uint8_t * const hardwareMaps[] = { + multiPWM, + multiPPM, + airPWM, + airPPM, +}; + +void pwmInit(drv_pwm_config_t *init) +{ + int i = 0; + const uint8_t *setup; + + int channelIndex = 0; + int servoIndex = 0; + int motorIndex = 0; + + // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] + if (init->airplane) + i = 2; // switch to air hardware config + if (init->usePPM) + i++; // next index is for PPM + + setup = hardwareMaps[i]; + + for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + uint8_t timerIndex = setup[i] & 0x0F; + uint8_t mask = setup[i] & 0xF0; + + if (setup[i] == 0xFF) // terminator + break; + +#ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER + // PWM2 is connected to LED2 on the board and cannot be connected unless you cut LED2_E + if (timerIndex == PWM2) + continue; +#endif + +#ifdef STM32F10X_MD + // skip UART ports for GPS + if (init->useUART && (timerIndex == PWM3 || timerIndex == PWM4)) + continue; + + // skip softSerial ports + if (init->useSoftSerial && (timerIndex == PWM5 || timerIndex == PWM6 || timerIndex == PWM7 || timerIndex == PWM8)) + continue; +#endif +#ifdef STM32F303xC + // skip softSerial ports + if (init->useSoftSerial && (timerIndex == PWM9 || timerIndex == PWM10 || timerIndex == PWM11 || timerIndex == PWM12)) + continue; +#endif + + // skip ADC for powerMeter if configured + if (init->adcChannel && (init->adcChannel == timerIndex)) + continue; + + // hacks to allow current functionality + if (mask & (TYPE_IP | TYPE_IW) && !init->enableInput) + mask = 0; + + if (init->useServos && !init->airplane) { +#ifdef STM32F10X_MD + // remap PWM9+10 as servos + if (timerIndex == PWM9 || timerIndex == PWM10) + mask = TYPE_S; +#endif +#ifdef STM32F303xC + // remap PWM 5+6 or 9+10 as servos - softserial pin pairs require timer ports that use the same timer + if (init->useSoftSerial) { + if (timerIndex == PWM5 || timerIndex == PWM6) + mask = TYPE_S; + } else { + if (timerIndex == PWM9 || timerIndex == PWM10) + mask = TYPE_S; + } +#endif + } + + if (init->extraServos && !init->airplane) { + // remap PWM5..8 as servos when used in extended servo mode + if (timerIndex >= PWM5 && timerIndex <= PWM8) + mask = TYPE_S; + } + + if (mask & TYPE_IP) { + ppmInConfig(timerIndex); + } else if (mask & TYPE_IW) { + pwmInConfig(timerIndex, channelIndex); + channelIndex++; + } else if (mask & TYPE_M) { + + + if (init->motorPwmRate > 500) { + pwmBrushedMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse); + } else { + pwmBrushlessMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse); + } + motorIndex++; + } else if (mask & TYPE_S) { + pwmServoConfig(&timerHardware[timerIndex], servoIndex, init->servoPwmRate, init->servoCenterPulse); + servoIndex++; + } + } +} diff --git a/src/drivers/pwm_common.h b/src/drivers/pwm_mapping.h similarity index 77% rename from src/drivers/pwm_common.h rename to src/drivers/pwm_mapping.h index c210c593a..0efc8cef6 100755 --- a/src/drivers/pwm_common.h +++ b/src/drivers/pwm_mapping.h @@ -2,16 +2,29 @@ #define MAX_PWM_MOTORS 12 #define MAX_PWM_SERVOS 8 -#define MAX_PPM_AND_PWM_INPUTS 8 + +#define MAX_PWM_INPUT_PORTS 8 + +#define MAX_MOTORS 12 +#define MAX_SERVOS 8 +#define MAX_PWM_OUTPUT_PORTS MAX_PWM_MOTORS // must be set to the largest of either MAX_MOTORS or MAX_SERVOS + +#if MAX_PWM_OUTPUT_PORTS < MAX_MOTORS || MAX_PWM_OUTPUT_PORTS < MAX_SERVOS +#error Invalid motor/servo/port configuration +#endif + #define PULSE_1MS (1000) // 1ms pulse width #define PULSE_MIN (750) // minimum PWM pulse width which is considered valid #define PULSE_MAX (2250) // maximum PWM pulse width which is considered valid -#define MAX_MOTORS 12 -#define MAX_SERVOS 8 + + + #define MAX_INPUTS 8 +#define PWM_TIMER_MHZ 1 + typedef struct drv_pwm_config_t { bool enableInput; bool usePPM; @@ -26,7 +39,6 @@ typedef struct drv_pwm_config_t { uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. uint16_t servoCenterPulse; - uint16_t failsafeThreshold; } drv_pwm_config_t; // This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data. @@ -46,11 +58,3 @@ enum { PWM13, PWM14 }; - -void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); - -void pwmWriteMotor(uint8_t index, uint16_t value); -void pwmWriteServo(uint8_t index, uint16_t value); -uint16_t pwmRead(uint8_t channel); - -// void pwmWrite(uint8_t channel, uint16_t value); diff --git a/src/drivers/pwm_output.c b/src/drivers/pwm_output.c new file mode 100644 index 000000000..4a340c7cc --- /dev/null +++ b/src/drivers/pwm_output.c @@ -0,0 +1,153 @@ +#include +#include + +#include + +#include "platform.h" + +#include "gpio_common.h" +#include "timer_common.h" + +#include "failsafe.h" // FIXME dependency into the main code from a driver + +#include "pwm_mapping.h" + +#include "pwm_output.h" + +typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors + +typedef struct { +#ifdef STM32F303xC + volatile uint32_t *ccr; +#endif + +#ifdef STM32F10X_MD + volatile uint16_t *ccr; +#endif + uint16_t period; + pwmWriteFuncPtr pwmWritePtr; +} pwmOutputPort_t; + +pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; + +static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; +static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; + +#define PWM_BRUSHED_TIMER_MHZ 8 + +static uint8_t allocatedOutputPortCount = 0; + +static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) +{ + TIM_OCInitTypeDef TIM_OCInitStructure; + + TIM_OCStructInit(&TIM_OCInitStructure); + TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2; + TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; + TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable; + TIM_OCInitStructure.TIM_Pulse = value; + TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; + TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set; + + switch (channel) { + case TIM_Channel_1: + TIM_OC1Init(tim, &TIM_OCInitStructure); + TIM_OC1PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_2: + TIM_OC2Init(tim, &TIM_OCInitStructure); + TIM_OC2PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_3: + TIM_OC3Init(tim, &TIM_OCInitStructure); + TIM_OC3PreloadConfig(tim, TIM_OCPreload_Enable); + break; + case TIM_Channel_4: + TIM_OC4Init(tim, &TIM_OCInitStructure); + TIM_OC4PreloadConfig(tim, TIM_OCPreload_Enable); + break; + } +} + +static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) +{ + gpio_config_t cfg; + + cfg.pin = pin; + cfg.mode = mode; + cfg.speed = Speed_2MHz; + gpioInit(gpio, &cfg); +} + +static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +{ + pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; + + configTimeBase(timerHardware->tim, period, mhz); + pwmGPIOConfig(timerHardware->gpio, timerHardware->pin, Mode_AF_PP); + pwmOCConfig(timerHardware->tim, timerHardware->channel, value); + if (timerHardware->outputEnable) + TIM_CtrlPWMOutputs(timerHardware->tim, ENABLE); + TIM_Cmd(timerHardware->tim, ENABLE); + + switch (timerHardware->channel) { + case TIM_Channel_1: + p->ccr = &timerHardware->tim->CCR1; + break; + case TIM_Channel_2: + p->ccr = &timerHardware->tim->CCR2; + break; + case TIM_Channel_3: + p->ccr = &timerHardware->tim->CCR3; + break; + case TIM_Channel_4: + p->ccr = &timerHardware->tim->CCR4; + break; + } + p->period = period; + + return p; +} + +static void pwmWriteBrushed(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000; +} + +static void pwmWriteStandard(uint8_t index, uint16_t value) +{ + *motors[index]->ccr = value; +} + +void pwmWriteMotor(uint8_t index, uint16_t value) +{ + if (motors[index] && index < MAX_MOTORS) + motors[index]->pwmWritePtr(index, value); +} + +void pwmWriteServo(uint8_t index, uint16_t value) +{ + if (servos[index] && index < MAX_SERVOS) + *servos[index]->ccr = value; +} + + +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +{ + uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse); + motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; + +} + +void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) +{ + uint32_t hz = PWM_TIMER_MHZ * 1000000; + motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); + motors[motorIndex]->pwmWritePtr = pwmWriteStandard; +} + +void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse) +{ + servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse); +} diff --git a/src/drivers/pwm_output.h b/src/drivers/pwm_output.h new file mode 100644 index 000000000..ca441032d --- /dev/null +++ b/src/drivers/pwm_output.h @@ -0,0 +1,8 @@ +#pragma once + +void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); +void pwmWriteMotor(uint8_t index, uint16_t value); + +void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); +void pwmWriteServo(uint8_t index, uint16_t value); diff --git a/src/drivers/pwm_rx.c b/src/drivers/pwm_rx.c new file mode 100644 index 000000000..51130c0a1 --- /dev/null +++ b/src/drivers/pwm_rx.c @@ -0,0 +1,170 @@ + +#include +#include + +#include + +#include "platform.h" + +#include "gpio_common.h" +#include "timer_common.h" + +#include "failsafe.h" // FIXME dependency into the main code from a driver + +#include "pwm_mapping.h" + +#include "pwm_rx.h" + +failsafe_t *failsafe; +failsafeConfig_t *failsafeConfig; + +void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); // from pwm_output.c + +typedef enum { + INPUT_MODE_PPM, + INPUT_MODE_PWM, +} pwmInputMode_t; + +typedef struct { + pwmInputMode_t mode; + uint8_t channel; // only used for pwm, ignored by ppm + + uint8_t state; + captureCompare_t rise; + captureCompare_t fall; + captureCompare_t capture; + + const timerHardware_t *timerHardware; +} pwmInputPort_t; + +static pwmInputPort_t pwmInputPorts[MAX_PWM_INPUT_PORTS]; + +static uint16_t captures[MAX_PWM_INPUT_PORTS]; + +static void failsafeCheck(uint8_t channel, uint16_t pulse) +{ + static uint8_t goodPulses; + + if (channel < 4 && pulse > failsafeConfig->failsafe_detect_threshold) + goodPulses |= (1 << channel); // if signal is valid - mark channel as OK + if (goodPulses == 0x0F) { // If first four chanells have good pulses, clear FailSafe counter + goodPulses = 0; + failsafe->vTable->validDataReceived(); + } +} + +static void ppmCallback(uint8_t port, captureCompare_t capture) +{ + captureCompare_t diff; + static captureCompare_t now; + static captureCompare_t last = 0; + + static uint8_t chan = 0; + + last = now; + now = capture; + diff = now - last; + + if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe." + chan = 0; + } else { + if (diff > PULSE_MIN && diff < PULSE_MAX && chan < MAX_PWM_INPUT_PORTS) { // 750 to 2250 ms is our 'valid' channel range + captures[chan] = diff; + failsafeCheck(chan, diff); + } + chan++; + } +} + +static void pwmCallback(uint8_t port, captureCompare_t capture) +{ + pwmInputPort_t *pwmInputPort = &pwmInputPorts[port]; + const timerHardware_t *timerHardware = pwmInputPort->timerHardware; + + if (pwmInputPort->state == 0) { + pwmInputPort->rise = capture; + pwmInputPort->state = 1; + pwmICConfig(timerHardware->tim, timerHardware->channel, TIM_ICPolarity_Falling); + } else { + pwmInputPort->fall = capture; + // compute capture + pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise; + if (pwmInputPort->capture > PULSE_MIN && pwmInputPort->capture < PULSE_MAX) { // valid pulse width + captures[pwmInputPort->channel] = pwmInputPort->capture; + failsafeCheck(pwmInputPort->channel, pwmInputPort->capture); + } + // switch state + pwmInputPort->state = 0; + pwmICConfig(timerHardware->tim, timerHardware->channel, TIM_ICPolarity_Rising); + } +} + +static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode) +{ + gpio_config_t cfg; + + cfg.pin = pin; + cfg.mode = mode; + cfg.speed = Speed_2MHz; + gpioInit(gpio, &cfg); +} + +void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) +{ + TIM_ICInitTypeDef TIM_ICInitStructure; + + TIM_ICStructInit(&TIM_ICInitStructure); + TIM_ICInitStructure.TIM_Channel = channel; + TIM_ICInitStructure.TIM_ICPolarity = polarity; + TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; + TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; + TIM_ICInitStructure.TIM_ICFilter = 0x0; + + TIM_ICInit(tim, &TIM_ICInitStructure); +} + +void pwmInConfig(uint8_t timerIndex, uint8_t channel) +{ + pwmInputPort_t *p = &pwmInputPorts[channel]; + + const timerHardware_t *timerHardwarePtr = &(timerHardware[timerIndex]); + + p->channel = channel; + p->mode = INPUT_MODE_PWM; + p->timerHardware = timerHardwarePtr; + + pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); + pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); + + timerConfigure(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ); + configureTimerCaptureCompareInterrupt(timerHardwarePtr, channel, pwmCallback); +} + +#define UNUSED_PPM_TIMER_REFERENCE 0 +#define FIRST_PWM_PORT 0 + +void ppmInConfig(uint8_t timerIndex) +{ + pwmInputPort_t *p = &pwmInputPorts[FIRST_PWM_PORT]; + + const timerHardware_t *timerHardwarePtr = &(timerHardware[timerIndex]); + + p->mode = INPUT_MODE_PPM; + + pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode); + pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); + + timerConfigure(timerHardwarePtr, 0xFFFF, PWM_TIMER_MHZ); + configureTimerCaptureCompareInterrupt(timerHardwarePtr, UNUSED_PPM_TIMER_REFERENCE, ppmCallback); +} + +uint16_t pwmRead(uint8_t channel) +{ + return captures[channel]; +} + +void pwmRxInit(failsafe_t *initialFailsafe, failsafeConfig_t *initialFailsafeConfig) +{ + failsafe = initialFailsafe; + failsafeConfig = initialFailsafeConfig; +} diff --git a/src/drivers/pwm_rx.h b/src/drivers/pwm_rx.h new file mode 100644 index 000000000..b03fd64e2 --- /dev/null +++ b/src/drivers/pwm_rx.h @@ -0,0 +1,7 @@ +#pragma once + +void ppmInConfig(uint8_t timerIndex); +void pwmInConfig(uint8_t timerIndex, uint8_t channel); + +uint16_t pwmRead(uint8_t channel); + diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 04e512ffb..470caa19c 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -7,7 +7,9 @@ #include "common/axis.h" #include "common/maths.h" -#include "drivers/pwm_common.h" +#include "drivers/gpio_common.h" +#include "drivers/timer_common.h" +#include "drivers/pwm_output.h" #include "gimbal.h" #include "escservo.h" diff --git a/src/main.c b/src/main.c index 84adb3434..f78a14b8e 100755 --- a/src/main.c +++ b/src/main.c @@ -15,7 +15,7 @@ #include "drivers/serial_softserial.h" #include "drivers/serial_uart_common.h" #include "drivers/accgyro_common.h" -#include "drivers/pwm_common.h" +#include "drivers/pwm_mapping.h" #include "drivers/adc_common.h" #include "flight_common.h" @@ -56,7 +56,8 @@ void timerInit(void); void initTelemetry(serialPorts_t *serialPorts); void serialInit(serialConfig_t *initialSerialConfig); failsafe_t* failsafeInit(rxConfig_t *intialRxConfig); -void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe); +void pwmInit(drv_pwm_config_t *init); +void pwmRxInit(failsafe_t *initialFailsafe, failsafeConfig_t *initialFailsafeConfig); void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe); void buzzerInit(failsafe_t *initialFailsafe); void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); @@ -172,7 +173,6 @@ int main(void) if (pwm_params.motorPwmRate > 500) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; - pwm_params.failsafeThreshold = currentProfile.failsafeConfig.failsafe_detect_threshold; switch (masterConfig.power_adc_channel) { case 1: @@ -191,7 +191,8 @@ int main(void) #ifndef FY90Q timerInit(); #endif - pwmInit(&pwm_params, failsafe); + pwmInit(&pwm_params); + pwmRxInit(failsafe, ¤tProfile.failsafeConfig); rxInit(&masterConfig.rxConfig, failsafe); diff --git a/src/rx_common.c b/src/rx_common.c index 451514cc3..e186f09a2 100644 --- a/src/rx_common.c +++ b/src/rx_common.c @@ -18,7 +18,7 @@ #include "rx_common.h" -void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *failsafe, rcReadRawDataPtr *callback); +void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *failsafe, rcReadRawDataPtr *callback); void sbusInit(rxConfig_t *initialRxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback); void spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback); void sumdInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback); @@ -47,7 +47,7 @@ void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe) if (feature(FEATURE_SERIALRX)) { serialRxInit(rxConfig, failsafe); } else { - pwmRxInit(&rxRuntimeConfig, failsafe, &rcReadRawFunc); + rxPwmInit(&rxRuntimeConfig, failsafe, &rcReadRawFunc); } } diff --git a/src/rx_pwm.c b/src/rx_pwm.c index d7a19ef4d..2ee00a44d 100644 --- a/src/rx_pwm.c +++ b/src/rx_pwm.c @@ -6,7 +6,7 @@ #include "platform.h" -#include "drivers/pwm_common.h" +#include "drivers/pwm_rx.h" #include "config.h" @@ -20,7 +20,7 @@ static uint16_t pwmReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeC return pwmRead(rxConfig->rcmap[chan]); } -void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback) +void rxPwmInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback) { // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled *callback = pwmReadRawRC;