mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-21 15:25:36 +03:00
Fix after rebase
This commit is contained in:
parent
b6263d0109
commit
33b67e860d
5 changed files with 146 additions and 184 deletions
|
@ -23,34 +23,20 @@
|
||||||
|
|
||||||
#include "io.h"
|
#include "io.h"
|
||||||
#include "timer.h"
|
#include "timer.h"
|
||||||
#include "pwm_mapping.h"
|
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
|
|
||||||
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
|
#define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5)
|
||||||
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
|
#define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f)
|
||||||
|
|
||||||
|
static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS];
|
||||||
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
volatile timCCR_t *ccr;
|
|
||||||
TIM_TypeDef *tim;
|
|
||||||
uint16_t period;
|
|
||||||
pwmWriteFuncPtr pwmWritePtr;
|
|
||||||
} pwmOutputPort_t;
|
|
||||||
|
|
||||||
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
|
|
||||||
|
|
||||||
static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
|
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
|
static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS];
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static uint8_t allocatedOutputPortCount = 0;
|
|
||||||
|
|
||||||
static bool pwmMotorsEnabled = true;
|
static bool pwmMotorsEnabled = true;
|
||||||
static void pwmOCConfig(TIM_TypeDef *tim, uint32_t channel, uint16_t value)
|
|
||||||
|
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output)
|
||||||
{
|
{
|
||||||
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
|
TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
|
||||||
if(Handle == NULL) return;
|
if(Handle == NULL) return;
|
||||||
|
@ -69,19 +55,14 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint32_t channel, uint16_t value)
|
||||||
//HAL_TIM_PWM_Start(Handle, channel);
|
//HAL_TIM_PWM_Start(Handle, channel);
|
||||||
}
|
}
|
||||||
|
|
||||||
static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
|
static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value)
|
||||||
{
|
{
|
||||||
pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++];
|
|
||||||
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
|
TIM_HandleTypeDef* Handle = timerFindTimerHandle(timerHardware->tim);
|
||||||
if(Handle == NULL) return p;
|
if(Handle == NULL) return;
|
||||||
|
|
||||||
configTimeBase(timerHardware->tim, period, mhz);
|
configTimeBase(timerHardware->tim, period, mhz);
|
||||||
|
pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output);
|
||||||
|
|
||||||
const IO_t io = IOGetByTag(timerHardware->tag);
|
|
||||||
IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount);
|
|
||||||
IOConfigGPIOAF(io, IOCFG_AF_PP, timerHardware->alternateFunction);
|
|
||||||
|
|
||||||
pwmOCConfig(timerHardware->tim, timerHardware->channel, value);
|
|
||||||
if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
|
if (timerHardware->output & TIMER_OUTPUT_ENABLED) {
|
||||||
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
|
HAL_TIM_PWM_Start(Handle, timerHardware->channel);
|
||||||
} else {
|
} else {
|
||||||
|
@ -91,55 +72,53 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
|
||||||
|
|
||||||
switch (timerHardware->channel) {
|
switch (timerHardware->channel) {
|
||||||
case TIM_CHANNEL_1:
|
case TIM_CHANNEL_1:
|
||||||
p->ccr = &timerHardware->tim->CCR1;
|
port->ccr = &timerHardware->tim->CCR1;
|
||||||
break;
|
break;
|
||||||
case TIM_CHANNEL_2:
|
case TIM_CHANNEL_2:
|
||||||
p->ccr = &timerHardware->tim->CCR2;
|
port->ccr = &timerHardware->tim->CCR2;
|
||||||
break;
|
break;
|
||||||
case TIM_CHANNEL_3:
|
case TIM_CHANNEL_3:
|
||||||
p->ccr = &timerHardware->tim->CCR3;
|
port->ccr = &timerHardware->tim->CCR3;
|
||||||
break;
|
break;
|
||||||
case TIM_CHANNEL_4:
|
case TIM_CHANNEL_4:
|
||||||
p->ccr = &timerHardware->tim->CCR4;
|
port->ccr = &timerHardware->tim->CCR4;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
p->period = period;
|
port->period = period;
|
||||||
p->tim = timerHardware->tim;
|
port->tim = timerHardware->tim;
|
||||||
|
|
||||||
*p->ccr = 0;
|
*port->ccr = 0;
|
||||||
|
|
||||||
return p;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
static void pwmWriteBrushed(uint8_t index, uint16_t value)
|
static void pwmWriteBrushed(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
*motors[index]->ccr = (value - 1000) * motors[index]->period / 1000;
|
*motors[index].ccr = (value - 1000) * motors[index].period / 1000;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void pwmWriteStandard(uint8_t index, uint16_t value)
|
static void pwmWriteStandard(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
*motors[index]->ccr = value;
|
*motors[index].ccr = value;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void pwmWriteOneShot125(uint8_t index, uint16_t value)
|
static void pwmWriteOneShot125(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
*motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
|
*motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f));
|
||||||
}
|
}
|
||||||
|
|
||||||
static void pwmWriteOneShot42(uint8_t index, uint16_t value)
|
static void pwmWriteOneShot42(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
*motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
|
*motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f));
|
||||||
}
|
}
|
||||||
|
|
||||||
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
|
static void pwmWriteMultiShot(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
*motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
|
*motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW);
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmWriteMotor(uint8_t index, uint16_t value)
|
void pwmWriteMotor(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) {
|
if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) {
|
||||||
motors[index]->pwmWritePtr(index, value);
|
motors[index].pwmWritePtr(index, value);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -147,7 +126,7 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount)
|
||||||
{
|
{
|
||||||
for (int index = 0; index < motorCount; index++) {
|
for (int index = 0; index < motorCount; index++) {
|
||||||
// Set the compare register to 0, which stops the output pulsing if the timer overflows
|
// Set the compare register to 0, which stops the output pulsing if the timer overflows
|
||||||
*motors[index]->ccr = 0;
|
*motors[index].ccr = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -167,40 +146,27 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
|
||||||
bool overflowed = false;
|
bool overflowed = false;
|
||||||
// If we have not already overflowed this timer
|
// If we have not already overflowed this timer
|
||||||
for (int j = 0; j < index; j++) {
|
for (int j = 0; j < index; j++) {
|
||||||
if (motors[j]->tim == motors[index]->tim) {
|
if (motors[j].tim == motors[index].tim) {
|
||||||
overflowed = true;
|
overflowed = true;
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
if (!overflowed) {
|
if (!overflowed) {
|
||||||
timerForceOverflow(motors[index]->tim);
|
timerForceOverflow(motors[index].tim);
|
||||||
}
|
}
|
||||||
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
|
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
|
||||||
// This compare register will be set to the output value on the next main loop.
|
// This compare register will be set to the output value on the next main loop.
|
||||||
*motors[index]->ccr = 0;
|
*motors[index].ccr = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate)
|
void motorInit(const motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount)
|
||||||
{
|
|
||||||
const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
|
|
||||||
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0);
|
|
||||||
motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
|
|
||||||
}
|
|
||||||
|
|
||||||
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
|
|
||||||
{
|
|
||||||
const uint32_t hz = PWM_TIMER_MHZ * 1000000;
|
|
||||||
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse);
|
|
||||||
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
|
|
||||||
}
|
|
||||||
|
|
||||||
void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType)
|
|
||||||
{
|
{
|
||||||
uint32_t timerMhzCounter;
|
uint32_t timerMhzCounter;
|
||||||
pwmWriteFuncPtr pwmWritePtr;
|
pwmWriteFuncPtr pwmWritePtr;
|
||||||
|
bool useUnsyncedPwm = motorConfig->useUnsyncedPwm;
|
||||||
|
|
||||||
switch (fastPwmProtocolType) {
|
switch (motorConfig->motorPwmProtocol) {
|
||||||
default:
|
default:
|
||||||
case (PWM_TYPE_ONESHOT125):
|
case (PWM_TYPE_ONESHOT125):
|
||||||
timerMhzCounter = ONESHOT125_TIMER_MHZ;
|
timerMhzCounter = ONESHOT125_TIMER_MHZ;
|
||||||
|
@ -214,27 +180,89 @@ void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn
|
||||||
timerMhzCounter = MULTISHOT_TIMER_MHZ;
|
timerMhzCounter = MULTISHOT_TIMER_MHZ;
|
||||||
pwmWritePtr = pwmWriteMultiShot;
|
pwmWritePtr = pwmWriteMultiShot;
|
||||||
break;
|
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (motorPwmRate > 0) {
|
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||||
const uint32_t hz = timerMhzCounter * 1000000;
|
const ioTag_t tag = motorConfig->ioTags[motorIndex];
|
||||||
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse);
|
|
||||||
} else {
|
if (!tag) {
|
||||||
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
|
break;
|
||||||
}
|
}
|
||||||
motors[motorIndex]->pwmWritePtr = pwmWritePtr;
|
|
||||||
|
motors[motorIndex].io = IOGetByTag(tag);
|
||||||
|
|
||||||
|
IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_OUTPUT, RESOURCE_INDEX(motorIndex));
|
||||||
|
|
||||||
|
const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
|
||||||
|
|
||||||
|
IOConfigGPIOAF(motors[motorIndex].io, IOCFG_AF_PP, timer->alternateFunction);
|
||||||
|
|
||||||
|
if (timer == NULL) {
|
||||||
|
/* flag failure and disable ability to arm */
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
motors[motorIndex].pwmWritePtr = pwmWritePtr;
|
||||||
|
if (useUnsyncedPwm) {
|
||||||
|
const uint32_t hz = timerMhzCounter * 1000000;
|
||||||
|
pwmOutConfig(&motors[motorIndex], timer, timerMhzCounter, hz / motorConfig->motorPwmProtocol, idlePulse);
|
||||||
|
} else {
|
||||||
|
pwmOutConfig(&motors[motorIndex], timer, timerMhzCounter, 0xFFFF, 0);
|
||||||
|
}
|
||||||
|
motors[motorIndex].enabled = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
pwmOutputPort_t *pwmGetMotors()
|
||||||
|
{
|
||||||
|
return motors;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
void pwmWriteServo(uint8_t index, uint16_t value)
|
void pwmWriteServo(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
if (servos[index] && index < MAX_SERVOS) {
|
if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) {
|
||||||
*servos[index]->ccr = value;
|
*servos[index].ccr = value;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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_OUTPUT, RESOURCE_INDEX(servoIndex));
|
||||||
|
|
||||||
|
const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED);
|
||||||
|
|
||||||
|
IOConfigGPIOAF(servos[servoIndex].io, IOCFG_AF_PP, timer->alternateFunction);
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -221,7 +221,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
||||||
s->port.rxBufferHead = s->port.rxBufferTail = 0;
|
s->port.rxBufferHead = s->port.rxBufferTail = 0;
|
||||||
s->port.txBufferHead = s->port.txBufferTail = 0;
|
s->port.txBufferHead = s->port.txBufferTail = 0;
|
||||||
// callback works for IRQ-based RX ONLY
|
// callback works for IRQ-based RX ONLY
|
||||||
s->port.callback = callback;
|
s->port.rxCallback = callback;
|
||||||
s->port.mode = mode;
|
s->port.mode = mode;
|
||||||
s->port.baudRate = baudRate;
|
s->port.baudRate = baudRate;
|
||||||
s->port.options = options;
|
s->port.options = options;
|
||||||
|
|
|
@ -303,8 +303,8 @@ void uartIrqHandler(uartPort_t *s)
|
||||||
{
|
{
|
||||||
uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff);
|
uint8_t rbyte = (uint8_t)(huart->Instance->RDR & (uint8_t)0xff);
|
||||||
|
|
||||||
if (s->port.callback) {
|
if (s->port.rxCallback) {
|
||||||
s->port.callback(rbyte);
|
s->port.rxCallback(rbyte);
|
||||||
} else {
|
} else {
|
||||||
s->port.rxBuffer[s->port.rxBufferHead] = rbyte;
|
s->port.rxBuffer[s->port.rxBufferHead] = rbyte;
|
||||||
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
|
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
|
||||||
|
|
|
@ -867,3 +867,18 @@ void timerForceOverflow(TIM_TypeDef *tim)
|
||||||
tim->EGR |= TIM_EGR_UG;
|
tim->EGR |= TIM_EGR_UG;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag)
|
||||||
|
{
|
||||||
|
for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
|
||||||
|
if (timerHardware[i].tag == tag) {
|
||||||
|
if (flag && (timerHardware[i].output & flag) == flag) {
|
||||||
|
return &timerHardware[i];
|
||||||
|
} else if (!flag && timerHardware[i].output == flag) {
|
||||||
|
// TODO: shift flag by one so not to be 0
|
||||||
|
return &timerHardware[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
|
@ -19,108 +19,27 @@
|
||||||
|
|
||||||
#include <platform.h>
|
#include <platform.h>
|
||||||
#include "drivers/io.h"
|
#include "drivers/io.h"
|
||||||
#include "drivers/pwm_mapping.h"
|
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
|
|
||||||
const uint16_t multiPPM[] = {
|
|
||||||
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
|
||||||
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM3 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM4 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM5 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM6 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
0xFFFF
|
|
||||||
};
|
|
||||||
|
|
||||||
const uint16_t multiPWM[] = {
|
|
||||||
PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1
|
|
||||||
PWM2 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM3 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
|
||||||
PWM16 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM13 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM14 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM15 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
0xFFFF
|
|
||||||
};
|
|
||||||
|
|
||||||
const uint16_t airPPM[] = {
|
|
||||||
PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
|
||||||
PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
|
||||||
PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
|
||||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
|
||||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4
|
|
||||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM13 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM14 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM15 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8),
|
|
||||||
0xFFFF
|
|
||||||
};
|
|
||||||
|
|
||||||
const uint16_t airPWM[] = {
|
|
||||||
PWM1 | (MAP_TO_PWM_INPUT << 8), // input #1
|
|
||||||
PWM2 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM3 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
|
||||||
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
|
||||||
PWM16 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
|
||||||
PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
|
||||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
|
||||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4
|
|
||||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM13 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM14 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
PWM15 | (MAP_TO_SERVO_OUTPUT << 8),
|
|
||||||
0xFFFF
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
{ TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF9_TIM12 }, // S1_IN
|
{ TIM12, IO_TAG(PB14), TIM_CHANNEL_1, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF9_TIM12 }, // S1_IN
|
||||||
{ TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_IPD, GPIO_AF9_TIM12 }, // S2_IN
|
{ TIM12, IO_TAG(PB15), TIM_CHANNEL_2, TIM8_BRK_TIM12_IRQn, 0, IOCFG_AF_PP, GPIO_AF9_TIM12 }, // S2_IN
|
||||||
{ TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF3_TIM8 }, // S3_IN
|
{ TIM8, IO_TAG(PC6), TIM_CHANNEL_1, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF3_TIM8 }, // S3_IN
|
||||||
{ TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF3_TIM8 }, // S4_IN
|
{ TIM8, IO_TAG(PC7), TIM_CHANNEL_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF3_TIM8 }, // S4_IN
|
||||||
{ TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF3_TIM8 }, // S5_IN
|
{ TIM8, IO_TAG(PC9), TIM_CHANNEL_4, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF3_TIM8 }, // S5_IN
|
||||||
{ TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_IPD, GPIO_AF3_TIM8 }, // S6_IN
|
{ TIM8, IO_TAG(PC8), TIM_CHANNEL_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF3_TIM8 }, // S6_IN
|
||||||
|
|
||||||
{ TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2 }, // S1_OUT
|
{ TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF2_TIM4 }, // S10_OUT 1
|
||||||
{ TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5 }, // S2_OUT
|
{ TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF1_TIM2 }, // S6_OUT 2
|
||||||
{ TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF3_TIM9 }, // S3_OUT
|
{ TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF2_TIM4 }, // S5_OUT 3
|
||||||
{ TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3 }, // S4_OUT
|
{ TIM2, IO_TAG(PA3), TIM_CHANNEL_4, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF1_TIM2 }, // S1_OUT 4
|
||||||
{ TIM4, IO_TAG(PB9), TIM_CHANNEL_4, TIM4_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM4 }, // S5_OUT
|
{ TIM5, IO_TAG(PA1), TIM_CHANNEL_2, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF2_TIM5 }, // S2_OUT
|
||||||
{ TIM2, IO_TAG(PA2), TIM_CHANNEL_3, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2 }, // S6_OUT
|
{ TIM9, IO_TAG(PE6), TIM_CHANNEL_2, TIM1_BRK_TIM9_IRQn, 1, IOCFG_AF_PP, GPIO_AF3_TIM9 }, // S3_OUT
|
||||||
{ TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM5 }, // S7_OUT
|
{ TIM3, IO_TAG(PB5), TIM_CHANNEL_2, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF2_TIM3 }, // S4_OUT
|
||||||
{ TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF1_TIM2 }, // S8_OUT
|
{ TIM5, IO_TAG(PA0), TIM_CHANNEL_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF2_TIM5 }, // S7_OUT
|
||||||
{ TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM3 }, // S9_OUT
|
{ TIM2, IO_TAG(PB3), TIM_CHANNEL_2, TIM2_IRQn, 1, IOCFG_AF_PP, GPIO_AF1_TIM2 }, // S8_OUT
|
||||||
{ TIM4, IO_TAG(PB8), TIM_CHANNEL_3, TIM4_IRQn, 1, IOCFG_AF_PP_PD, GPIO_AF2_TIM4 }, // S10_OUT
|
{ TIM3, IO_TAG(PB4), TIM_CHANNEL_1, TIM3_IRQn, 1, IOCFG_AF_PP, GPIO_AF2_TIM3 }, // S9_OUT
|
||||||
};
|
};
|
||||||
|
|
||||||
// ALTERNATE LAYOUT
|
// ALTERNATE LAYOUT
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue