[SOLVED: removed level converter]OV7670 - modified Uno code for Mega, no success

**EDIT: [SOLVED]**The problem turned out to be the schematic that I used, the I2C bus was not working so nothing was being written to the camera registers. I read that there are on-board I2C pull-ups to 5V on Mega that's why in the schematic it uses the level converter for I2C but I didn't really understand the configuration tbh.
Anyways, I just used 10K pull-up resistors to 3.3V and it's all working fine now.

Hi, I'm having trouble with operating OV7670 camera(without FIFO) on Arduino Mega2560, I initially had it working on Arduino Uno but due to needing extra I/Os for other components I made the jump to Mega2560. I had to slightly modify the code I used for Uno so it can work on Mega too, I have managed to at least receive images(attached) but it's still not really working as it's just giving me rubbish.

I haven't put the whole code here and skipped the initial parts that I'm quite sure works on both Uno and Mega.

The only changes I have made in the code are related to the new location of the camera pins since I moved them when I decided to use Mega and replaced the original PWM code with another one that works on Mega. Link to the original code is at the bottom of this post. Is it the TWI functions not working on Mega thus failing to write to camera registers ?

These are the wiring:

21 -(level shift)-> SIOC
20 <-(level shift)-> SIOD
46 -(level shift)-> XCLK
30 <- D7
31 <- D6
32 <- D5
33 <- D4
34 <- D3
35 <- D2
36 <- D1
37 <- D0
26 <- VSYNC
28 <- PCLK
GND -> PWDN
3.3v -> RESET
GND -> GND
3.3v -> 3V3

Code:

struct regval_list{
  uint8_t reg_num;
  uint16_t value;
};


/*void error_led(void){
  DDRB |= 32;//make sure led is output
  while (1){//wait for reset
    PORTB ^= 32;// toggle led
    _delay_ms(100);
  }
}*/

void twiStart(void){
  TWCR = _BV(TWINT) | _BV(TWSTA) | _BV(TWEN);//send start
  while (!(TWCR & (1 << TWINT)));//wait for start to be transmitted
 /*if ((TWSR & 0xF8) != TW_START)
    error_led();*/
}

void twiWriteByte(uint8_t DATA, uint8_t type){
  TWDR = DATA;
  TWCR = _BV(TWINT) | _BV(TWEN);
  while (!(TWCR & (1 << TWINT))) {}
  /*if ((TWSR & 0xF8) != type)
    error_led();*/
}

void twiAddr(uint8_t addr, uint8_t typeTWI){
  TWDR = addr;//send address
  TWCR = _BV(TWINT) | _BV(TWEN);    /* clear interrupt to start transmission */
  while ((TWCR & _BV(TWINT)) == 0); /* wait for transmission */
  /*if ((TWSR & 0xF8) != typeTWI)
    error_led();*/
}

void wrReg(uint8_t reg, uint8_t dat){
  //send start condition
  twiStart();
  twiAddr(camAddr_WR, TW_MT_SLA_ACK);
  twiWriteByte(reg, TW_MT_DATA_ACK);
  twiWriteByte(dat, TW_MT_DATA_ACK);
  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);//send stop
  _delay_ms(1);
}

static uint8_t twiRd(uint8_t nack){
  if (nack){
    TWCR = _BV(TWINT) | _BV(TWEN);
    while ((TWCR & _BV(TWINT)) == 0); /* wait for transmission */
    /*if ((TWSR & 0xF8) != TW_MR_DATA_NACK)
      error_led();*/
    return TWDR;
  }
  else{
    TWCR = _BV(TWINT) | _BV(TWEN) | _BV(TWEA);
    while ((TWCR & _BV(TWINT)) == 0); /* wait for transmission */
    /*if ((TWSR & 0xF8) != TW_MR_DATA_ACK)
      error_led();*/
    return TWDR;
  }
}

