rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-415 New functions PIOS_Servo_Disable(), PIOS_Servo_Enable() and PIOS_Servo_Config(). Moved timer initialization code to _Enable() function. Removed call to PIOS_TIM_InitChannels().

Browse Source
This commit is contained in:
Vladimir Zidar 2016-09-24 02:28:45 +02:00
parent 33188bd78e
commit 7e6494c8a8
3 changed files with 94 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/pios/inc/pios_servo.h
View File

@ -32,8 +32,9 @@
/* Global types */
enum pios_servo_bank_mode {
PIOS_SERVO_BANK_MODE_PWM = 0,
PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 1
PIOS_SERVO_BANK_MODE_NONE = 0,
PIOS_SERVO_BANK_MODE_PWM = 1,
PIOS_SERVO_BANK_MODE_SINGLE_PULSE = 2
};
/* Public Functions */
extern void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t banks);
@ -42,6 +43,10 @@ extern void PIOS_Servo_Update();
extern void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode);
extern uint8_t PIOS_Servo_GetPinBank(uint8_t pin);
/* ESC Bridge support */
extern void PIOS_Servo_Disable();
extern void PIOS_Servo_Enable();
#endif /* PIOS_SERVO_H */
/**

1
flight/pios/inc/pios_servo_priv.h
View File

@ -45,6 +45,7 @@ struct pios_servo_cfg {
};
extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg);
const struct pios_servo_cfg *PIOS_Servo_Config();
#endif /* PIOS_SERVO_PRIV_H */

132
flight/pios/stm32f4xx/pios_servo.c
View File

@ -50,19 +50,83 @@ static TIM_TypeDef *pios_servo_bank_timer[PIOS_SERVO_BANKS] = { 0 };
// index of bank used for each pin
static uint8_t *pios_servo_pin_bank;
static bool pios_servo_enabled = true;
#define PIOS_SERVO_TIMER_CLOCK 1000000
#define PIOS_SERVO_SAFE_MARGIN 50
extern void PIOS_Servo_Disable()
{
if (!servo_cfg) {
return;
}
pios_servo_enabled = false;
/* NOTE: Following will stop pulses and force low level on output pins.
* this is ok with ESC and servos, but brushed motors could be in trouble
* if using inverted setup */
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
GPIO_InitTypeDef init = chan->pin.init;
init.GPIO_Mode = GPIO_Mode_OUT;
GPIO_Init(chan->pin.gpio, &init);
GPIO_ResetBits(chan->pin.gpio, chan->pin.init.GPIO_Pin);
}
}
extern void PIOS_Servo_Enable()
{
if (!servo_cfg) {
return;
}
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
GPIO_Init(chan->pin.gpio, &chan->pin.init);
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
}
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer && (pios_servo_bank_mode[i] != PIOS_SERVO_BANK_MODE_NONE)) {
TIM_SelectOnePulseMode(timer, TIM_OPMode_Repetitive);
TIM_Cmd(timer, ENABLE);
}
}
pios_servo_enabled = true;
}
/**
* Initialise Servos
*/
int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
{
uint32_t tim_id;
if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) {
return -1;
}
/* Store away the requested configuration */
servo_cfg = cfg;
pios_servo_pin_bank = pios_malloc(sizeof(uint8_t) * cfg->num_channels);
@ -91,27 +155,9 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
bank++;
}
}
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_2:
TIM_OC2Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_3:
TIM_OC3Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
case TIM_Channel_4:
TIM_OC4Init(chan->timer, &servo_cfg->tim_oc_init);
TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
break;
}
}
PIOS_Servo_Enable();
return 0;
}
@ -121,28 +167,11 @@ void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
PIOS_Assert(bank < PIOS_SERVO_BANKS);
pios_servo_bank_mode[bank] = mode;
if (pios_servo_bank_timer[bank]) {
for (uint8_t i = 0; (i < servo_cfg->num_channels); i++) {
if (pios_servo_pin_bank[i] == bank) {
const struct pios_tim_channel *chan = &servo_cfg->channels[i];
/* Set up for output compare function */
switch (chan->timer_chan) {
case TIM_Channel_1:
TIM_OC1PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_2:
TIM_OC2PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_3:
TIM_OC3PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
case TIM_Channel_4:
TIM_OC4PolarityConfig(chan->timer, TIM_OCPolarity_High);
break;
}
}
if (!pios_servo_enabled) {
return;
}
if (pios_servo_bank_timer[bank]) {
// Setup the timer accordingly
TIM_SelectOnePulseMode(pios_servo_bank_timer[bank], TIM_OPMode_Repetitive);
TIM_Cmd(pios_servo_bank_timer[bank], ENABLE);
@ -152,6 +181,10 @@ void PIOS_Servo_SetBankMode(uint8_t bank, uint8_t mode)
void PIOS_Servo_Update()
{
if (!pios_servo_enabled) {
return;
}
for (uint8_t i = 0; (i < PIOS_SERVO_BANKS); i++) {
const TIM_TypeDef *timer = pios_servo_bank_timer[i];
if (timer && pios_servo_bank_mode[i] == PIOS_SERVO_BANK_MODE_SINGLE_PULSE) {
@ -231,7 +264,7 @@ void PIOS_Servo_SetHz(const uint16_t *speeds, const uint32_t *clock, uint8_t ban
void PIOS_Servo_Set(uint8_t servo, uint16_t position)
{
/* Make sure servo exists */
if (!servo_cfg || servo >= servo_cfg->num_channels) {
if (!pios_servo_enabled || !servo_cfg || servo >= servo_cfg->num_channels) {
return;
}
@ -273,4 +306,9 @@ uint8_t PIOS_Servo_GetPinBank(uint8_t pin)
}
}
const struct pios_servo_cfg *PIOS_Servo_Config()
{
return servo_cfg;
}
#endif /* PIOS_INCLUDE_SERVO */