Commit 82446241 authored by Gus Grubba's avatar Gus Grubba

Objectify log processor.

parent 95f0259e
This diff is collapsed.
...@@ -73,24 +73,31 @@ private: ...@@ -73,24 +73,31 @@ private:
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
class CurrentRunningLog class MavlinkLogProcessor
{ {
public: public:
CurrentRunningLog() MavlinkLogProcessor();
: fd(NULL) ~MavlinkLogProcessor();
, record(NULL) void close ();
, written(0) bool valid ();
{ bool create (MavlinkLogManager *manager, const QString path, uint8_t id);
} MavlinkLogFiles* record () { return _record; }
~CurrentRunningLog() QString fileName () { return _fileName; }
{ bool processStreamData(uint16_t _sequence, uint8_t first_message, QByteArray data);
close(); private:
} bool _checkSequence(uint16_t seq, int &num_drops);
void close(); QByteArray _writeUlogMessage(QByteArray &data);
FILE* fd; void _writeData(void* data, int len);
QString fileName; private:
MavlinkLogFiles* record; FILE* _fd;
quint32 written; quint32 _written;
int _sequence;
int _numDrops;
bool _gotHeader;
bool _error;
QByteArray _ulogMessage;
QString _fileName;
MavlinkLogFiles* _record;
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
...@@ -163,7 +170,7 @@ private slots: ...@@ -163,7 +170,7 @@ private slots:
void _dataAvailable (); void _dataAvailable ();
void _uploadProgress (qint64 bytesSent, qint64 bytesTotal); void _uploadProgress (qint64 bytesSent, qint64 bytesTotal);
void _activeVehicleChanged (Vehicle* vehicle); void _activeVehicleChanged (Vehicle* vehicle);
void _mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool acked); void _mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
void _armedChanged (bool armed); void _armedChanged (bool armed);
void _commandLongAck (uint8_t compID, uint16_t command, uint8_t result); void _commandLongAck (uint8_t compID, uint16_t command, uint8_t result);
void _processCmdAck (); void _processCmdAck ();
...@@ -191,8 +198,7 @@ private: ...@@ -191,8 +198,7 @@ private:
Vehicle* _vehicle; Vehicle* _vehicle;
bool _logRunning; bool _logRunning;
bool _loggingDisabled; bool _loggingDisabled;
CurrentRunningLog* _currentSavingFile; MavlinkLogProcessor* _logProcessor;
uint16_t _sequence;
bool _deleteAfterUpload; bool _deleteAfterUpload;
int _loggingCmdTryCount; int _loggingCmdTryCount;
QTimer _ackTimer; QTimer _ackTimer;
......
...@@ -2015,7 +2015,8 @@ Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message) ...@@ -2015,7 +2015,8 @@ Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
{ {
mavlink_logging_data_t log; mavlink_logging_data_t log;
mavlink_msg_logging_data_decode(&message, &log); mavlink_msg_logging_data_decode(&message, &log);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, log.length, log.first_message_offset, log.data, false); emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
...@@ -2025,7 +2026,8 @@ Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message) ...@@ -2025,7 +2026,8 @@ Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
mavlink_logging_data_t log; mavlink_logging_data_t log;
mavlink_msg_logging_data_decode(&message, &log); mavlink_msg_logging_data_decode(&message, &log);
_ackMavlinkLogData(log.sequence); _ackMavlinkLogData(log.sequence);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence, log.length, log.first_message_offset, log.data, true); emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
} }
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
......
...@@ -643,7 +643,7 @@ signals: ...@@ -643,7 +643,7 @@ signals:
void mavlinkScaledImu3(mavlink_message_t message); void mavlinkScaledImu3(mavlink_message_t message);
// Mavlink Log Download // Mavlink Log Download
void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message, const uint8_t* data, bool acked); void mavlinkLogData (Vehicle* vehicle, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t first_message, QByteArray data, bool acked);
private slots: private slots:
void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment