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:
parent
a695ddd66a
commit
e8b3d00003
5 changed files with 40 additions and 13 deletions
|
@ -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 },
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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:
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue