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;