1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +03:00

implement (not flight tested) brushed motors support.

set motor_pwm_rate to some value > 500 (1000, 8000, 16000 etc works on my scope).
then the motor output can be used to directly drive brushed motor fets.
PWM is rescaled to 0-base in brushed mode, so all same values of min/maxthrottle apply.
This commit is contained in:
dongie 2014-01-31 11:56:55 +09:00
parent a695ddd66a
commit e8b3d00003
5 changed files with 40 additions and 13 deletions

View file

@ -120,7 +120,7 @@ const clivalue_t valueTable[] = {
{ "deadband3d_high", VAR_UINT16, &mcfg.deadband3d_high, 0, 2000 }, { "deadband3d_high", VAR_UINT16, &mcfg.deadband3d_high, 0, 2000 },
{ "neutral3d", VAR_UINT16, &mcfg.neutral3d, 0, 2000 }, { "neutral3d", VAR_UINT16, &mcfg.neutral3d, 0, 2000 },
{ "deadband3d_throttle", VAR_UINT16, &mcfg.deadband3d_throttle, 0, 2000 }, { "deadband3d_throttle", VAR_UINT16, &mcfg.deadband3d_throttle, 0, 2000 },
{ "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 498 }, { "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 32000 },
{ "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 }, { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 },
{ "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 }, { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 },
{ "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 }, { "flaps_speed", VAR_UINT8, &mcfg.flaps_speed, 0, 100 },

View file

@ -1,7 +1,5 @@
#include "board.h" #include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width
/* /*
Configuration maps: Configuration maps:
@ -53,10 +51,13 @@ enum {
TYPE_S = 0x80 TYPE_S = 0x80
}; };
typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
static pwmPortData_t pwmPorts[MAX_PORTS]; static pwmPortData_t pwmPorts[MAX_PORTS];
static uint16_t captures[MAX_INPUTS]; static uint16_t captures[MAX_INPUTS];
static pwmPortData_t *motors[MAX_MOTORS]; static pwmPortData_t *motors[MAX_MOTORS];
static pwmPortData_t *servos[MAX_SERVOS]; static pwmPortData_t *servos[MAX_SERVOS];
static pwmWriteFuncPtr pwmWritePtr = NULL;
static uint8_t numMotors = 0; static uint8_t numMotors = 0;
static uint8_t numServos = 0; static uint8_t numServos = 0;
static uint8_t numInputs = 0; static uint8_t numInputs = 0;
@ -138,6 +139,7 @@ static const uint8_t * const hardwareMaps[] = {
}; };
#define PWM_TIMER_MHZ 1 #define PWM_TIMER_MHZ 1
#define PWM_BRUSHED_TIMER_MHZ 8
static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value) static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value)
{ {
@ -195,10 +197,10 @@ static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
gpioInit(gpio, &cfg); gpioInit(gpio, &cfg);
} }
static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value) static pwmPortData_t *pwmOutConfig(uint8_t port, uint8_t mhz, uint16_t period, uint16_t value)
{ {
pwmPortData_t *p = &pwmPorts[port]; pwmPortData_t *p = &pwmPorts[port];
configTimeBase(timerHardware[port].tim, period, PWM_TIMER_MHZ); configTimeBase(timerHardware[port].tim, period, mhz);
pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP); pwmGPIOConfig(timerHardware[port].gpio, timerHardware[port].pin, Mode_AF_PP);
pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value); pwmOCConfig(timerHardware[port].tim, timerHardware[port].channel, value);
// Needed only on TIM1 // Needed only on TIM1
@ -220,6 +222,7 @@ static pwmPortData_t *pwmOutConfig(uint8_t port, uint16_t period, uint16_t value
p->ccr = &timerHardware[port].tim->CCR4; p->ccr = &timerHardware[port].tim->CCR4;
break; break;
} }
p->period = period;
return p; return p;
} }
@ -290,6 +293,16 @@ static void pwmCallback(uint8_t port, uint16_t capture)
} }
} }
static void pwmWriteBrushed(uint8_t index, uint16_t value)
{
*motors[index]->ccr = (value - 1000) * motors[index]->period / 1000;
}
static void pwmWriteStandard(uint8_t index, uint16_t value)
{
*motors[index]->ccr = value;
}
bool pwmInit(drv_pwm_config_t *init) bool pwmInit(drv_pwm_config_t *init)
{ {
int i = 0; int i = 0;
@ -354,19 +367,30 @@ bool pwmInit(drv_pwm_config_t *init)
pwmInConfig(port, pwmCallback, numInputs); pwmInConfig(port, pwmCallback, numInputs);
numInputs++; numInputs++;
} else if (mask & TYPE_M) { } else if (mask & TYPE_M) {
motors[numMotors++] = pwmOutConfig(port, 1000000 / init->motorPwmRate, init->idlePulse > 0 ? init->idlePulse : PULSE_1MS); uint32_t hz, mhz;
if (init->motorPwmRate > 500)
mhz = PWM_BRUSHED_TIMER_MHZ;
else
mhz = PWM_TIMER_MHZ;
hz = mhz * 1000000;
motors[numMotors++] = pwmOutConfig(port, mhz, hz / init->motorPwmRate, init->idlePulse);
} else if (mask & TYPE_S) { } else if (mask & TYPE_S) {
servos[numServos++] = pwmOutConfig(port, 1000000 / init->servoPwmRate, PULSE_1MS); servos[numServos++] = pwmOutConfig(port, PWM_TIMER_MHZ, 1000000 / init->servoPwmRate, PULSE_1MS);
} }
} }
// determine motor writer function
pwmWritePtr = pwmWriteStandard;
if (init->motorPwmRate > 500)
pwmWritePtr = pwmWriteBrushed;
return false; return false;
} }
void pwmWriteMotor(uint8_t index, uint16_t value) void pwmWriteMotor(uint8_t index, uint16_t value)
{ {
if (index < numMotors) if (index < numMotors)
*motors[index]->ccr = value; pwmWritePtr(index, value);
} }
void pwmWriteServo(uint8_t index, uint16_t value) void pwmWriteServo(uint8_t index, uint16_t value)

View file

@ -3,6 +3,7 @@
#define MAX_MOTORS 12 #define MAX_MOTORS 12
#define MAX_SERVOS 8 #define MAX_SERVOS 8
#define MAX_INPUTS 8 #define MAX_INPUTS 8
#define PULSE_1MS (1000) // 1ms pulse width
typedef struct drv_pwm_config_t { typedef struct drv_pwm_config_t {
bool enableInput; bool enableInput;
@ -15,8 +16,8 @@ typedef struct drv_pwm_config_t {
uint8_t adcChannel; // steal one RC input for current sensor uint8_t adcChannel; // steal one RC input for current sensor
uint16_t motorPwmRate; uint16_t motorPwmRate;
uint16_t servoPwmRate; uint16_t servoPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
// default of zero means PULSE_1MS, otherwise set to given value. Used by 3D mode. // some higher value (used by 3d mode), or 0, for brushed pwm drivers.
uint16_t failsafeThreshold; uint16_t failsafeThreshold;
} drv_pwm_config_t; } drv_pwm_config_t;

View file

@ -1,7 +1,5 @@
#include "board.h" #include "board.h"
#define PULSE_1MS (1000) // 1ms pulse width
/* FreeFlight/Naze32 timer layout /* FreeFlight/Naze32 timer layout
TIM2_CH1 RC1 PWM1 TIM2_CH1 RC1 PWM1
TIM2_CH2 RC2 PWM2 TIM2_CH2 RC2 PWM2

View file

@ -68,7 +68,11 @@ int main(void)
pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = mcfg.motor_pwm_rate; pwm_params.motorPwmRate = mcfg.motor_pwm_rate;
pwm_params.servoPwmRate = mcfg.servo_pwm_rate; pwm_params.servoPwmRate = mcfg.servo_pwm_rate;
pwm_params.idlePulse = feature(FEATURE_3D) ? mcfg.neutral3d : 0; pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = mcfg.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold; pwm_params.failsafeThreshold = cfg.failsafe_detect_threshold;
switch (mcfg.power_adc_channel) { switch (mcfg.power_adc_channel) {
case 1: case 1: