Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Unor4 timer period updates #116

Merged
merged 7 commits into from
Jan 9, 2024
Merged

Unor4 timer period updates #116

Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
4969711
UNOR4 - update timer period
KurtE Sep 1, 2023
604d84f
Initial support if FspTimer is AGT timer
KurtE Sep 2, 2023
02c5ee9
remove debug, extra lines spaces =
KurtE Sep 9, 2023
04b9322
Move timer specific stuff into core.
KurtE Sep 10, 2023
fc280bb
Update to match method name change in name in core
KurtE Sep 12, 2023
0091d6d
Apply suggestions from code review
KurtE Jan 4, 2024
27dd79b
Fix errors due to changes done in Code Review
KurtE Jan 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
89 changes: 68 additions & 21 deletions src/renesas/Servo.cpp
View file
Open in desktop
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 sturct 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