cómo cambiar el registro OPR_MODE en Adafruit BNO055 usando ATMEGA328p

Estoy intentando crear una IMU para un drone utilizando un Adafruit BNO055. He logrado establecer un protocolo I2C entre el sensor y un ATMEGA328p utilizando el código C en Atmel Studio 7. Pero el modo predeterminado es CONFIG_MODE, lo que significa que el sensor no está midiendo nada. La hoja de datos dice que al escribir en el registro OPR_MODE del sensor, puedo cambiar el modo para que los sensores comiencen a leer datos. Sin embargo, no sé cómo hacer esto. ¿Alguien sabe cómo puedo hacer esto? He adjuntado mi código hasta aquí abajo.

código:

#include <avr/io.h>
#include <util/delay.h>

void Error_I2C()
{
    for (int i = 0; i < 49; i++)
    {
        PORTB = 0x01;
        _delay_ms(1000);
        PORTB = 0x00;
        _delay_ms(1000);
    }
}

void passedStatusCheck()
{
    PORTB = 0x02;
    for (int i = 0; i < 25; i++)
    {
        _delay_ms(1000);
    }
    PORTB = 0x00;
}


void waitForFlagToBeSet()
{
    while (!(TWCR & (1 << TWINT)));
}

void checkStatusRegister(uint8_t status)
{
    if ((TWSR & 0xF8) != status)
    {
        Error_I2C();
    }
    else
    {
        passedStatusCheck();
    }
}

void clearFlagAndEnablewithNACK()
{
    TWCR = (1 << TWINT) | (1 << TWEN); //clear the flag and enable
}

void clearFlagAndEnablewithACK()
{
    TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); //clear the flag and enable
}

void startOrRestart()
{
    TWCR = (1 << TWEN) | (1 << TWINT) | (1 << TWSTA);
}

int main(void)
{
DDRB = 0xFF;
PORTB = 0x00;
//_delay_ms(10000);
//PORTB = 0x02;
//_delay_ms(10000);
//PORTB = 0x00;

//initialize
PRR &= ~(1 << PRTWI);
TWCR &= ~(1 << TWIE); 
TWBR = 2;
TWSR &= ~(1 << TWPS1) | (1 << TWPS0);

OPR_MODE

//start condition
startOrRestart();
waitForFlagToBeSet();
checkStatusRegister(0x08);

//send slave address + write bit(0)
TWDR = 0x50;
clearFlagAndEnablewithNACK();
waitForFlagToBeSet();
checkStatusRegister(0x18);  //check status register for ACK

//write data (register address)
TWDR = 0x08;
clearFlagAndEnablewithNACK();
waitForFlagToBeSet();
checkStatusRegister(0x28);

//repeat start
startOrRestart();
waitForFlagToBeSet();
checkStatusRegister(0x10);  //check for repeat start

//send slave address + read bit(1)
TWDR = 0x51;
clearFlagAndEnablewithNACK();
waitForFlagToBeSet();
checkStatusRegister(0x40);  //check status register for ACK

//read data
uint8_t data = 0x00;
data = TWDR;
clearFlagAndEnablewithACK();
waitForFlagToBeSet();
checkStatusRegister(0x50); //check status register for data received and ACK received

//repeat read data
data = 0x00;
data = TWDR;
clearFlagAndEnablewithNACK();
waitForFlagToBeSet();
checkStatusRegister(0x58); //check status register for data received and NACK received

//stop condition
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);



/* Replace with your application code */
while (1) 
{

}
return 0;
}