AVR Butterfly - Timer reuse after internal oscillator calibration

Question

I am calibrating the AVR Butterfly internal oscillator for being able to use USART, based on the sample code provided by AVR (see code below). As I also wanted to use two timer-controlled servo motors, I am wondering whether it is possible to reuse 16-bit timer 1 after the calibration process - I tried resetting the TCCR1A/B but it did not work out (code also below). I hope you can help me out with this.

void OSCCAL_Calibrate(void){
    unsigned char calibrate = 0;
    int temp;
    unsigned char tempL;

    CLKPR = (1<<CLKPCE);        // set Clock Prescaler Change Enable
    // set prescaler = 8, Inter RC 8Mhz / 8 = 1Mhz
    CLKPR = (1<<CLKPS1) | (1<<CLKPS0);
    TIMSK2 = 0;             //disable OCIE2A and TOIE2
    ASSR = (1<<AS2);        //select asynchronous operation of timer2 (32,768kHz)    
    OCR2A = 200;            // set timer2 compare value 
    TIMSK0 = 0;             // delete any interrupt sources
    TCCR1B = (1<<CS10);     // start timer1 with no prescaling
    TCCR2A = (1<<CS20);     // start timer2 with no prescaling
    while((ASSR & 0x01) | (ASSR & 0x04));       //wait for TCN2UB and TCR2UB to be cleared

    delayMs(1000);    // wait for external crystal to stabilise

    while(!calibrate)
    {
        cli();  // disable global interrupt

        TIFR1 = 0xFF;   // delete TIFR1 flags
        TIFR2 = 0xFF;   // delete TIFR2 flags
        TCNT1H = 0;     // clear timer1 counter
        TCNT1L = 0;
        TCNT2 = 0;      // clear timer2 counter

        while ( !(TIFR2 & (1<<OCF2A)) );   // wait for timer2 compareflag

        TCCR1B = 0; // stop timer1

        sei();  // enable global interrupt

        if ( (TIFR1 & (1<<TOV1)) )
        {
            temp = 0xFFFF;      // if timer1 overflows, set the temp to 0xFFFF
        }else
        {   // read out the timer1 counter value
            tempL = TCNT1L;
            temp = TCNT1H;
            temp = (temp << 8);
            temp += tempL;
        }

        if (temp > 6250)
            OSCCAL--;   // the internRC oscillator runs to fast, decrease the OSCCAL
        else if (temp < 6120)
            OSCCAL++;   // the internRC oscillator runs to slow, increase the OSCCAL
        else
            calibrate = 1;   // the interRC is correct

        TCCR1B = (1<<CS10); // start timer1
    }
} 

void motorInit(){
    // reset timer 1
    TCCR1A = 0;
    TCCR1B = 0;

    // initialize Servo Pins
    DDRB |= (1<<PB5) | (1<<PB6);

    ICR1H = ICR_VALUE >> 8;
    ICR1L = ICR_VALUE & (TOP_VALUE);

    // reset OCRs
    setServoSpeed(0, 0);
    setServoSpeed(1, 0);

    // Set Timer mode (PWM Phase & Freq. correct, clear on compare match)
    // and prescaler (8)
    TCCR1A = ((1<<COM1A1) | (1<<COM1B1));                 
    TCCR1B = ((1<<WGM13) | (0<<CS12) | (1<<CS11) | (0<<CS10));
}

Answer 1

Maybe you can check code which I used for a project while ago, but you should take care that I decreased the system frequency to 7.3768 MHz for 56700 baudrate, which maybe you need to adjust.

void OSCCAL_Calibrate(void)
{
uint8_t LoopCount = (0x7F / 2); 

ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
{
// Make sure all clock division is turned off (8MHz RC clock)
CLKPR = (1 << CLKPCE);
CLKPR = 0x00;

// Inital OSCCAL of half its maximum
OSCCAL = (0x7F / 2);

// Disable timer interrupts
TIMSK1 = 0;
TIMSK2 = 0;

// Set timer 2 to asyncronous mode (32.768KHz crystal)
ASSR = (1 << AS2);

// Ensure timer 1 control register A is cleared
TCCR1A = 0;

// Start both counters with no prescaling
TCCR1B = (1 << CS10);
TCCR2A = (1 << CS20);

// Wait until timer 2's external 32.768KHz crystal is stable
while (ASSR & ((1 << TCN2UB) | (1 << TCR2UB) | (1 << OCR2UB)));

// Clear the timer values
TCNT1 = 0;
TCNT2 = 0;

while (LoopCount--)
{
// Wait until timer 2 overflows
while (!(TIFR2 & (1 << TOV2)));

// Stop timer 1 so it can be read
TCCR1B = 0x00;

// Check timer value against ideal constant
if (TCNT1 > OSCCAL_TARGETCOUNT) // Clock is running too fast
OSCCAL--;
else if (TCNT1 < OSCCAL_TARGETCOUNT) // Clock is running too slow
OSCCAL++;

// Clear timer 2 overflow flag
TIFR2 |= (1 << TOV2);

Check this out!