Skip to content

Commit

Permalink
UNO R4: fix PWM timer period updates (#116)
Browse files Browse the repository at this point in the history 
Co-authored-by: Ibrahim Abdelkader <[email protected]>
  • Loading branch information
@KurtE @iabdalkader
KurtE and iabdalkader authored Jan 9, 2024
1 parent f36eb39 commit 962ead5
Showing 1 changed file with 68 additions and 21 deletions.
89 changes: 68 additions & 21 deletions src/renesas/Servo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,9 @@
#define SERVO_MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER)
#define SERVO_INVALID_INDEX (255)
// Lower the timer ticks for finer resolution.
#define SERVO_TIMER_TICK_US (100)
#define SERVO_US_PER_CYCLE (20000)
#define SERVO_IO_PORT_ADDR(pn) &((R_PORT0 + ((uint32_t) (R_PORT1 - R_PORT0) * (pn)))->PCNTR3)
#define SERVO_MIN_CYCLE_OFF_US 50

// Internal Servo struct to keep track of RA configuration.
typedef struct {
Expand All @@ -43,8 +43,8 @@ typedef struct {
// Servo class are not wide enough for the pulse width.
uint32_t period_min;
uint32_t period_max;
// Period period_count in microseconds.
uint32_t period_count;
// Period period_count in timer ticks.
uint32_t period_ticks;
// Internal FSP GPIO port/pin control bits.
volatile uint32_t *io_port;
uint32_t io_mask;
Expand All @@ -58,6 +58,16 @@ static FspTimer servo_timer;
static bool servo_timer_started = false;
void servo_timer_callback(timer_callback_args_t *args);

static uint32_t servo_ticks_per_cycle = 0;
static uint32_t min_servo_cycle_low = 0;
static uint32_t active_servos_mask = 0;
static uint32_t active_servos_mask_refresh = 0;


static uint32_t us_to_ticks(uint32_t time_us) {
return ((float) servo_ticks_per_cycle / (float) SERVO_US_PER_CYCLE) * time_us;
}

static int servo_timer_config(uint32_t period_us)
{
static bool configured = false;
Expand All @@ -68,9 +78,14 @@ static int servo_timer_config(uint32_t period_us)
if (channel != -1) {
servo_timer.begin(TIMER_MODE_PERIODIC, type, channel,
1000000.0f/period_us, 50.0f, servo_timer_callback, nullptr);
servo_timer.setup_overflow_irq();
servo_timer.set_period_buffer(false); // disable period buffering
servo_timer.setup_overflow_irq(10);
servo_timer.open();
servo_timer.stop();
// Read the timer's period count.
servo_ticks_per_cycle = servo_timer.get_period_raw();
min_servo_cycle_low = us_to_ticks(SERVO_MIN_CYCLE_OFF_US);

configured = true;
}
}
Expand Down Expand Up @@ -99,22 +114,47 @@ static int servo_timer_stop()
return 0;
}

inline static void servo_timer_set_period(uint32_t period) {
servo_timer.set_period(period);
}

void servo_timer_callback(timer_callback_args_t *args)
{
for (size_t i=0; i<SERVO_MAX_SERVOS; i++) {
ra_servo_t *servo = &ra_servos[i];
if (servo->period_us) {
servo->period_count += SERVO_TIMER_TICK_US;
if (servo->period_count <= servo->period_us) {
*servo->io_port = (uint32_t) servo->io_mask;
} else {
*servo->io_port = (uint32_t) (servo->io_mask << 16);
}
if (servo->period_count == SERVO_US_PER_CYCLE) {
servo->period_count = 0;
}
(void)args; // remove warning
static uint8_t channel = SERVO_MAX_SERVOS;
static uint8_t channel_pin_set_high = 0xff;
static uint32_t ticks_accum = 0;

// See if we need to set a servo back low
if (channel_pin_set_high != 0xff) {
*ra_servos[channel_pin_set_high].io_port = ra_servos[channel_pin_set_high].io_mask << 16;
}

// Find the next servo to set high
while (active_servos_mask_refresh) {
channel = __builtin_ctz(active_servos_mask_refresh);
if (ra_servos[channel].period_us) {
*ra_servos[channel].io_port = ra_servos[channel].io_mask;
servo_timer_set_period(ra_servos[channel].period_ticks);
channel_pin_set_high = channel;
ticks_accum += ra_servos[channel].period_ticks;
active_servos_mask_refresh &= ~(1 << channel);
return;
}
active_servos_mask_refresh &= ~(1 << channel);
}
// Finished processing all servos, now delay to start of next pass.
ticks_accum += min_servo_cycle_low;
uint32_t time_to_next_cycle;
if (servo_ticks_per_cycle > ticks_accum) {
time_to_next_cycle = servo_ticks_per_cycle - ticks_accum;
} else {
time_to_next_cycle = min_servo_cycle_low;
}
ticks_accum = 0;
servo_timer_set_period(time_to_next_cycle);
channel_pin_set_high = 0xff;
active_servos_mask_refresh = active_servos_mask;
}

Servo::Servo()
Expand All @@ -139,6 +179,11 @@ uint8_t Servo::attach(int pin, int min, int max)
return 0;
}

// Configure the servo timer.
if (servo_timer_config(SERVO_US_PER_CYCLE) != 0) {
return 0;
}

// Try to find a free servo slot.
ra_servo_t *servo = NULL;
bsp_io_port_pin_t io_pin = g_pin_cfg[pin].pin;
Expand All @@ -151,6 +196,7 @@ uint8_t Servo::attach(int pin, int min, int max)
servo->period_max = max;
servo->io_mask = (1U << (io_pin & 0xFF));
servo->io_port = SERVO_IO_PORT_ADDR(((io_pin >> 8U) & 0xFF));
active_servos_mask |= (1 << i); // update mask of servos that are active.
writeMicroseconds(DEFAULT_PULSE_WIDTH);
break;
}
Expand All @@ -164,9 +210,8 @@ uint8_t Servo::attach(int pin, int min, int max)
R_IOPORT_PinCfg(&g_ioport_ctrl, io_pin,
IOPORT_CFG_PORT_DIRECTION_OUTPUT | IOPORT_CFG_PORT_OUTPUT_HIGH);

// Configure and start the timer if it's not started.
if (servo_timer_config(SERVO_TIMER_TICK_US) != 0 ||
servo_timer_start() != 0) {
// Start the timer if it's not started.
if (servo_timer_start() != 0) {
return 0;
}
return 1;
Expand All @@ -178,10 +223,12 @@ void Servo::detach()
ra_servo_t *servo = &ra_servos[servoIndex];
servo_timer_stop();
servo->period_us = 0;
active_servos_mask &= ~(1 << servoIndex); // update mask of servos that are active.
servoIndex = SERVO_INVALID_INDEX;
if (--n_servos) {
servo_timer_start();
}
servoIndex = SERVO_INVALID_INDEX;

}
}

Expand All @@ -207,8 +254,8 @@ void Servo::writeMicroseconds(int us)
{
if (servoIndex != SERVO_INVALID_INDEX) {
ra_servo_t *servo = &ra_servos[servoIndex];
servo->period_count = 0;
servo->period_us = constrain(us, servo->period_min, servo->period_max);
servo->period_ticks = us_to_ticks(servo->period_us);
}
}

Expand Down

0 comments on commit 962ead5

Please sign in to comment.