|
|
|
@ -291,11 +291,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
@@ -291,11 +291,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
|
|
|
|
|
// Log data
|
|
|
|
|
if (m_loggingEnabled && m_logfile) |
|
|
|
|
{ |
|
|
|
|
uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)]; |
|
|
|
|
uint8_t buf[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)] = {0}; |
|
|
|
|
quint64 time = QGC::groundTimeUsecs(); |
|
|
|
|
memcpy(buf, (void*)&time, sizeof(quint64)); |
|
|
|
|
// Write message to buffer
|
|
|
|
|
int len = mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); |
|
|
|
|
mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message); |
|
|
|
|
//we need to write the maximum package length for having a
|
|
|
|
|
//consistent file structure and beeing able to parse it again
|
|
|
|
|
int len = MAVLINK_MAX_PACKET_LEN + sizeof(quint64); |
|
|
|
|
QByteArray b((const char*)buf, len); |
|
|
|
|
if(m_logfile->write(b) != len) |
|
|
|
|
{ |
|
|
|
|