Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
PiOS Servo: remove old soft prescalar
Previous we got smart about setting the divisor
for very slow PWM rates. This had the same complexities
of inferring it for fast rates. Now we have the code to
set it explicitly, it is easy for someone to create a
new mode that uses 100khz as the time base, or whatever
is appropriate for their task.
  • Loading branch information
peabody124 committed May 18, 2015
1 parent 236e4eb commit a40a7cf
Show file tree
Hide file tree
Showing 3 changed files with 86 additions and 162 deletions.
88 changes: 32 additions & 56 deletions flight/PiOS/STM32F10x/pios_servo.c
Expand Up @@ -38,13 +38,11 @@
/* Private Function Prototypes */
static const struct pios_servo_cfg * servo_cfg;

//! This is a soft divisor for ultra low frequency counters. Rarely used.
static uint8_t *output_timer_frequency_scaler;
//! The counter rate for the channel, used to calculate compare values.
static uint32_t *output_channel_clk_rate; // The clock rate for that timer
#if defined(PIOS_INCLUDE_HPWM)
//! The update rate for that channel. 0 if using synchronous updates.
static uint16_t *output_channel_frequency;
//! The counter rate for the channel, used to calculate compare values.
static uint32_t *output_channel_clk_rate; // The clock rate for that timer
#endif

/**
Expand Down Expand Up @@ -89,27 +87,18 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg)
TIM_Cmd(chan->timer, ENABLE);
}

/* Allocate memory */
output_timer_frequency_scaler = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_timer_frequency_scaler)));
// Check that memory was successfully allocated, and return if not
if (output_timer_frequency_scaler == NULL) {
output_channel_clk_rate = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
if (output_channel_clk_rate == NULL) {
return -1;
}
memset(output_timer_frequency_scaler, 0, servo_cfg->num_channels * sizeof(typeof(output_timer_frequency_scaler)));

memset(output_channel_clk_rate, 0, servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
#if defined(PIOS_INCLUDE_HPWM)
/* Allocate memory for frequency table */
output_channel_frequency = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_channel_frequency)));
if (output_channel_frequency == NULL) {
return -1;
}
memset(output_channel_frequency, 0, servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));

