50 volatile uint16_t *
ccr;
51 volatile uint16_t *
cr1;
52 volatile uint16_t *
cnt;
76 #define PWM_TIMER_MHZ 1 77 #define PWM_TIMER_8_MHZ 8 81 uint16_t tim_oc_preload;
199 static uint16_t last = 0;
200 static uint8_t chan = 0;
221 if (pwmPorts[port].state == 0) {
222 pwmPorts[port].
rise = capture;
223 pwmPorts[port].
state = 1;
226 pwmPorts[port].
fall = capture;
233 pwmPorts[port].
state = 0;
240 if (pwmPorts[port].state == 0) {
242 pwmPorts[port].
rise = capture;
243 pwmPorts[port].
state = 1;
246 pwmPorts[port].
fall = capture;
254 pwmPorts[port].state = 0;
345 *motors[index]->
ccr = (value - 1000) * motors[index]->period / 1000;
350 *motors[index]->
ccr = value;
353 void pwmInit(
bool useCPPM,
bool usePwmFilter,
bool fastPWM, uint32_t motorPwmRate, uint16_t idlePulseUsec)
355 const uint16_t *
setup;
366 uint8_t port = setup[
i] & 0x0F;
367 uint8_t mask = setup[
i] & 0xF0;
368 uint16_t output = setup[
i] &0x0F00;
370 if (setup[i] == 0x0FFF)
383 }
else if (mask &
TYPE_S) {
386 }
else if (mask &
TYPE_M) {
388 uint32_t hz = mhz * 1000000;
390 uint16_t period = hz / (fastPWM ? 4000 : motorPwmRate);
398 if (motorPwmRate > 500) {
static uint16_t captures[MAX_INPUTS]
uint16_t TIM_OutputNState
void pwmInit(bool useCPPM, bool usePwmFilter, bool fastPWM, uint32_t motorPwmRate, uint16_t idlePulseUsec)
static pwmPortData_t * motors[MAX_PORTS]
void TIM_OC4Init(TIM_TypeDef *TIMx, TIM_OCInitTypeDef *TIM_OCInitStruct)
Initializes the TIMx Channel4 according to the specified parameters in the TIM_OCInitStruct.
static uint8_t sonar_trigger_port
static void ppmCallback(uint8_t port, uint16_t capture)
void TIM_ICInit(TIM_TypeDef *TIMx, TIM_ICInitTypeDef *TIM_ICInitStruct)
Initializes the TIM peripheral according to the specified parameters in the TIM_ICInitStruct.
void TIM_OC4PreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreload)
Enables or disables the TIMx peripheral Preload register on CCR4.
volatile uint32_t millis(void)
float sonarRead(uint8_t channel)
static void pwmWriteStandard(uint8_t index, uint16_t value)
#define TIM_OutputNState_Disable
static uint16_t sonar_reads[MAX_INPUTS]
void timerCCCallbackPtr(uint8_t port, uint16_t capture)
static bool sonar_present
static void pwmCallback(uint8_t port, uint16_t capture)
static void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
void(* pwmWriteFuncPtr)(uint8_t index, uint16_t value)
static void pwmWriteBrushed(uint8_t index, uint16_t value)
#define TIM_OCPolarity_Low
static pwmWriteFuncPtr pwmWritePtr
const timerHardware_t timerHardware[]
void TIM_Cmd(TIM_TypeDef *TIMx, FunctionalState NewState)
Enables or disables the specified TIM peripheral.
static pwmPortData_t * pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uint8_t channel)
void configureTimerCaptureCompareInterrupt(const timerHardware_t *timerHardwarePtr, uint8_t reference, timerCCCallbackPtr *callback)
#define TIM_OCPreload_Enable
static pwmPortData_t pwmPorts[MAX_PORTS]
void TIM_CtrlPWMOutputs(TIM_TypeDef *TIMx, FunctionalState NewState)
Enables or disables the TIM peripheral Main Outputs.
void timerConfigure(const timerHardware_t *timerHardwarePtr, uint16_t period, uint8_t mhz)
void TIM_ICStructInit(TIM_ICInitTypeDef *TIM_ICInitStruct)
Fills each TIM_ICInitStruct member with its default value.
uint16_t pwmRead(uint8_t channel)
void TIM_OC1PreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreload)
Enables or disables the TIMx peripheral Preload register on CCR1.
static const uint16_t multiPPM[]
TIM Input Capture Init structure definition.
void TIM_OC2Init(TIM_TypeDef *TIMx, TIM_OCInitTypeDef *TIM_OCInitStruct)
Initializes the TIMx Channel2 according to the specified parameters in the TIM_OCInitStruct.
void TIM_OC2PreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreload)
Enables or disables the TIMx peripheral Preload register on CCR2.
static const uint16_t multiPPMSONAR[]
#define TIM_OutputState_Enable
static pwmPortData_t * pwmOutConfig(uint8_t port, uint8_t mhz, uint16_t period, uint16_t value)
void TIM_OC1Init(TIM_TypeDef *TIMx, TIM_OCInitTypeDef *TIM_OCInitStruct)
Initializes the TIMx Channel1 according to the specified parameters in the TIM_OCInitStruct.
void TIM_OC3Init(TIM_TypeDef *TIMx, TIM_OCInitTypeDef *TIM_OCInitStruct)
Initializes the TIMx Channel3 according to the specified parameters in the TIM_OCInitStruct.
static void sonarCallback(uint8_t port, uint16_t capture)
static uint32_t pwmLastUpdateTime_ms
#define TIM_OCIdleState_Set
void TIM_OC3PreloadConfig(TIM_TypeDef *TIMx, uint16_t TIM_OCPreload)
Enables or disables the TIMx peripheral Preload register on CCR3.
static void configureSonar(uint8_t port)
TIM Output Compare Init structure definition.
void TIM_OCStructInit(TIM_OCInitTypeDef *TIM_OCInitStruct)
Fills each TIM_OCInitStruct member with its default value.
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
static const uint16_t multiPWM[]
void pwmWriteMotor(uint8_t index, uint16_t value)
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)