только красный RGB 24 бита c

Я работаю с камерой OV7670, mbed и лазером, используя следующий код

void CameraSnap(){
led4 = 1;

// Kick things off by capturing an image
camera.CaptureNext();
while(camera.CaptureDone() == false);

camera.ReadStart();

t1 = t.read_ms();

unsigned colorpixel;
unsigned int byte1, byte2;
int x;
int y;

int i = 0;for (int i=0; i < SIZEX; i++) {

Maximo[i] = 0;
PuntosMaximo[i] = 0;
}for(y = 0; y<SIZEY; y++)
for(x = 0; x<SIZEX; x++)
{

// Read in the first half of the image
if(format == 'b')
{
camera.ReadOnebyte();
}
else
if(format == 'y' || format == 'r')
{
byte1 = camera.ReadOnebyte();
//pc.putc(byte1);
}
// Read in the Second half of the image

byte2 = camera.ReadOnebyte();

red = (byte1 & 0xF8)>>3;
green = (((byte1 << 8) | byte2) & 0x5E)>>5;
blue = (byte2 & 0x1F);
unsigned short data = (red+ green + blue) /3;
pc.putc(data);

if ((red > Maximo[x] )&& (green < 10) && (blue < 10 )) {
Maximo[x] = rojo;
PuntosMaximo[x] = y;

}

}
camera.ReadStop();t2 = t.read_ms();

Я хочу, чтобы программа сохраняла максимальную интенсивность пикселей в Maximo[i] из 24-битного изображения RGB, но в граббере OV7670 (https://mbed.org/users/edodm85/code/OV7670_Test_Code/file/d0d4760d32b2/main.cpp) он показывает только черно-белое изображение.

Не могли бы вы дать мне несколько советов? Заранее спасибо!