rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Dominic Clifton 2014-05-06 07:48:11 +01:00
parent adfbb8ac43
commit 6721587566
13 changed files with 601 additions and 473 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Makefile
View File

@ -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 \

2
src/build_config.c
View File

@ -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"

448
src/drivers/pwm_common.c
View File

@ -1,448 +0,0 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#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];
}

225
src/drivers/pwm_mapping.c Executable file
View File

@ -0,0 +1,225 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#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++;
}
}
}

28
src/drivers/pwm_common.h → src/drivers/pwm_mapping.h
View File

@ -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);

153
src/drivers/pwm_output.c Normal file
View File

@ -0,0 +1,153 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#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);
}

8
src/drivers/pwm_output.h Normal file
View File

@ -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);

170
src/drivers/pwm_rx.c Normal file
View File

@ -0,0 +1,170 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#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;
}

7
src/drivers/pwm_rx.h Normal file
View File

@ -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);

4
src/flight_mixer.c
View File

@ -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"

9
src/main.c
View File

@ -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, &currentProfile.failsafeConfig);
rxInit(&masterConfig.rxConfig, failsafe);

4
src/rx_common.c
View File

@ -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);
}
}

4
src/rx_pwm.c
View File

@ -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;