read from serial port using mavlink protocol with qt c++ - c++

Question

I'm trying to read from ardupilot that using mavlink protocol for sending data. if you visit this site:
http://qgroundcontrol.org/mavlink/start
you can find how mavlink protocol works and what I'm trying to do. at the bottom of the site you can see the header's bytes arrangement.
I wrote some code and it's works correctly but after running for a while runtime error appears. as far as I know this errors are about memory allocations but I can't find where I allocate memory illegally.
Here is my reader thread code:
QByteArray header;
quint8 ID;
QByteArray payload;
QSerialPort *serial;
void mThread::run()
{//1
while(serial->isOpen())
{//2
if(serial->bytesAvailable() >= 200)
{//3
this->msleep(1);
header = serial->read(1);
if(header.contains(254))
{//4
this->msleep(5);
header.append(serial->read(5));
if((header[3] == 1) && (header[4] == 1))
{//5
ID = header[5];
msleep(12);
payload = serial->read(header[1]);
emit updated(payload , ID);
payload.clear();
header.clear();
}//5
else
{//6
header.clear();
}//6
}//4
else
{//7
header.clear();
}//7
}//3
}//2
}//1
I'm using qt5 whit ubuntu 14.04
and I should mention that I'm reading data with 115200baud and it should manage with thread. If not reading from it will lock my ui.

Answer 1

Do you really need all those msleeps... Reading at that baud rate I would think your serial port input buffer would overflow pretty quickly. Might be worth the time to have a look at it.
As a complete different approach why not use the MAVlink generator to generate a library that you could use. This way it saves you from the trouble of parsing the messages and error checking them and all that...