mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 12:25:20 +03:00
Add and handle USE_PWM_OUTPUT
This commit is contained in:
parent
de1c1d5377
commit
45b7fa8095
7 changed files with 37 additions and 5 deletions
|
@ -24,6 +24,9 @@
|
|||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
|
@ -682,7 +685,7 @@ FAST_CODE uint16_t prepareDshotPacket(motorDmaOutput_t *const motor)
|
|||
|
||||
return packet;
|
||||
}
|
||||
#endif
|
||||
#endif // USE_DSHOT
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
void pwmWriteServo(uint8_t index, float value)
|
||||
|
@ -723,7 +726,7 @@ void servoDevInit(const servoDevConfig_t *servoConfig)
|
|||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif // USE_SERVOS
|
||||
|
||||
#ifdef USE_BEEPER
|
||||
void pwmWriteBeeper(bool onoffBeep)
|
||||
|
@ -766,4 +769,5 @@ void beeperPwmInit(const ioTag_t tag, uint16_t frequency)
|
|||
beeperPwm.enabled = false;
|
||||
}
|
||||
}
|
||||
#endif // USE_BEEPER
|
||||
#endif
|
||||
|
|
|
@ -41,9 +41,12 @@ void systemBeep(bool onoff)
|
|||
#ifdef USE_BEEPER
|
||||
if (beeperFrequency == 0) {
|
||||
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
|
||||
} else {
|
||||
}
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
else {
|
||||
pwmWriteBeeper(onoff);
|
||||
}
|
||||
#endif
|
||||
#else
|
||||
UNUSED(onoff);
|
||||
#endif
|
||||
|
@ -54,10 +57,13 @@ void systemBeepToggle(void)
|
|||
#ifdef USE_BEEPER
|
||||
if (beeperFrequency == 0) {
|
||||
IOToggle(beeperIO);
|
||||
} else {
|
||||
}
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
else {
|
||||
pwmToggleBeeper();
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
void beeperInit(const beeperDevConfig_t *config)
|
||||
|
@ -72,10 +78,13 @@ void beeperInit(const beeperDevConfig_t *config)
|
|||
IOConfigGPIO(beeperIO, config->isOpenDrain ? IOCFG_OUT_OD : IOCFG_OUT_PP);
|
||||
}
|
||||
systemBeep(false);
|
||||
} else {
|
||||
}
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
else {
|
||||
const ioTag_t beeperTag = config->ioTag;
|
||||
beeperPwmInit(beeperTag, beeperFrequency);
|
||||
}
|
||||
#endif
|
||||
#else
|
||||
UNUSED(config);
|
||||
#endif
|
||||
|
|
|
@ -419,9 +419,11 @@ static void validateAndFixConfig(void)
|
|||
#endif
|
||||
|
||||
#if defined(USE_BEEPER)
|
||||
#ifdef USE_TIMER
|
||||
if (beeperDevConfig()->frequency && !timerGetByTag(beeperDevConfig()->ioTag)) {
|
||||
beeperDevConfigMutable()->frequency = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (beeperConfig()->beeper_off_flags & ~BEEPER_ALLOWED_MODES) {
|
||||
beeperConfigMutable()->beeper_off_flags = 0;
|
||||
|
|
|
@ -391,11 +391,15 @@ void init(void)
|
|||
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {
|
||||
idlePulse = 0; // brushed motors
|
||||
}
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
/* Motors needs to be initialized soon as posible because hardware initialization
|
||||
* may send spurious pulses to esc's causing their early initialization. Also ppm
|
||||
* receiver may share timer with motors so motors MUST be initialized here. */
|
||||
motorDevInit(&motorConfig()->dev, idlePulse, getMotorCount());
|
||||
systemState |= SYSTEM_STATE_MOTORS_READY;
|
||||
#else
|
||||
UNUSED(idlePulse);
|
||||
#endif
|
||||
|
||||
if (0) {}
|
||||
#if defined(USE_PPM)
|
||||
|
@ -776,7 +780,9 @@ void init(void)
|
|||
rcdeviceInit();
|
||||
#endif // USE_RCDEVICE
|
||||
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
pwmEnableMotors();
|
||||
#endif
|
||||
|
||||
setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
|
||||
|
||||
|
|
|
@ -516,6 +516,7 @@ void mixerResetDisarmedMotors(void)
|
|||
|
||||
void writeMotors(void)
|
||||
{
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
if (pwmAreMotorsEnabled()) {
|
||||
#if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
|
||||
if (!pwmStartMotorUpdate(motorCount)) {
|
||||
|
@ -527,6 +528,7 @@ void writeMotors(void)
|
|||
}
|
||||
pwmCompleteMotorUpdate(motorCount);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
static void writeAllMotors(int16_t mc)
|
||||
|
@ -546,8 +548,10 @@ void stopMotors(void)
|
|||
|
||||
void stopPwmAllMotors(void)
|
||||
{
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
pwmShutdownPulsesForAllMotors(motorCount);
|
||||
delayMicroseconds(1500);
|
||||
#endif
|
||||
}
|
||||
|
||||
static FAST_RAM_ZERO_INIT float throttle = 0;
|
||||
|
|
|
@ -935,13 +935,18 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
|
||||
case MSP_MOTOR:
|
||||
for (unsigned i = 0; i < 8; i++) {
|
||||
#ifdef USE_PWM_OUTPUT
|
||||
if (i >= MAX_SUPPORTED_MOTORS || !pwmGetMotors()[i].enabled) {
|
||||
sbufWriteU16(dst, 0);
|
||||
continue;
|
||||
}
|
||||
|
||||
sbufWriteU16(dst, convertMotorToExternal(motor[i]));
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MSP_RC:
|
||||
|
|
|
@ -140,6 +140,8 @@
|
|||
|
||||
#define USE_BRUSHED_ESC_AUTODETECT // Detect if brushed motors are connected and set defaults appropriately to avoid motors spinning on boot
|
||||
|
||||
#define USE_PWM_OUTPUT
|
||||
|
||||
#define USE_CLI
|
||||
#define USE_SERIAL_PASSTHROUGH
|
||||
#define USE_TASK_STATISTICS
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue