Skip to content

Unor4 timer period updates #116

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

Merged
merged 7 commits into from
Jan 9, 2024
Merged

Unor4 timer period updates #116

Changes from 5 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
93 changes: 71 additions & 22 deletions src/renesas/Servo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#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 MIN_CYCLE_OFF_US 50

// Internal Servo sturct to keep track of RA configuration.
typedef struct {
Expand All @@ -43,8 +44,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,19 +59,37 @@ 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 usToticks(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;
if (configured == false) {
// Configure and enable the servo timer.
// Configure and enable the servo timer, for full 20ms
uint8_t type = 0;
int8_t channel = FspTimer::get_available_timer(type);
if (channel != -1) {
// lets initially configure the servo to 50ms
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();

// Now lets see what the period;
servo_ticks_per_cycle = servo_timer.get_period_raw();
min_servo_cycle_low = usToticks(MIN_CYCLE_OFF_US);

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

inline static void updateClockPeriod(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 = (uint32_t)(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 = (uint32_t)ra_servos[channel].io_mask;
updateClockPeriod(ra_servos[channel].period_ticks);
channel_pin_set_high = channel;
ticks_accum += ra_servos[channel_pin_set_high].period_ticks;
active_servos_mask_refresh &= ~(1 << channel);
return;
}
active_servos_mask_refresh &= ~(1 << channel);
}

// Got to hear we finished processing all servos, now delay to start of next pass.
ticks_accum += min_servo_cycle_low;
uint32_t time_to_next_cycle = (servo_ticks_per_cycle > ticks_accum)? servo_ticks_per_cycle - ticks_accum : min_servo_cycle_low;
ticks_accum = 0;
updateClockPeriod(time_to_next_cycle);
channel_pin_set_high = 0xff;
active_servos_mask_refresh = active_servos_mask;
}

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

// Configure and the 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 +197,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 +211,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 +224,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 +255,9 @@ 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_count = 0;
servo->period_us = constrain(us, (int)servo->period_min, (int)servo->period_max);
servo->period_ticks = usToticks(servo->period_us);
}
}

Expand Down