void servoInit(const servoConfig_t *servoConfig)
{
for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) {
const ioTag_t tag = servoConfig->ioTags[servoIndex];
if (!tag) {
break;
}
servos[servoIndex].io = IOGetByTag(tag);
IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_INDEX(servoIndex));
IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP);
const timerHardware_t *timer = timerGetByTag(tag, TIM_USE_ANY);
if (timer == NULL) {
/* flag failure and disable ability to arm */
break;
}
pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse);
servos[servoIndex].enabled = true;
}
}
void pwmRxInit(const pwmConfig_t *pwmConfig)
{
inputFilteringMode = pwmConfig->inputFilteringMode;
for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) {
pwmInputPort_t *port = &pwmInputPorts[channel];
const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIM_USE_ANY);
if (!timer) {
/* TODO: maybe fail here if not enough channels? */
continue;
}
port->state = 0;
port->missedEvents = 0;
port->channel = channel;
port->mode = INPUT_MODE_PWM;
port->timerHardware = timer;
IO_t io = IOGetByTag(pwmConfig->ioTags[channel]);
IOInit(io, OWNER_PWMINPUT, RESOURCE_INDEX(channel));
#ifdef STM32F1
IOConfigGPIO(io, IOCFG_IPD);
#else
IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
#endif
timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
#if defined(USE_HAL_DRIVER)
pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
#else
pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
#endif
}
}
void ppmRxInit(const ppmConfig_t *ppmConfig)
{
ppmResetDevice();
pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT];
const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIM_USE_ANY);
if (!timer) {
/* TODO: fail here? */
return;
}
ppmAvoidPWMTimerClash(timer->tim);
port->mode = INPUT_MODE_PPM;
port->timerHardware = timer;
IO_t io = IOGetByTag(ppmConfig->ioTag);
IOInit(io, OWNER_PPMINPUT, 0);
#ifdef STM32F1
IOConfigGPIO(io, IOCFG_IPD);
#else
IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
#endif
timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);
timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
#if defined(USE_HAL_DRIVER)
pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
#else
pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
#endif
}
serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr rxCallback, uint32_t baud, portMode_t mode, portOptions_t options)
{
softSerial_t *softSerial = &(softSerialPorts[portIndex]);
int pinCfgIndex = portIndex + RESOURCE_SOFT_OFFSET;
ioTag_t tagRx = serialPinConfig()->ioTagRx[pinCfgIndex];
ioTag_t tagTx = serialPinConfig()->ioTagTx[pinCfgIndex];
const timerHardware_t *timerRx = timerGetByTag(tagRx, TIM_USE_ANY);
const timerHardware_t *timerTx = timerGetByTag(tagTx, TIM_USE_ANY);
IO_t rxIO = IOGetByTag(tagRx);
IO_t txIO = IOGetByTag(tagTx);
if (options & SERIAL_BIDIR) {
// If RX and TX pins are both assigned, we CAN use either with a timer.
// However, for consistency with hardware UARTs, we only use TX pin,
// and this pin must have a timer.
if (!timerTx)
return NULL;
softSerial->timerHardware = timerTx;
softSerial->txIO = txIO;
softSerial->rxIO = txIO;
IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex) + RESOURCE_SOFT_OFFSET);
} else {
if (mode & MODE_RX) {
// Need a pin & a timer on RX
if (!(tagRx && timerRx))
return NULL;
softSerial->rxIO = rxIO;
softSerial->timerHardware = timerRx;
IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(portIndex) + RESOURCE_SOFT_OFFSET);
}
if (mode & MODE_TX) {
// Need a pin on TX
if (!tagTx)
return NULL;
softSerial->txIO = txIO;
if (!(mode & MODE_RX)) {
// TX Simplex, must have a timer
if (!timerTx)
return NULL;
softSerial->timerHardware = timerTx;
} else {
// Duplex
softSerial->exTimerHardware = timerTx;
}
IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex) + RESOURCE_SOFT_OFFSET);
}
}
softSerial->port.vTable = &softSerialVTable;
softSerial->port.baudRate = baud;
softSerial->port.mode = mode;
softSerial->port.options = options;
softSerial->port.rxCallback = rxCallback;
resetBuffers(softSerial);
softSerial->softSerialPortIndex = portIndex;
softSerial->transmissionErrors = 0;
softSerial->receiveErrors = 0;
softSerial->rxActive = false;
softSerial->isTransmittingData = false;
// Configure master timer (on RX); time base and input capture
serialTimerConfigureTimebase(softSerial->timerHardware, baud);
timerChConfigIC(softSerial->timerHardware, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
// Initialize callbacks
timerChCCHandlerInit(&softSerial->edgeCb, onSerialRxPinChange);
timerChOvrHandlerInit(&softSerial->overCb, onSerialTimerOverflow);
// Configure bit clock interrupt & handler.
// If we have an extra timer (on TX), it is initialized and configured
// for overflow interrupt.
// Receiver input capture is configured when input is activated.
if ((mode & MODE_TX) && softSerial->exTimerHardware && softSerial->exTimerHardware->tim != softSerial->timerHardware->tim) {
softSerial->timerMode = TIMER_MODE_DUAL;
serialTimerConfigureTimebase(softSerial->exTimerHardware, baud);
timerChConfigCallbacks(softSerial->exTimerHardware, NULL, &softSerial->overCb);
timerChConfigCallbacks(softSerial->timerHardware, &softSerial->edgeCb, NULL);
} else {
softSerial->timerMode = TIMER_MODE_SINGLE;
timerChConfigCallbacks(softSerial->timerHardware, &softSerial->edgeCb, &softSerial->overCb);
}
#ifdef USE_HAL_DRIVER
softSerial->timerHandle = timerFindTimerHandle(softSerial->timerHardware->tim);
#endif
if (!(options & SERIAL_BIDIR)) {
serialOutputPortActivate(softSerial);
setTxSignal(softSerial, ENABLE);
}
serialInputPortActivate(softSerial);
return &softSerial->port;
}
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
{
uint32_t timerMhzCounter = 0;
pwmWriteFuncPtr pwmWritePtr;
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
bool isDigital = false;
switch (motorConfig->motorPwmProtocol) {
default:
case PWM_TYPE_ONESHOT125:
timerMhzCounter = ONESHOT125_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot125;
break;
case PWM_TYPE_ONESHOT42:
timerMhzCounter = ONESHOT42_TIMER_MHZ;
pwmWritePtr = pwmWriteOneShot42;
break;
case PWM_TYPE_MULTISHOT:
timerMhzCounter = MULTISHOT_TIMER_MHZ;
pwmWritePtr = pwmWriteMultiShot;
break;
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
pwmWritePtr = pwmWriteBrushed;
useUnsyncedPwm = true;
idlePulse = 0;
break;
case PWM_TYPE_STANDARD:
timerMhzCounter = PWM_TIMER_MHZ;
pwmWritePtr = pwmWriteStandard;
useUnsyncedPwm = true;
idlePulse = 0;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT600:
case PWM_TYPE_DSHOT300:
case PWM_TYPE_DSHOT150:
pwmCompleteWritePtr = pwmCompleteDigitalMotorUpdate;
isDigital = true;
break;
#endif
}
if (!useUnsyncedPwm && !isDigital) {
pwmCompleteWritePtr = pwmCompleteOneshotMotorUpdate;
}
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
const ioTag_t tag = motorConfig->ioTags[motorIndex];
if (!tag) {
break;
}
const timerHardware_t *timerHardware = timerGetByTag(tag, TIM_USE_ANY);
if (timerHardware == NULL) {
/* flag failure and disable ability to arm */
break;
}
motors[motorIndex].io = IOGetByTag(tag);
#ifdef USE_DSHOT
if (isDigital) {
pwmDigitalMotorHardwareConfig(timerHardware, motorIndex, motorConfig->motorPwmProtocol);
motors[motorIndex].pwmWritePtr = pwmWriteDigital;
motors[motorIndex].enabled = true;
continue;
}
#endif
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP);
motors[motorIndex].pwmWritePtr = pwmWritePtr;
if (useUnsyncedPwm) {
const uint32_t hz = timerMhzCounter * 1000000;
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, hz / motorConfig->motorPwmRate, idlePulse);
} else {
pwmOutConfig(&motors[motorIndex], timerHardware, timerMhzCounter, 0xFFFF, 0);
}
motors[motorIndex].enabled = true;
}
}