uint8_t rdReg(uint8_t reg){
  uint8_t dat;
  twiStart();
  twiAddr(camAddr_WR, TW_MT_SLA_ACK);
  twiWriteByte(reg, TW_MT_DATA_ACK);
  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);//send stop
  _delay_ms(1);
  twiStart();
  twiAddr(camAddr_RD, TW_MR_SLA_ACK);
  dat = twiRd(1);
  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);//send stop
  _delay_ms(1);
  return dat;
}

void wrSensorRegs8_8(const struct regval_list reglist[]){
  uint8_t reg_addr, reg_val;
  const struct regval_list *next = reglist;
  while ((reg_addr != 0xff) | (reg_val != 0xff)){
    reg_addr = pgm_read_byte(&next->reg_num);
    reg_val = pgm_read_byte(&next->value);
    wrReg(reg_addr, reg_val);
    next++;
  }
}

void setColor(void){
  wrSensorRegs8_8(yuv422_ov7670);
}

void setRes(void){
  wrReg(REG_COM3, 4); // REG_COM3 enable scaling
  wrSensorRegs8_8(qvga_ov7670);
}

void camInit(void){
  wrReg(0x12, 0x80);
  _delay_ms(100);
  wrSensorRegs8_8(ov7670_default_regs);
  wrReg(REG_COM10, 32);//PCLK does not toggle on HBLANK.
}

void arduinoUnoInut(void) {
  cli();//disable interrupts
  // Setup the 8mhz PWM clock
  // OV7670 XCLK => (through level shift!) => pin 46 = PL3(OC5A), output
  DDRL |= _BV(3);
  TCCR5A = _BV(COM5A0) | _BV(WGM51) | _BV(WGM50);
  TCCR5B = _BV(WGM53) | _BV(WGM52) | _BV(CS50);
  OCR5A = 0; // (F_CPU)/(2*(X+1))
  // OV7670 VSYNC => pin 26 = PA4, input
  DDRA &= ~ _BV(4);
  // OV7670 PCLK => pin 28 = PA6, input
  DDRA &= ~ _BV(6);
  // OV7670 D7-D0 => pin 30-37 = PC7-PC0, input
  DDRC = 0;
  _delay_ms(3000);
    //set up twi for 100khz
  TWSR &= ~3;//disable prescaler for TWI
  TWBR = 72;//set to 100khz
  
    //enable serial
  UBRR0H = 0;
  UBRR0L = 1;//0 = 2M baud rate. 1 = 1M baud. 3 = 0.5M. 7 = 250k 207 is 9600 baud rate.
  UCSR0A |= 2;//double speed aysnc
  UCSR0B = (1 << RXEN0) | (1 << TXEN0);//Enable receiver and transmitter
  UCSR0C = 6;//async 1 stop bit 8bit char no parity bits
}


void StringPgm(const char * str){
  do{
      while (!(UCSR0A & (1 << UDRE0)));//wait for byte to transmit
      UDR0 = pgm_read_byte_near(str);
      while (!(UCSR0A & (1 << UDRE0)));//wait for byte to transmit
  } while (pgm_read_byte_near(++str));
}

static void captureImg(uint16_t wg, uint16_t hg){
  uint16_t y, x;

  StringPgm(PSTR("*RDY*"));

  while (!(PINA & 16));//wait for high
  while ((PINA & 16));//wait for low

    y = hg;
  while (y--){
        x = wg;
    while (x--){
      while ((PINA & 64));//wait for low
            UDR0 = (PINC & 255);
          while (!(UCSR0A & (1 << UDRE0)));//wait for byte to transmit
      while (!(PINA & 64));//wait for high
      while ((PINA & 64));//wait for low
      while (!(PINA & 64));//wait for high
    }
   
  }
    _delay_ms(100);
}

void setup(){
  arduinoUnoInut();
  camInit();
  setRes();
  setColor();
  wrReg(0x11, 11);
}


void loop(){
  captureImg(320, 240);
}

Original code
Schematic
Atmega2560 datasheet
OV7670 datasheet

14.png