void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint8_t fastPwmProtocolType, uint16_t motorPwmRate, uint16_t idlePulse)
{
uint32_t timerMhzCounter;
switch (fastPwmProtocolType) {
default:
case (PWM_TYPE_ONESHOT125):
timerMhzCounter = ONESHOT125_TIMER_MHZ;
break;
case (PWM_TYPE_ONESHOT42):
timerMhzCounter = ONESHOT42_TIMER_MHZ;
break;
case (PWM_TYPE_MULTISHOT):
timerMhzCounter = MULTISHOT_TIMER_MHZ;
}
if (motorPwmRate > 0) {
uint32_t hz = timerMhzCounter * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse);
} else {
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0);
}
motors[motorIndex]->pwmWritePtr = (fastPwmProtocolType == PWM_TYPE_MULTISHOT) ? pwmWriteMultiShot :
((fastPwmProtocolType == PWM_TYPE_ONESHOT125) ? pwmWriteOneShot125 :
pwmWriteOneShot42);
}
bool pwmInit(drv_pwm_config_t *init)
{
int i = 0;
const uint8_t *setup;
// 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 < MAX_PORTS; i++) {
uint8_t port = setup[i] & 0x0F;
uint8_t mask = setup[i] & 0xF0;
if (setup[i] == 0xFF) // terminator
break;
// skip UART ports for GPS
if (init->useUART && (port == PWM3 || port == PWM4))
continue;
// 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) {
// remap PWM9+10 as servos (but not in airplane mode LOL)
if (port == PWM9 || port == PWM10)
mask = TYPE_S;
}
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) {
motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, PULSE_1MS);
} else if (mask & TYPE_S) {
servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS);
}
}
return false;
}
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 pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, motorPwmProtocolTypes_e proto, bool enableOutput)
{
uint32_t timerMhzCounter;
pwmWriteFuncPtr pwmWritePtr;
switch (proto) {
#ifdef BRUSHED_MOTORS
default:
#endif
case PWM_TYPE_BRUSHED:
timerMhzCounter = PWM_BRUSHED_TIMER_MHZ;
pwmWritePtr = pwmWriteBrushed;
idlePulse = 0;
break;
#ifndef BRUSHED_MOTORS
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_STANDARD:
default:
timerMhzCounter = PWM_TIMER_MHZ;
pwmWritePtr = pwmWriteStandard;
break;
#endif
}
const uint32_t hz = timerMhzCounter * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse, enableOutput);
motors[motorIndex]->pwmWritePtr = pwmWritePtr;
}
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 pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex)
{
motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, 0);
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}
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 pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate)
{
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0);
motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
}
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;
}
}
bool pwmInit(drv_pwm_config_t *init)
{
int i = 0;
const uint8_t *setup;
// 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 < MAX_PORTS; 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
// 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;
// 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) {
// remap PWM9+10 as servos (but not in airplane mode LOL)
if (port == PWM9 || port == PWM10)
mask = TYPE_S;
}
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) {
motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, init->idlePulse > 0 ? init->idlePulse : PULSE_1MS);
} else if (mask & TYPE_S) {
servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS);
}
}
return false;
}
bool pwmInit(drv_pwm_config_t *init)
{
int i = 0;
const uint8_t *setup;
uint16_t period;
// to avoid importing cfg/mcfg
failsafeThreshold = init->failsafeThreshold;
// pwm filtering on input
pwmFilter = init->pwmFilter;
syncPWM = init->syncPWM;
// 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 < MAX_PORTS; i++) {
uint8_t port = setup[i] & 0x0F;
uint8_t mask = setup[i] & 0xF0;
if (setup[i] == 0xFF) // terminator
break;
// 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;
// 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) {
// remap PWM9+10 as servos (but not in airplane mode LOL)
if (port == PWM9 || port == PWM10)
mask = TYPE_S;
}
if (init->extraServos && !init->airplane) {
// remap PWM5..8 as servos when used in extended servo mode.
// condition for airplane because airPPM already has these as servos
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 || init->fastPWM)
mhz = PWM_TIMER_8_MHZ;
else
mhz = PWM_TIMER_MHZ;
hz = mhz * 1000000;
if (init->syncPWM)
period = 8000 * mhz; // 8ms period in syncPWM mode, cycletime should be smaller than this
else if (init->fastPWM)
period = hz / 4000;
else
period = hz / init->motorPwmRate;
motors[numMotors++] = pwmOutConfig(port, mhz, period, 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;
else if (init->syncPWM)
pwmWritePtr = pwmWriteSyncPwm;
// set return values in init struct
init->numServos = numServos;
return false;
}
bool pwmInit(drv_pwm_config_t *init)
{
uint8_t i = 0;
const uint8_t *setup;
// 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 < MAX_PORTS; i++)
{
uint8_t port = setup[i] & 0x0F;
uint8_t mask = setup[i] & 0xF0; // Mask = TYPE_IP, TYPE_IW, TYPE_M, TYPE_S
if (setup[i] == 0xFF) break; // terminator
if (init->useUART && (port == PWM3 || port == PWM4)) // skip UART ports for GPS
continue; // Jump to beginning of for loop again
if (init->adcChannel && (init->adcChannel == port)) // skip ADC for powerMeter if configured
continue;
if (init->useRC5 && port == PWM5) // skip RC5 ports for MONO_LED
continue;
if (init->useRC6 && port == PWM6) // skip RC6 ports for MONO_LED
continue;
if (init->usePWM56 && (port == PWM13 || port == PWM14))// skip PWM5 ports for SONAR they are pwm13/14 in other nomenclature
continue;
if (init->useRC78 && (port == PWM7 || port == PWM8)) // skip RC78 ports for SONAR
continue;
if ((mask & (TYPE_IP | TYPE_IW)) && !init->enablePWMInput)// hacks to allow current functionality
mask = 0;
if (!init->airplane)
{
if (init->useServos && (port == PWM9 || port == PWM10)) mask = TYPE_S; // remap PWM9+10 as servos (but not in airplane mode LOL)
if (init->extraServos && port >= PWM5 && port <= PWM8) mask = TYPE_S; // remap PWM5..8 as servos when used in extended servo mode
}
switch(mask)
{
case TYPE_IP: // PPM input
pwmInConfig(port, ppmCallback, 0);
numInputs = 8; // ONLY 8 here with that auxchannelstuff?
break;
case TYPE_IW: // Single PWM input
pwmInConfig(port, pwmCallback, numInputs);
numInputs++;
break;
case TYPE_M: // Motor
motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, PULSE_1MS);
break;
case TYPE_S: // Servo
servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS);
break;
}
}
return false;
}
