Проблема с использованием интерфейса AVR TWI

Я пытаюсь получить доступ к модулю HMC5883L, используя atmega2560. Я написал класс (I2C), содержащий основные методы, необходимые для связи I2C.

Во-первых, я объясню проблему. Это то, что я сделал.

int main(){
    I2C i2c;  //an object with basic I2C communication methods

    i2c.init();
    i2c.start();
    i2c.sendSLAW();
    ...
    i2c.write(...);
    ...  //configure registers, CRA, CRB, MR ...
    i2c.stop();
    while (1)
    {
        i2c.start();        
        i2c.sendSLAR();     
            .... //read x,y,z register values
        i2c.stop();     
            .... //say, display x,y,z readings
        _delay_ms(500);
    }
}

(Считайте, что термины имеют свои обычные значения. SLAW = SLA+W (адрес подчиненного устройства + запись)...)

Все идет хорошо, пока не доходит до цикла while. В цикле кажется, что он застрял на i2c.stop()

i2c.stop() реализовано так;

void I2C::I2C_stop(){
    TWCR = (1<<TWINT)|(1<<TWSTO)|(1<<TWEN);
    while (TWCR & (1<<TWSTO));
}

Я сделал что-то не так ? Как я могу это решить?

(Все остальные функции реализованы так же, как в примерах таблицы.)