output_channel_clk_rate = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
if (output_channel_clk_rate == NULL) {
return -1;
}
memset(output_channel_clk_rate, 0, servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
#endif

return 0;
Expand Down Expand Up @@ -150,49 +139,29 @@ void PIOS_Servo_SetMode(const uint16_t * speeds, const enum pwm_mode *pwm_mode,

if (new) {

uint32_t output_timer_frequency = 0;
uint32_t clk_rate = 0;

// Based on PWM mode determine the desired output period (which sets the
// channel resolution)
if (pwm_mode[set] == PWM_MODE_1US) {
output_timer_frequency = 1000000; // Default output timer frequency in hertz
clk_rate = 1000000; // Default output timer frequency in hertz
} else if (pwm_mode[set] == PWM_MODE_12US) {
output_timer_frequency = 12000000; // Default output timer frequency in hertz
clk_rate = 12000000; // Default output timer frequency in hertz
}

if (speeds[set] == 0) {

// OneShot (i.e. synchronous updates). Do not want the timer periodic. This field
// is used as a flag in the update method for whether to rephase the timer. Set to
// 0 initially.
output_timer_frequency_scaler[set] = 0;

// Use a maximally long period because we don't want pulses actually repeating
// without new data arriving.
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;

} else {

// To allow very slow PWM rates a secondary software scaling is potentially calculated
// this path is not normally doing anything. (e.g. even for 12 MHz output -- HPWM, with
// 400 Hz output the period is 30000 which does not overflow).
output_timer_frequency_scaler[set] = 0; // Scaling applied to frequency in order to bring the period into unsigned 16-bit integer range

/* While the output frequency is so high that the period register overflows, reduce frequency by half */
while ((output_timer_frequency >> output_timer_frequency_scaler[i]) / speeds[set] - 1 > UINT16_MAX) {
output_timer_frequency_scaler[set]++;

// If the output frequency is so low that the prescaler register overflows, break
if (MAX(PIOS_PERIPHERAL_APB1_CLOCK, PIOS_PERIPHERAL_APB2_CLOCK) / (output_timer_frequency >> output_timer_frequency_scaler[set]) - 1 > UINT16_MAX) {
output_timer_frequency_scaler[set]--;
break;
}
}

TIM_TimeBaseStructure.TIM_Period = (((output_timer_frequency >> output_timer_frequency_scaler[i]) / speeds[set]) - 1);
// Note: this can be extended with a new PWM mode that is lower resolution
// for very long periods
TIM_TimeBaseStructure.TIM_Period = (clk_rate / speeds[set]) - 1;
}



/* Choose the correct prescaler value for the APB the timer is attached */
// "The timer clock frequencies are automatically fixed by hardware. There are two cases:
// 1. if the APB prescaler is 1, the timer clock frequencies are set to the same frequency as
Expand All @@ -204,27 +173,27 @@ void PIOS_Servo_SetMode(const uint16_t * speeds, const enum pwm_mode *pwm_mode,
return;
} else if (chan->timer==TIM1 || chan->timer==TIM8) {
if (PIOS_PERIPHERAL_APB2_CLOCK == PIOS_SYSCLK)
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / (output_timer_frequency >> output_timer_frequency_scaler[i])) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / clk_rate) - 1;
else
TIM_TimeBaseStructure.TIM_Prescaler = ((PIOS_PERIPHERAL_APB2_CLOCK*2) / (output_timer_frequency >> output_timer_frequency_scaler[i])) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = ((PIOS_PERIPHERAL_APB2_CLOCK*2) / clk_rate) - 1;
} else {
if (PIOS_PERIPHERAL_APB1_CLOCK == PIOS_SYSCLK)
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / (output_timer_frequency >> output_timer_frequency_scaler[i])) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / clk_rate) - 1;
else
TIM_TimeBaseStructure.TIM_Prescaler = ((PIOS_PERIPHERAL_APB1_CLOCK*2) / (output_timer_frequency >> output_timer_frequency_scaler[i])) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = ((PIOS_PERIPHERAL_APB1_CLOCK*2) / clk_rate) - 1;
}


// Configure this timer appropriately.
TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);

/* Configure frequency scaler for all channels that use the same timer */
for (uint8_t j=0; (j < servo_cfg->num_channels); j++) {
if (chan->timer == servo_cfg->channels[j].timer) {
output_timer_frequency_scaler[j] = output_timer_frequency_scaler[i];
#if defined(PIOS_INCLUDE_HPWM)
/* save the frequency for these channels */
output_channel_frequency[j] = speeds[set];
output_channel_clk_rate[j] = output_timer_frequency >> output_timer_frequency_scaler[set];
output_channel_clk_rate[j] = clk_rate;
#endif
}
}
Expand Down Expand Up @@ -288,20 +257,25 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
return;
}

/* Update the position. Right shift for channels that have non-standard prescalers */
const struct pios_tim_channel * chan = &servo_cfg->channels[servo];

/* recalculate the position value based on timer clock rate */
/* position is in us */
/* clk_rate is in count per second */

/* Update the position */
switch(chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare1(chan->timer, position);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare2(chan->timer, position);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare3(chan->timer, position);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare4(chan->timer, position);
break;
}
}
Expand All @@ -321,12 +295,14 @@ void PIOS_Servo_Update()
const struct pios_tim_channel * chan = &servo_cfg->channels[i];

/* Check for channels that are using synchronous output */
if (output_channel_frequency[i] == 0) {
/* Look for a disabled timer using synchronous output */
if (!(chan->timer->CR1 & TIM_CR1_CEN) &&
(output_channel_frequency[i] == 0)) {
/* enable it again and reinitialize it */
TIM_Cmd(chan->timer, ENABLE);
TIM_GenerateEvent(chan->timer, TIM_EventSource_Update);
}
}
}

#endif /* PIOS_INCLUDE_HPWM */
#endif /* PIOS_INCLUDE_HPWM */
80 changes: 27 additions & 53 deletions flight/PiOS/STM32F30x/pios_servo.c
Expand Up @@ -38,13 +38,11 @@
/* Private Function Prototypes */
static const struct pios_servo_cfg * servo_cfg;

//! This is a soft divisor for ultra low frequency counters. Rarely used.
static uint8_t *output_timer_frequency_scaler;
//! The counter rate for the channel, used to calculate compare values.
static uint32_t *output_channel_clk_rate; // The clock rate for that timer
#if defined(PIOS_INCLUDE_HPWM)
//! The update rate for that channel. 0 if using synchronous updates.
static uint16_t *output_channel_frequency;
//! The counter rate for the channel, used to calculate compare values.
static uint32_t *output_channel_clk_rate; // The clock rate for that timer
#endif

/**
Expand Down Expand Up @@ -89,27 +87,18 @@ int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg)
TIM_Cmd(chan->timer, ENABLE);
}

/* Allocate memory */
output_timer_frequency_scaler = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_timer_frequency_scaler)));
// Check that memory was successfully allocated, and return if not
if (output_timer_frequency_scaler == NULL) {
output_channel_clk_rate = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
if (output_channel_clk_rate == NULL) {
return -1;
}
memset(output_timer_frequency_scaler, 0, servo_cfg->num_channels * sizeof(typeof(output_timer_frequency_scaler)));

memset(output_channel_clk_rate, 0, servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
#if defined(PIOS_INCLUDE_HPWM)
/* Allocate memory for frequency table */
output_channel_frequency = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_channel_frequency)));
if (output_channel_frequency == NULL) {
return -1;
}
memset(output_channel_frequency, 0, servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));

output_channel_clk_rate = PIOS_malloc(servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
if (output_channel_clk_rate == NULL) {
return -1;
}
memset(output_channel_clk_rate, 0, servo_cfg->num_channels * sizeof(typeof(output_channel_clk_rate)));
#endif

return 0;
Expand Down Expand Up @@ -150,46 +139,25 @@ void PIOS_Servo_SetMode(const uint16_t * speeds, const enum pwm_mode *pwm_mode,

if (new) {

uint32_t output_timer_frequency = 0;
uint32_t clk_rate = 0;

// Based on PWM mode determine the desired output period (which sets the
// channel resolution)
if (pwm_mode[set] == PWM_MODE_1US) {
output_timer_frequency = 1000000; // Default output timer frequency in hertz
clk_rate = 1000000; // Default output timer frequency in hertz
} else if (pwm_mode[set] == PWM_MODE_12US) {
output_timer_frequency = 12000000; // Default output timer frequency in hertz
clk_rate = 12000000; // Default output timer frequency in hertz
}

if (speeds[set] == 0) {

// OneShot (i.e. synchronous updates). Do not want the timer periodic. This field
// is used as a flag in the update method for whether to rephase the timer. Set to
// 0 initially.
output_timer_frequency_scaler[set] = 0;

// Use a maximally long period because we don't want pulses actually repeating
// without new data arriving.
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;

} else {

// To allow very slow PWM rates a secondary software scaling is potentially calculated
// this path is not normally doing anything. (e.g. even for 12 MHz output -- HPWM, with
// 400 Hz output the period is 30000 which does not overflow).
output_timer_frequency_scaler[set] = 0; // Scaling applied to frequency in order to bring the period into unsigned 16-bit integer range

/* While the output frequency is so high that the period register overflows, reduce frequency by half */
while ((output_timer_frequency >> output_timer_frequency_scaler[i]) / speeds[set] - 1 > UINT16_MAX) {
output_timer_frequency_scaler[set]++;

// If the output frequency is so low that the prescaler register overflows, break
if (MAX(PIOS_PERIPHERAL_APB1_CLOCK, PIOS_PERIPHERAL_APB2_CLOCK) / (output_timer_frequency >> output_timer_frequency_scaler[set]) - 1 > UINT16_MAX) {
output_timer_frequency_scaler[set]--;
break;
}
}

TIM_TimeBaseStructure.TIM_Period = (((output_timer_frequency >> output_timer_frequency_scaler[i]) / speeds[set]) - 1);
// Note: this can be extended with a new PWM mode that is lower resolution
// for very long periods
TIM_TimeBaseStructure.TIM_Period = (clk_rate / speeds[set]) - 1;
}

/* Choose the correct prescaler value for the APB to which the timer is attached */
Expand All @@ -198,9 +166,9 @@ void PIOS_Servo_SetMode(const uint16_t * speeds, const enum pwm_mode *pwm_mode,
return;
} else if (chan->timer==TIM2 || chan->timer==TIM3 || chan->timer==TIM4) {
//those timers run at double APB1 speed if APB1 prescaler is != 1 which is usually the case
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / (output_timer_frequency >> output_timer_frequency_scaler[i]) * 2) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB1_CLOCK / clk_rate * 2) - 1;
} else {
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / (output_timer_frequency >> output_timer_frequency_scaler[i])) - 1;
TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_PERIPHERAL_APB2_CLOCK / clk_rate) - 1;
}

