Biblioteca de comunicación Msp430g2553 i2c con giroscopio itg3200

He estado creando una biblioteca I2C para comunicarme con el giroscopio ITG3200 para la plataforma de lanzamiento experimental MSP430g2553. Todavía tengo pequeños problemas con la lectura secuencial y los valores firmados, pero estoy casi completo. ¿Puedes ayudarme con los últimos problemas con los que estoy lidiando? Parece que estoy atascado en un punto. Aquí está la biblioteca http://e2e.ti.com/cfs-file.ashx/__key/communityserver-discussions-components-files/166/8272.i2c_5F00_lib.rar

guía de usuario familiar: http://www.ti.com/lit/ug/slau144i/slau144i.pdf

Guía del usuario del giroscopio: https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf

Si no desea descargar esos 3 archivos fuente, aquí está la explicación de las funciones requeridas:

Principal:

WDTCTL = WDTPW + WDTHOLD; // Stop WDT
BCSCTL1 = CALBC1_1MHZ; // Set DCO to 1Mhz
DCOCTL = CALDCO_1MHZ;
P1SEL |= BIT1 + BIT2 + BIT6 + BIT7; // Assign I2C pins to USCI_B0 // Assign Uart pins to USCI_A0
P1SEL2 |= BIT1 + BIT2 + BIT6 + BIT7; // Assign I2C pins to USCI_B0 // Assign Uart pins to USCI_A0
init_I2C(); // initialize i2c
initUart(); // initialize uart

..

..

Inicio I2C:

void init_I2C(void) {
    UCB0CTL1 |= UCSWRST; // Enable SW reset
    UCB0CTL0 = UCMST + UCMODE_3 + UCSYNC; // I2C Master, synchronous mode
    UCB0CTL1 = UCSSEL_2 + UCSWRST; // Use SMCLK, keep SW reset
    UCB0BR0 = 10; // fSCL = 1Mhz/10 = ~100kHz
    UCB0BR1 = 0;
    UCB0I2CSA = itgAddress; // Slave Address is 069h
    UCB0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
    IE2 |= UCB0RXIE + UCB0TXIE; // Enable RX and TX interrupt
}

Recibir función:

uint8_t Receive(char registerAddr){
    uint8_t receivedByte;
    while (UCB0CTL1 & UCTXSTP); // Ensure stop condition got sent
    UCB0CTL1 |= UCTR + UCTXSTT; // I2C start condition with UCTR flag for transmit
    while((IFG2 & UCB0TXIFG) == 0); //UCB0TXIFG is set immidiately
    UCB0TXBUF = registerAddr; //write registerAddr in TX buffer
    while((IFG2 & UCB0TXIFG) == 0); // wait until TX buffer is empty and transmitted
    UCB0CTL1 &= ~UCTR ; // Clear I2C TX flag for receive
    UCB0CTL1 |= UCTXSTT + UCTXNACK; // I2C start condition with NACK for single byte reading
    while (UCB0CTL1 & UCTXSTT); // Start condition sent? RXBuffer full?
    receivedByte = UCB0RXBUF;
    UCB0CTL1 |= UCTXSTP; // I2C stop condition
    return receivedByte;
}

Inicialización de Uart:

void initUart(void) {
    UCA0CTL1 |= UCSSEL_2; // Use SMCLK
    UCA0BR0 = 104; // 1MHz 9600
    UCA0BR1 = 0; // 1MHz 9600
    UCA0MCTL = UCBRS0; // Modulation UCBRSx = 1
    UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
}

letra entera:

void serialPrintInteger(uint16_t num) {
    int i;
    uint16_t num_send[5];
    uint16_t numTemp;
    num_send[0] = num/10000; // extract 5th digit
    numTemp = num % 10000; // get remaining 4
    num_send[1] = numTemp/1000; // extract 4th digit
    numTemp = numTemp % 1000; // get remamining 3
    num_send[2] = numTemp/100; // extract 3th digit
    numTemp = numTemp % 100; // get remaining 2
    num_send[3] = numTemp/10; // extract 2th digit
    num_send[4] = numTemp % 10; // extract 1th digit

    if(num_send[0] > 0) { // if num is 5 digit
        for(i = 0 ; i <= 4 ; i++)
            serialWrite(num_send[i]); // send each digit as one byte
    }
    else if(num_send[1] > 0) { // if num is 4 digit
        for(i = 1 ; i <= 4 ; i++)
            serialWrite(num_send[i]);
    }
    else if(num_send[2] > 0) { // if num is 3 digit
        for(i = 2 ; i <= 4 ; i++)
            serialWrite(num_send[i]);
    }
    else if(num_send[3] > 0) { // if num is 2 digit
        for(i = 3 ; i <= 4 ; i++)
            serialWrite(num_send[i]);
    }
    else { // if num is 1 digit
        serialWrite(num_send[4]);
    }
}

void serialPrintAscii(uint8_t ascii) {
    UCA0TXBUF = ascii;
    while (!(IFG2&UCA0TXIFG)); // USCI_A0 TX buffer ready?
}

Y aquí está mi problema. Cuando pruebo estas lecturas en mi bucle principal por separado, obtengo los resultados que debería tener.

serialPrintInteger(Receive(0x00));

o

serialPrintInteger(Receive(0x15));

o

serialPrintInteger(Receive(0x16));

y las salidas de estos son 0x69 de Recibir (0x00) que lee el registro de dirección del esclavo del giroscopio, 9 de Recibir (0x15) donde escribí 9 dentro del registro 0x15 para la configuración y 25 de Recibir (0x16) donde también escribí.

No creo que mis funciones de serialPrint también estén dañadas, lo he intentado con muchas combinaciones en el rango de 16 bits donde debería estar. Esta secuencia está funcionando bien:

serialPrintInteger(5);
serialPrintAscii(Ascii_Comma);
serialPrintInteger(10);
serialPrintAscii(Ascii_And);
serialPrintInteger(15);
serialPrintAscii(Ascii_Dot);

Veo una salida como esta en mi consola serial: 5,10&15.5,10&15.5...

La parte divertida comienza cuando pruebo esta lógica en mi función Recibir. Aquí está la secuencia que uso

serialPrintInteger(Receive(0x00)); // result 105
serialPrintAscii(Ascii_Comma);
serialPrintInteger(Receive(0x15)); // result 9
serialPrintAscii(Ascii_And);
serialPrintInteger(Receive(0x00)); // result 105
serialPrintAscii(Ascii_Dot);

La secuencia en mi consola es así: 105,105&9.105,105&9.105...

Primero pensé que no estaba enviando correctamente NACK al esclavo por un solo byte y pensé que seguía incrementando la dirección de registro por su cuenta, pero funcionan bien por separado y el giroscopio también tiene registros x, y, z y no están corrompiendo mi secuencia.

Estuve luchando con los registros de movimiento del giroscopio por un tiempo, pero me di cuenta de que todavía no tengo el control total de mi I2C. Entonces, ¿puedes señalar lo que estoy haciendo mal aquí?