IP Camera h264 streaming using udp multicast with RTPS - qt

Question

I have a video conference software that works with h264 multicast streamings, now I need to make this software works with an ip camera that provides an RTPS control interface.
I requested the h264 stream and it's coming via udp multicast, the packages are in RTP format.
So, acording with some research I need to remove the RTP header from the udp payload to have my wanted data, and I need to reconstruct the I frames becouse they may be fragmented.
I'm using Qt and the class QUdpSocket
QByteArray IDR;
while(socket->hasPendingDatagrams())
{
int pendingDataSize = socket->pendingDatagramSize();
char * data = (char *) malloc(pendingDataSize);
socket->readDatagram(data, pendingDataSize);
int fragment_type = data[12] & 0x1F;
int nal_type = data[13] & 0x1F;
int start_bit = data[13] & 0x80;
int end_bit = data[13] & 0x40;
//If it is an I Frame
if (((fragment_type == 28) || (fragment_type == 29)) && (nal_type == 5))
{
if(start_bit == 128 && end_bit == 64)
{
char nalByte = (data[12] & 0xE0) | (data[13] & 0x1F);
data[13] = nalByte;
char * dataWithoutHeader = data + 13;
uint8_t* datagramToQueue = (uint8_t*) queue_malloc(pendingDataSize - 13);
memcpy(datagramToQueue, dataWithoutHeader, pendingDataSize - 13);
f << "\nI Begin + I End\n";
}
if(start_bit == 128)
{
f << "\nI Begin\n";
char nalByte = (data[12] & 0xE0) | (data[13] & 0x1F);
data[13] = nalByte;
char * dataWithoutHeader = data + 13;
IDR.append(dataWithoutHeader, pendingDataSize - 13);
}
if(end_bit == 64)
{
f << "\nI End\n";
char* dataWithoutHeader = data + 13;
IDR.append(dataWithoutHeader, pendingDataSize - 13);
datagramToQueue = (uint8_t*) queue_malloc(IDR.size());
memcpy(datagramToQueue, IDR.data(), IDR.size());
queue_enqueue(this->encodedQueue, datagramToQueue, IDR.size(),0, NULL);
IDR.clear();
}
if(start_bit != 128 && end_bit != 64)
{
char* dataWithoutHeader = data + 13;
IDR.append(dataWithoutHeader, pendingDataSize - 13);
}
f << "\nI\n";
continue;
}
f << "\nP\n";
uint8_t* datagramToQueue = (uint8_t*) queue_malloc(pendingDataSize - 12);
char* datacharMid = data + 13;
memcpy(datagramToQueue, datacharMid, pendingDataSize - 12);
queue_enqueue(this->encodedQueue, datagramToQueue, pendingDataSize - 12,0, NULL);
}
To decode the stream, My software have two implementations, the first uses FFMPEG and the second Intel Media SDK.
The both identifies the parametes of the video, FFMPEG shows as result a little strip of video where I can define some stuff that is in front of the camera and the rest of image is an entire mass with the colors of my scenario. The Intel Media SDK results in a pink screen with some gray lines moving around.
So, someone can tell me if there is some mistake in my packages parser? The order fragment_type, start_bit, end_bit are coming just don't make much sense to me.