// Configure this timer appropriately.
Expand All @@ -209,11 +177,10 @@ void PIOS_Servo_SetMode(const uint16_t * speeds, const enum pwm_mode *pwm_mode,
/* Configure frequency scaler for all channels that use the same timer */
for (uint8_t j=0; (j < servo_cfg->num_channels); j++) {
if (chan->timer == servo_cfg->channels[j].timer) {
output_timer_frequency_scaler[j] = output_timer_frequency_scaler[i];
#if defined(PIOS_INCLUDE_HPWM)
/* save the frequency for these channels */
output_channel_frequency[j] = speeds[set];
output_channel_clk_rate[j] = output_timer_frequency >> output_timer_frequency_scaler[set];
output_channel_clk_rate[j] = clk_rate;
#endif
}
}
Expand Down Expand Up @@ -277,20 +244,25 @@ void PIOS_Servo_Set(uint8_t servo, uint16_t position)
return;
}

/* Update the position. Right shift for channels that have non-standard prescalers */
const struct pios_tim_channel * chan = &servo_cfg->channels[servo];

/* recalculate the position value based on timer clock rate */
/* position is in us */
/* clk_rate is in count per second */

/* Update the position */
switch(chan->timer_chan) {
case TIM_Channel_1:
TIM_SetCompare1(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare1(chan->timer, position);
break;
case TIM_Channel_2:
TIM_SetCompare2(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare2(chan->timer, position);
break;
case TIM_Channel_3:
TIM_SetCompare3(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare3(chan->timer, position);
break;
case TIM_Channel_4:
TIM_SetCompare4(chan->timer, position >> output_timer_frequency_scaler[servo]);
TIM_SetCompare4(chan->timer, position);
break;
}
}
Expand All @@ -310,7 +282,9 @@ void PIOS_Servo_Update()
const struct pios_tim_channel * chan = &servo_cfg->channels[i];

/* Check for channels that are using synchronous output */
if (output_channel_frequency[i] == 0) {
/* Look for a disabled timer using synchronous output */
if (!(chan->timer->CR1 & TIM_CR1_CEN) &&
(output_channel_frequency[i] == 0)) {
/* enable it again and reinitialize it */
TIM_Cmd(chan->timer, ENABLE);
TIM_GenerateEvent(chan->timer, TIM_EventSource_Update);
Expand Down

0 comments on commit a40a7cf

Please sign in to comment.