Period improperly calculated in Servo library for megaavr microcontrollers (Arduino Nano Every)

[tomalmy]

This was submitted to the wrong area, so is being resubmitted here. Excuse me for lack of familiarity with GitHub.

The ServoHandler function in libraries/Servo/src/megaavr/Servo.cpp improperly calculates the wait time for the refresh period to expire. This certainly came about from the differences between ATmega328P and ATmega4809 Timer/Counters. The number of ticks into the period must be accumulated to calculate the new CCMP value necessary. This should have been done with the tcCounterValue variable, which I have done here. I'm providing my fix for consideration.

void ServoHandler(int timer)
{
	static uint16_t tcCounterValue = 0;
    if (currentServoIndex[timer] < 0) {
        // Write compare register
        _timer->CCMP = 0;
    } else {
        if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
            digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW);   // pulse this channel low if activated
        }
    }

    // Select the next servo controlled by this timer
    currentServoIndex[timer]++;

    if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
        if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {   // check if activated
            digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH);   // it's an active channel so pulse it high
        }

        // Get the counter value
        tcCounterValue += (_timer->CCMP = (uint16_t) (SERVO(timer, currentServoIndex[timer]).ticks));
    }
    else {
        // finished all channels so wait for the refresh period to expire before starting over

        if (tcCounterValue + 4UL < usToTicks(REFRESH_INTERVAL)) {   // allow a few ticks to ensure the next OCR1A not missed
            _timer->CCMP = (uint16_t) usToTicks(REFRESH_INTERVAL) - tcCounterValue;
        }
        else {
            _timer->CCMP = (uint16_t) (4UL);   // at least REFRESH_INTERVAL has elapsed
        }
		tcCounterValue = 0;
        currentServoIndex[timer] = -1;   // this will get incremented at the end of the refresh period to start again at the first channel
    }

    /* Clear flag */
    _timer->INTFLAGS = TCB_CAPT_bm;
}