BNO055 problem: I2C bus is busy

BNO055_RETURN_FUNCTION_TYPE BNO055_I2C_bus_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *reg_data, uint8_t cnt)
{
    int comres = 0;  // Result of communication

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    
    // Start transmission
    i2c_master_start(cmd);
    
    // Send device address and write bit
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
    
    // Send the register address
    i2c_master_write_byte(cmd, reg_addr, ACK_CHECK_EN);
    
    // Stop transmission
    i2c_master_stop(cmd);
    
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C Write failed");
        return -1;  // Communication failure
    }

    // Small delay to ensure data is ready
    esp_rom_delay_us(150);

    // Create new command for reading data
    cmd = i2c_cmd_link_create();
    
    // Start transmission
    i2c_master_start(cmd);
    
    // Send device address and read bit
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_READ, ACK_CHECK_EN);
    
    // Read the data
    if (cnt > 1) {
        i2c_master_read(cmd, reg_data, cnt - 1, ACK_VAL);
    }
    i2c_master_read_byte(cmd, reg_data + cnt - 1, NACK_VAL);
    
    // Stop transmission
    i2c_master_stop(cmd);
    
    ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C Read failed");
        return -1;
    }

    return comres;
}

BNO055_RETURN_FUNCTION_TYPE BNO055_I2C_bus_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *reg_data, uint8_t cnt)
{
    BNO055_RETURN_FUNCTION_TYPE comres = 0;  // Result of communication

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    
    // Start transmission
    i2c_master_start(cmd);
    
    // Send device address and write bit
    i2c_master_write_byte(cmd, (dev_addr << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
    
    // Send the register address
    i2c_master_write_byte(cmd, reg_addr, ACK_CHECK_EN);
    
    // Write the data
    for (uint8_t index = 0; index < cnt; index++) {
        i2c_master_write_byte(cmd, reg_data[index], ACK_CHECK_EN);
    }
    
    // Stop transmission
    i2c_master_stop(cmd);
    
    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C Write failed");
        return -1;  // Communication failure
    }

    // Small delay to ensure data is written
    esp_rom_delay_us(150);

    return comres;
}

void bno055_task(void *pvParameter) {
    struct bno055_t bno055 = {
        .bus_read = BNO055_I2C_bus_read,
        .bus_write = BNO055_I2C_bus_write,
        .dev_addr = 0x29,
        .delay_msec = _delay
    };
    BNO055_RETURN_FUNCTION_TYPE com_rslt;
    struct bno055_accel_t accel_data;
    bool sensor_ready = false;

    // Initialize BNO055 sensor
    com_rslt = bno055_init(&bno055);
    if (com_rslt == BNO055_SUCCESS) {
        // Set operation mode
        com_rslt = bno055_set_operation_mode(BNO055_OPERATION_MODE_NDOF);
        if (com_rslt == BNO055_SUCCESS) {
            sensor_ready = true;
        } else {
            ESP_LOGE(TAG, "BNO055 set operation mode failed with code: %d", com_rslt);
        }
    } else {
        ESP_LOGE(TAG, "BNO055 initialization failed with code: %d", com_rslt);
    }

    while (1) {
        if (sensor_ready) {
            // Read Euler angles with error handling
            com_rslt = bno055_read_accel_xyz(&accel_data);
            if (com_rslt == BNO055_SUCCESS) 
            {
                ESP_LOGI(TAG, "Accel X: %d, Y: %d, Z: %d", accel_data.x, accel_data.y, accel_data.z);
            } 
            else 
            {
                ESP_LOGE(TAG, "BNO055 read accel failed with code: %d", com_rslt);
            }
        }

        vTaskDelay(100); // Adjust delay as needed
    }
}