diff --git a/src/renesas/Servo.cpp b/src/renesas/Servo.cpp index eaeb073..e9e940f 100644 --- a/src/renesas/Servo.cpp +++ b/src/renesas/Servo.cpp @@ -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 { @@ -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; @@ -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; @@ -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; } } @@ -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; iperiod_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() @@ -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; @@ -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; } @@ -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; @@ -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; + } } @@ -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); } }