Leyendo MPU-6050 con MSP430g2553 usando I2C

He estado buscando bibliotecas o documentaciones para ayudarme a leer y escribir en el quad ACU + GYRO MPU-6050 de InvenSense. Logré leerlo con las bibliotecas de arduino hechas por Jeff Rowberg ( Link ), pero ahora quiero usar la TI MSP430g2553 (con la CCS o IAR). Tengo la intención de hacer una buena biblioteca y algunos ejemplos para compartirla, ya que no puedo encontrarla tan fácilmente como encontré para Arduino y PIC.

El código que tengo hasta ahora parece funcionar, pero no puedo estar seguro. La prueba más básica es leer el registro WHO_AM_I, que devuelve la dirección del esclavo (0x68). El código que usé está basado en este tema , que es basado en una documentación de TI.

Mi código es muy simple:

Incluye :

#include <msp430g2553.h>
#include "MPU6050.h"

Prototipos:

void init_I2C(void);
int i2c_notready(void);
char Receive(char);
void Transmit(char, char);

Main :

int main(void) {

         volatile char who_am_i;
         char test;

         WDTCTL = WDTPW + WDTHOLD;                 // Stop WDT
         BCSCTL1 = CALBC1_1MHZ;                    // Set DCO to 1Mhz
         DCOCTL = CALDCO_1MHZ;
         P1SEL |= BIT6 + BIT7;       // Assign I2C pins to USCI_B0
         P1SEL2 |= BIT6 + BIT7;       // Assign I2C pins to USCI_B0

         init_I2C();    // initialize i2c
         __delay_cycles(10000);

         while ( i2c_notready() );       // wait for bus to be free
         __delay_cycles(10000);

         who_am_i = Receive(MPU6050_RA_WHO_AM_I);

         while(1) {}
}

Funciones :

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 = MPU6050_DEFAULT_ADDRESS;
          UCB0CTL1 &= ~UCSWRST;                     // **Initialize USCI state machine**
          IE2 |= UCB0RXIE + UCB0TXIE;    // Enable RX and TX interrupt
}


int i2c_notready(){
        if(UCB0STAT & UCBBUSY) return 1;
        else return 0;
}


char Receive(char registerAddr){
        char 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;
}


void Transmit(char registerAddr, char data){
    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
    UCB0TXBUF = data;                       //Write data in register
    while((IFG2 & UCB0TXIFG) == 0);         // wait until TX buffer is empty and transmitted
    UCB0CTL1 |= UCTXSTP;                    // I2C stop condition
    IFG2 &= ~UCB0TXIFG;                     // Clear TX interrupt flag
}

El problema es: Solo puedo obtener los datos si trato de leer dos veces. Es decir, solo puedo obtener el valor de registro si escribo la línea

         who_am_i = Receive(MPU6050_RA_WHO_AM_I);

dos veces. (Y obtengo el valor esperado).

Puede ser una tontería, pero creo que esto está sucediendo porque entendí mal algo que podría llevar a un mal funcionamiento total del código completo. Por lo tanto, me gustaría resolver o entender este problema antes de continuar con mi Código. Muchas gracias.