Extract mavlink protocol messages using qt
-
hi
I'm having a problem for a while and I can't find where this happening. I'm trying to read from an ardupilot that uses mavlink protocol for sendig data over serial port with 115200 baudRate. as you know data is serial of bytes and I should extract them into packets. here is the mavlink protocol packet form :
http://qgroundcontrol.org/mavlink/startmy code is compiling well but after an hour runnig it's have some runtime errors like :
malloc.c:3700: _int_malloc: Assertion `victim->fd_nextsize->bk_nextsize == victim' failed.
I can't find where my pointers and alocations goes wrong!
I'm using a thread for reading and extracting and at the end of loop I emit packets payload and message ID that shows this message use wich structur.here is my run loop that causes error :
@QByteArray array;
qint8 start_0xFE;
char first_01;
char second_01;
char msg_len;
qint8 ID;
QByteArray payload;
void mThread::run()
{
while(1)
{
if(serial->bytesAvailable() >= 150)
{
this->msleep(18);
array.append(serial->read(6));
if(array.contains(254))
{
this->msleep(2);
start_0xFE = array.indexOf(254);
if((start_0xFE + 6) < array.size())
{
first_01 = array.at(start_0xFE + 3);
second_01 = array.at(start_0xFE + 4);
if(first_01 == second_01 && first_01 == 01)
{
ID = array.at(start_0xFE + 5);
msg_len = array.at(start_0xFE + 1);
if(msg_len + start_0xFE + 8 > array.size())
{
if(serial->bytesAvailable() >= msg_len + 20)
{
this->msleep(2);
array.append(serial->read(msg_len + 2));
payload = array.mid(start_0xFE + 6 , msg_len);
array.remove(0 , start_0xFE + msg_len + 8);
}
}
else
{
this->msleep(2);
payload = array.mid(start_0xFE + 6 , msg_len);
array.remove(0, start_0xFE + msg_len + 8);
}
}
else
goto clr;
}
if((start_0xFE+6) >= array.size())
{
this->msleep(2);
array.append(serial->read(10));
first_01 = array.at(start_0xFE + 3);
second_01 = array.at(start_0xFE + 4);
if(first_01 == second_01 && first_01 == 01)
{
msg_len = array.at(start_0xFE + 1);
ID = array.at(start_0xFE + 5);
if(serial->bytesAvailable() >= msg_len + 2)
{
this->msleep(2);
array.append(serial->read(msg_len + 2));
payload = array.mid(start_0xFE + 6 , msg_len);
array.remove(0 , start_0xFE + msg_len + 8);
}
}
else
array.remove(0 , start_0xFE + 1);
}
}
else
clr : array.remove(0 , start_0xFE + 1);
emit updated(payload , ID);
}
}
}
@I'm sorry for my english , I know it's too bad!
and please ignore go to.
it's not the problem here. I changed it and it's not making any difference. I used it for increasing speed.