From 78bb82ad934486bb8965a21abfd53d912ff45e5b Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Thu, 4 May 2017 22:00:13 +0200 Subject: [PATCH] GeoTagController : add offline ULog geotagging interface 1. PX4Log parser is split off into its own class 2. New ULog parser implemented --- qgroundcontrol.pro | 4 + src/AnalyzeView/ExifParser.cc | 20 ++-- src/AnalyzeView/ExifParser.h | 4 +- src/AnalyzeView/GeoTagController.cc | 142 ++++++---------------- src/AnalyzeView/GeoTagController.h | 19 ++- src/AnalyzeView/PX4LogParser.cc | 98 +++++++++++++++ src/AnalyzeView/PX4LogParser.h | 20 ++++ src/AnalyzeView/ULogParser.cc | 179 ++++++++++++++++++++++++++++ src/AnalyzeView/ULogParser.h | 67 +++++++++++ 9 files changed, 430 insertions(+), 123 deletions(-) create mode 100644 src/AnalyzeView/PX4LogParser.cc create mode 100644 src/AnalyzeView/PX4LogParser.h create mode 100644 src/AnalyzeView/ULogParser.cc create mode 100644 src/AnalyzeView/ULogParser.h diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9b2c5cffc..dd2a9c877 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -450,6 +450,8 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin { HEADERS += \ src/AnalyzeView/ExifParser.h \ + src/AnalyzeView/ULogParser.h \ + src/AnalyzeView/PX4LogParser.h \ src/CmdLineOptParser.h \ src/FirmwarePlugin/PX4/px4_custom_mode.h \ src/FlightDisplay/VideoManager.h \ @@ -635,6 +637,8 @@ AndroidBuild { SOURCES += \ src/AnalyzeView/ExifParser.cc \ + src/AnalyzeView/ULogParser.cc \ + src/AnalyzeView/PX4LogParser.cc \ src/CmdLineOptParser.cc \ src/FlightDisplay/VideoManager.cc \ src/FlightMap/Widgets/ValuesWidgetController.cc \ diff --git a/src/AnalyzeView/ExifParser.cc b/src/AnalyzeView/ExifParser.cc index 3dddf642e..b82d4f1fd 100644 --- a/src/AnalyzeView/ExifParser.cc +++ b/src/AnalyzeView/ExifParser.cc @@ -56,7 +56,7 @@ double ExifParser::readTime(QByteArray& buf) return tagTime.toMSecsSinceEpoch()/1000.0; } -bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate) +bool ExifParser::write(QByteArray& buf, GeoTagWorker::cameraFeedbackPacket& geotag) { QByteArray app1Header("\xff\xe1", 2); uint32_t app1HeaderInd = buf.indexOf(app1Header); @@ -141,7 +141,7 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate) gpsData.readable.fields.gpsLatRef.tagID = 1; gpsData.readable.fields.gpsLatRef.type = 2; gpsData.readable.fields.gpsLatRef.size = 2; - gpsData.readable.fields.gpsLatRef.content = coordinate.latitude() > 0 ? 'N' : 'S'; + gpsData.readable.fields.gpsLatRef.content = geotag.latitude > 0 ? 'N' : 'S'; gpsData.readable.fields.gpsLat.tagID = 2; gpsData.readable.fields.gpsLat.type = 5; @@ -151,7 +151,7 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate) gpsData.readable.fields.gpsLonRef.tagID = 3; gpsData.readable.fields.gpsLonRef.type = 2; gpsData.readable.fields.gpsLonRef.size = 2; - gpsData.readable.fields.gpsLonRef.content = coordinate.longitude() > 0 ? 'E' : 'W'; + gpsData.readable.fields.gpsLonRef.content = geotag.longitude > 0 ? 'E' : 'W'; gpsData.readable.fields.gpsLon.tagID = 4; gpsData.readable.fields.gpsLon.type = 5; @@ -176,21 +176,21 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate) gpsData.readable.fields.finishedDataField = 0; // Filling up the additional information that does not fit into the fields - gpsData.readable.extendedData.gpsLat[0] = abs(static_cast(coordinate.latitude())); + gpsData.readable.extendedData.gpsLat[0] = abs(static_cast(geotag.latitude)); gpsData.readable.extendedData.gpsLat[1] = 1; - gpsData.readable.extendedData.gpsLat[2] = static_cast((fabs(coordinate.latitude()) - floor(fabs(coordinate.latitude()))) * 60.0); + gpsData.readable.extendedData.gpsLat[2] = static_cast((fabs(geotag.latitude) - floor(fabs(geotag.latitude))) * 60.0); gpsData.readable.extendedData.gpsLat[3] = 1; - gpsData.readable.extendedData.gpsLat[4] = static_cast((fabs(coordinate.latitude()) * 60.0 - floor(fabs(coordinate.latitude()) * 60.0)) * 60000.0); + gpsData.readable.extendedData.gpsLat[4] = static_cast((fabs(geotag.latitude) * 60.0 - floor(fabs(geotag.latitude) * 60.0)) * 60000.0); gpsData.readable.extendedData.gpsLat[5] = 1000; - gpsData.readable.extendedData.gpsLon[0] = abs(static_cast(coordinate.longitude())); + gpsData.readable.extendedData.gpsLon[0] = abs(static_cast(geotag.longitude)); gpsData.readable.extendedData.gpsLon[1] = 1; - gpsData.readable.extendedData.gpsLon[2] = static_cast((fabs(coordinate.longitude()) - floor(fabs(coordinate.longitude()))) * 60.0); + gpsData.readable.extendedData.gpsLon[2] = static_cast((fabs(geotag.longitude) - floor(fabs(geotag.longitude))) * 60.0); gpsData.readable.extendedData.gpsLon[3] = 1; - gpsData.readable.extendedData.gpsLon[4] = static_cast((fabs(coordinate.longitude()) * 60.0 - floor(fabs(coordinate.longitude()) * 60.0)) * 60000.0); + gpsData.readable.extendedData.gpsLon[4] = static_cast((fabs(geotag.longitude) * 60.0 - floor(fabs(geotag.longitude) * 60.0)) * 60000.0); gpsData.readable.extendedData.gpsLon[5] = 1000; - gpsData.readable.extendedData.gpsAlt[0] = coordinate.altitude() * 100; + gpsData.readable.extendedData.gpsAlt[0] = geotag.altitude * 100; gpsData.readable.extendedData.gpsAlt[1] = 100; gpsData.readable.extendedData.mapDatum[0] = 'W'; gpsData.readable.extendedData.mapDatum[1] = 'G'; diff --git a/src/AnalyzeView/ExifParser.h b/src/AnalyzeView/ExifParser.h index 9a29da380..bf7578375 100644 --- a/src/AnalyzeView/ExifParser.h +++ b/src/AnalyzeView/ExifParser.h @@ -4,13 +4,15 @@ #include #include +#include "GeoTagController.h" + class ExifParser { public: ExifParser(); ~ExifParser(); double readTime(QByteArray& buf); - bool write(QByteArray& data, QGeoCoordinate coordinate); + bool write(QByteArray& buf, GeoTagWorker::cameraFeedbackPacket& geotag); }; #endif // EXIFPARSER_H diff --git a/src/AnalyzeView/GeoTagController.cc b/src/AnalyzeView/GeoTagController.cc index 0fbab4ff2..bf846bc0d 100644 --- a/src/AnalyzeView/GeoTagController.cc +++ b/src/AnalyzeView/GeoTagController.cc @@ -8,7 +8,6 @@ ****************************************************************************/ #include "GeoTagController.h" -#include "ExifParser.h" #include "QGCQFileDialog.h" #include "QGCLoggingCategory.h" #include "MainWindow.h" @@ -18,6 +17,10 @@ #include #include +#include "ExifParser.h" +#include "ULogParser.h" +#include "PX4LogParser.h" + GeoTagController::GeoTagController(void) : _progress(0) , _inProgress(false) @@ -35,7 +38,7 @@ GeoTagController::~GeoTagController() void GeoTagController::pickLogFile(void) { - QString filename = QGCQFileDialog::getOpenFileName(MainWindow::instance(), "Select log file load", QString(), "PX4 log file (*.px4log);;All Files (*.*)"); + QString filename = QGCQFileDialog::getOpenFileName(MainWindow::instance(), "Select log file load", QString(), "ULog file (*.ulg);;PX4 log file (*.px4log);;All Files (*.*)"); if (!filename.isEmpty()) { _worker.setLogFile(filename); emit logFileChanged(filename); @@ -173,7 +176,7 @@ void GeoTagWorker::run(void) // Parse EXIF ExifParser exifParser; - _tagTime.clear(); + _imageTime.clear(); for (int i = 0; i < _imageList.size(); ++i) { QFile file(_imageList.at(i).absoluteFilePath()); if (!file.open(QIODevice::ReadOnly)) { @@ -183,7 +186,7 @@ void GeoTagWorker::run(void) QByteArray imageBuffer = file.readAll(); file.close(); - _tagTime.append(exifParser.readTime(imageBuffer)); + _imageTime.append(exifParser.readTime(imageBuffer)); emit progressChanged((100/nSteps) + ((100/nSteps) / _imageList.size())*i); @@ -194,10 +197,30 @@ void GeoTagWorker::run(void) } } - // Load PX4 log - _geoRef.clear(); - _triggerTime.clear(); - if (!parsePX4Log()) { + // Load log + bool isULog = _logFile.endsWith(".ulg", Qt::CaseSensitive); + QFile file(_logFile); + if (!file.open(QIODevice::ReadOnly)) { + emit error(tr("Geotagging failed. Couldn't open log file.")); + return; + } + QByteArray log = file.readAll(); + file.close(); + + // Instantiate appropriate parser + _triggerList.clear(); + bool parseComplete = false; + if(isULog) { + ULogParser parser; + parseComplete = parser.getTagsFromLog(log, _triggerList); + + } else { + PX4LogParser parser; + parseComplete = parser.getTagsFromLog(log, _triggerList); + + } + + if (!parseComplete) { if (_cancel) { qCDebug(GeotaggingLog) << "Tagging cancelled"; emit error(tr("Tagging cancelled")); @@ -210,7 +233,7 @@ void GeoTagWorker::run(void) } emit progressChanged(3*(100/nSteps)); - qCDebug(GeotaggingLog) << "Found " << _geoRef.count() << " trigger logs."; + qCDebug(GeotaggingLog) << "Found " << _triggerList.count() << " trigger logs."; if (_cancel) { qCDebug(GeotaggingLog) << "Tagging cancelled"; @@ -244,7 +267,7 @@ void GeoTagWorker::run(void) QByteArray imageBuffer = fileRead.readAll(); fileRead.close(); - if (!exifParser.write(imageBuffer, _geoRef[_triggerIndices[i]])) { + if (!exifParser.write(imageBuffer, _triggerList[_triggerIndices[i]])) { emit error(tr("Geotagging failed. Couldn't write to image.")); return; } else { @@ -279,108 +302,11 @@ void GeoTagWorker::run(void) emit progressChanged(100); } -bool GeoTagWorker::parsePX4Log() -{ - // general message header - char header[] = {(char)0xA3, (char)0x95, (char)0x00}; - // header for GPOS message header - char gposHeaderHeader[] = {(char)0xA3, (char)0x95, (char)0x80, (char)0x10, (char)0x00}; - int gposHeaderOffset; - // header for GPOS message - char gposHeader[] = {(char)0xA3, (char)0x95, (char)0x10, (char)0x00}; - int gposOffsets[3] = {3, 7, 11}; - int gposLengths[3] = {4, 4, 4}; - // header for trigger message header - char triggerHeaderHeader[] = {(char)0xA3, (char)0x95, (char)0x80, (char)0x37, (char)0x00}; - int triggerHeaderOffset; - // header for trigger message - char triggerHeader[] = {(char)0xA3, (char)0x95, (char)0x37, (char)0x00}; - int triggerOffsets[2] = {3, 11}; - int triggerLengths[2] = {8, 4}; - - // load log - QFile file(_logFile); - if (!file.open(QIODevice::ReadOnly)) { - qCDebug(GeotaggingLog) << "Could not open log file " << _logFile; - return false; - } - QByteArray log = file.readAll(); - file.close(); - - // extract header information: message lengths - uint8_t* iptr = reinterpret_cast(log.mid(log.indexOf(gposHeaderHeader) + 4, 1).data()); - gposHeaderOffset = static_cast(qFromLittleEndian(*iptr)); - iptr = reinterpret_cast(log.mid(log.indexOf(triggerHeaderHeader) + 4, 1).data()); - triggerHeaderOffset = static_cast(qFromLittleEndian(*iptr)); - - // extract trigger data - int index = 1; - int sequence = -1; - QGeoCoordinate lastCoordinate; - while(index < log.count() - 1) { - - if (_cancel) { - return false; - } - - // first extract trigger - index = log.indexOf(triggerHeader, index + 1); - // check for whether last entry has been passed - if (index < 0) { - break; - } - - if (log.indexOf(header, index + 1) != index + triggerHeaderOffset) { - continue; - } - uint64_t* time = reinterpret_cast(log.mid(index + triggerOffsets[0], triggerLengths[0]).data()); - double timeDouble = static_cast(qFromLittleEndian(*time)) / 1.0e6; - uint32_t* seq = reinterpret_cast(log.mid(index + triggerOffsets[1], triggerLengths[1]).data()); - int seqInt = static_cast(qFromLittleEndian(*seq)); - if (sequence >= seqInt || sequence + 20 < seqInt) { // assume that logging has not skipped more than 20 triggers. this prevents wrong header detection - continue; - } - _triggerTime.append(timeDouble); - sequence = seqInt; - - // second extract position - bool lookForGpos = true; - while (lookForGpos) { - - if (_cancel) { - return false; - } - - int gposIndex = log.indexOf(gposHeader, index + 1); - if (gposIndex < 0) { - _geoRef.append(lastCoordinate); - break; - } - index = gposIndex; - // verify that at an offset of gposHeaderOffset the next log message starts - if (gposIndex + gposHeaderOffset == log.indexOf(header, gposIndex + 1)) { - int32_t* lat = reinterpret_cast(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data()); - double latitude = static_cast(qFromLittleEndian(*lat))/1.0e7; - lastCoordinate.setLatitude(latitude); - int32_t* lon = reinterpret_cast(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data()); - double longitude = static_cast(qFromLittleEndian(*lon))/1.0e7; - longitude = fmod(180.0 + longitude, 360.0) - 180.0; - lastCoordinate.setLongitude(longitude); - float* alt = reinterpret_cast(log.mid(gposIndex + gposOffsets[2], gposLengths[2]).data()); - lastCoordinate.setAltitude(qFromLittleEndian(*alt)); - _geoRef.append(lastCoordinate); - break; - } - } - } - return true; -} - bool GeoTagWorker::triggerFiltering() { _imageIndices.clear(); _triggerIndices.clear(); - for(int i = 0; i < _tagTime.count() && i < _triggerTime.count(); i++) { + for(int i = 0; i < _imageTime.count() && i < _triggerList.count(); i++) { _imageIndices.append(i); _triggerIndices.append(i); } diff --git a/src/AnalyzeView/GeoTagController.h b/src/AnalyzeView/GeoTagController.h index a9c67f68c..aee58f6dd 100644 --- a/src/AnalyzeView/GeoTagController.h +++ b/src/AnalyzeView/GeoTagController.h @@ -38,6 +38,18 @@ public: void cancelTagging (void) { _cancel = true; } + struct cameraFeedbackPacket { + double timestamp; + double timestampUTC; + uint32_t imageSequence; + double latitude; + double longitude; + float altitude; + float groundDistance; + float attitudeQuaternion[4]; + uint8_t captureResult; + }; + protected: void run(void) final; @@ -47,7 +59,6 @@ signals: void progressChanged (double progress); private: - bool parsePX4Log(); bool triggerFiltering(); bool _cancel; @@ -55,11 +66,11 @@ private: QString _imageDirectory; QString _saveDirectory; QFileInfoList _imageList; - QList _tagTime; - QList _geoRef; - QList _triggerTime; + QList _imageTime; + QList _triggerList; QList _imageIndices; QList _triggerIndices; + }; /// Controller for GeoTagPage.qml. Supports geotagging images based on logfile camera tags. diff --git a/src/AnalyzeView/PX4LogParser.cc b/src/AnalyzeView/PX4LogParser.cc new file mode 100644 index 000000000..fac390702 --- /dev/null +++ b/src/AnalyzeView/PX4LogParser.cc @@ -0,0 +1,98 @@ +#include "PX4LogParser.h" +#include +#include +#include + +PX4LogParser::PX4LogParser() +{ + +} + +PX4LogParser::~PX4LogParser() +{ + +} + +bool PX4LogParser::getTagsFromLog(QByteArray& log, QList& cameraFeedback) +{ + + // general message header + char header[] = {(char)0xA3, (char)0x95, (char)0x00}; + // header for GPOS message header + char gposHeaderHeader[] = {(char)0xA3, (char)0x95, (char)0x80, (char)0x10, (char)0x00}; + int gposHeaderOffset; + // header for GPOS message + char gposHeader[] = {(char)0xA3, (char)0x95, (char)0x10, (char)0x00}; + int gposOffsets[3] = {3, 7, 11}; + int gposLengths[3] = {4, 4, 4}; + // header for trigger message header + char triggerHeaderHeader[] = {(char)0xA3, (char)0x95, (char)0x80, (char)0x37, (char)0x00}; + int triggerHeaderOffset; + // header for trigger message + char triggerHeader[] = {(char)0xA3, (char)0x95, (char)0x37, (char)0x00}; + int triggerOffsets[2] = {3, 11}; + int triggerLengths[2] = {8, 4}; + + // extract header information: message lengths + uint8_t* iptr = reinterpret_cast(log.mid(log.indexOf(gposHeaderHeader) + 4, 1).data()); + gposHeaderOffset = static_cast(qFromLittleEndian(*iptr)); + iptr = reinterpret_cast(log.mid(log.indexOf(triggerHeaderHeader) + 4, 1).data()); + triggerHeaderOffset = static_cast(qFromLittleEndian(*iptr)); + + // extract trigger data + int index = 1; + int sequence = -1; + while(index < log.count() - 1) { + + // first extract trigger + index = log.indexOf(triggerHeader, index + 1); + // check for whether last entry has been passed + if (index < 0) { + break; + } + + if (log.indexOf(header, index + 1) != index + triggerHeaderOffset) { + continue; + } + + GeoTagWorker::cameraFeedbackPacket feedback{}; + + uint64_t* time = reinterpret_cast(log.mid(index + triggerOffsets[0], triggerLengths[0]).data()); + double timeDouble = static_cast(qFromLittleEndian(*time)) / 1.0e6; + uint32_t* seq = reinterpret_cast(log.mid(index + triggerOffsets[1], triggerLengths[1]).data()); + int seqInt = static_cast(qFromLittleEndian(*seq)); + if (sequence >= seqInt || sequence + 20 < seqInt) { // assume that logging has not skipped more than 20 triggers. this prevents wrong header detection + continue; + } + feedback.timestamp = timeDouble; + feedback.imageSequence = seqInt; + sequence = seqInt; + + // second extract position + bool lookForGpos = true; + while (lookForGpos) { + + int gposIndex = log.indexOf(gposHeader, index + 1); + if (gposIndex < 0) { + cameraFeedback.append(feedback); + break; + } + index = gposIndex; + + // verify that at an offset of gposHeaderOffset the next log message starts + if (gposIndex + gposHeaderOffset == log.indexOf(header, gposIndex + 1)) { + int32_t* lat = reinterpret_cast(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data()); + feedback.latitude = static_cast(qFromLittleEndian(*lat))/1.0e7; + int32_t* lon = reinterpret_cast(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data()); + feedback.longitude = static_cast(qFromLittleEndian(*lon))/1.0e7; + feedback.longitude = fmod(180.0 + feedback.longitude, 360.0) - 180.0; + float* alt = reinterpret_cast(log.mid(gposIndex + gposOffsets[2], gposLengths[2]).data()); + feedback.altitude = qFromLittleEndian(*alt); + cameraFeedback.append(feedback); + break; + } + } + } + + return true; +} diff --git a/src/AnalyzeView/PX4LogParser.h b/src/AnalyzeView/PX4LogParser.h new file mode 100644 index 000000000..ae010f4af --- /dev/null +++ b/src/AnalyzeView/PX4LogParser.h @@ -0,0 +1,20 @@ +#ifndef PX4LOGPARSER_H +#define PX4LOGPARSER_H + +#include +#include + +#include "GeoTagController.h" + +class PX4LogParser +{ +public: + PX4LogParser(); + ~PX4LogParser(); + bool getTagsFromLog(QByteArray& log, QList& cameraFeedback); + +private: + +}; + +#endif // PX4LOGPARSER_H diff --git a/src/AnalyzeView/ULogParser.cc b/src/AnalyzeView/ULogParser.cc new file mode 100644 index 000000000..41e7eca6a --- /dev/null +++ b/src/AnalyzeView/ULogParser.cc @@ -0,0 +1,179 @@ + #include "ULogParser.h" +#include +#include + +ULogParser::ULogParser() +{ + +} + +ULogParser::~ULogParser() +{ + +} + +int ULogParser::sizeOfType(QString& typeName) +{ + if (typeName == "int8_t" || typeName == "uint8_t") { + return 1; + + } else if (typeName == "int16_t" || typeName == "uint16_t") { + return 2; + + } else if (typeName == "int32_t" || typeName == "uint32_t") { + return 4; + + } else if (typeName == "int64_t" || typeName == "uint64_t") { + return 8; + + } else if (typeName == "float") { + return 4; + + } else if (typeName == "double") { + return 8; + + } else if (typeName == "char" || typeName == "bool") { + return 1; + } + + qWarning() << "Unkown type in ULog : " << typeName; + return 0; +} + +int ULogParser::sizeOfFullType(QString& typeNameFull) +{ + int arraySize; + QString typeName = extractArraySize(typeNameFull, arraySize); + return sizeOfType(typeName) * arraySize; +} + +QString ULogParser::extractArraySize(QString &typeNameFull, int &arraySize) +{ + int startPos = typeNameFull.indexOf('['); + int endPos = typeNameFull.indexOf(']'); + + if (startPos == -1 || endPos == -1) { + arraySize = 1; + return typeNameFull; + } + + arraySize = typeNameFull.mid(startPos + 1, endPos - startPos - 1).toInt(); + return typeNameFull.mid(0, startPos); +} + +bool ULogParser::parseFieldFormat(QString& fields) +{ + int prevFieldEnd = 0; + int fieldEnd = fields.indexOf(';'); + int offset = 0; + + while (fieldEnd != -1) { + int spacePos = fields.indexOf(' ', prevFieldEnd); + + if (spacePos != -1) { + QString typeNameFull = fields.mid(prevFieldEnd, spacePos - prevFieldEnd); + QString fieldName = fields.mid(spacePos + 1, fieldEnd - spacePos - 1); + + if (!fieldName.contains("_padding")) { + _cameraCaptureOffsets.insert(fieldName, offset); + offset += sizeOfFullType(typeNameFull); + } + } + + prevFieldEnd = fieldEnd + 1; + fieldEnd = fields.indexOf(';', prevFieldEnd); + } + return false; +} + +bool ULogParser::getTagsFromLog(QByteArray& log, QList& cameraFeedback) +{ + //verify it's an ULog file + if(!log.contains(_ULogMagic)) { + qWarning() << "Could not detect ULog file header magic"; + return false; + } + + int index = ULOG_FILE_HEADER_LEN; + bool geotagFound = false; + + while(index < log.count() - 1) { + + ULogMessageHeader header{}; + memcpy(&header, log.data() + index, ULOG_MSG_HEADER_LEN); + + switch (header.msgType) { + case (int)ULogMessageType::FORMAT: + { + ULogMessageFormat format_msg{}; + memcpy(&format_msg, log.data() + index, ULOG_MSG_HEADER_LEN + header.msgSize); + + QString fmt(format_msg.format); + int posSeparator = fmt.indexOf(':'); + QString messageName = fmt.left(posSeparator); + QString messageFields = fmt.mid(posSeparator + 1, header.msgSize - posSeparator - 1); + + if(messageName == "camera_capture") { + parseFieldFormat(messageFields); + } + break; + } + + case (int)ULogMessageType::ADD_LOGGED_MSG: + { + ULogMessageAddLogged addLoggedMsg{}; + memcpy(&addLoggedMsg, log.data() + index, ULOG_MSG_HEADER_LEN + header.msgSize); + + QString messageName(addLoggedMsg.msgName); + + if(messageName.contains("camera_capture")) { + _cameraCaptureMsgID = addLoggedMsg.msgID; + geotagFound = true; + } + + break; + } + + case (int)ULogMessageType::DATA: + { + if (!geotagFound) { + qWarning() << "Could not detect geotag packets in ULog"; + return false; + } + + uint16_t msgID = -1; + memcpy(&msgID, log.data() + index + ULOG_MSG_HEADER_LEN, 2); + + if(msgID == _cameraCaptureMsgID) { + + // Completely dynamic parsing, so that changing/reordering the message format will not break the parser + GeoTagWorker::cameraFeedbackPacket feedback{}; + memcpy(&feedback.timestamp, log.data() + index + 5 + _cameraCaptureOffsets.value("timestamp"), 8); + feedback.timestamp /= 1.0e6; // to seconds + memcpy(&feedback.timestampUTC, log.data() + index + 5 + _cameraCaptureOffsets.value("timestamp_utc"), 8); + feedback.timestampUTC /= 1.0e6; // to seconds + memcpy(&feedback.imageSequence, log.data() + index + 5 + _cameraCaptureOffsets.value("seq"), 4); + memcpy(&feedback.latitude, log.data() + index + 5 + _cameraCaptureOffsets.value("lat"), 8); + memcpy(&feedback.longitude, log.data() + index + 5 + _cameraCaptureOffsets.value("lon"), 8); + feedback.longitude = fmod(180.0 + feedback.longitude, 360.0) - 180.0; + memcpy(&feedback.altitude, log.data() + index + 5 + _cameraCaptureOffsets.value("alt"), 4); + memcpy(&feedback.groundDistance, log.data() + index + 5 + _cameraCaptureOffsets.value("ground_distance"), 4); + memcpy(&feedback.captureResult, log.data() + index + 5 + _cameraCaptureOffsets.value("result"), 1); + + cameraFeedback.append(feedback); + + } + + break; + } + + default: + break; + } + + index += (3 + header.msgSize); + + } + + return true; +} diff --git a/src/AnalyzeView/ULogParser.h b/src/AnalyzeView/ULogParser.h new file mode 100644 index 000000000..bb2de6e72 --- /dev/null +++ b/src/AnalyzeView/ULogParser.h @@ -0,0 +1,67 @@ +#ifndef ULOGPARSER_H +#define ULOGPARSER_H + +#include +#include + +#include "GeoTagController.h" + +#define ULOG_FILE_HEADER_LEN 16 + +class ULogParser +{ +public: + ULogParser(); + ~ULogParser(); + bool getTagsFromLog(QByteArray& log, QList& cameraFeedback); + +private: + + QMap _cameraCaptureOffsets; // + int _cameraCaptureMsgID; + + const char _ULogMagic[8] = {'U', 'L', 'o', 'g', 0x01, 0x12, 0x35}; + + int sizeOfType(QString& typeName); + int sizeOfFullType(QString &typeNameFull); + QString extractArraySize(QString& typeNameFull, int& arraySize); + + bool parseFieldFormat(QString& fields); + + enum class ULogMessageType : uint8_t { + FORMAT = 'F', + DATA = 'D', + INFO = 'I', + PARAMETER = 'P', + ADD_LOGGED_MSG = 'A', + REMOVE_LOGGED_MSG = 'R', + SYNC = 'S', + DROPOUT = 'O', + LOGGING = 'L', + }; + + #define ULOG_MSG_HEADER_LEN 3 + struct ULogMessageHeader { + uint16_t msgSize; + uint8_t msgType; + }; + + struct ULogMessageFormat { + uint16_t msgSize; + uint8_t msgType; + + char format[2096]; + }; + + struct ULogMessageAddLogged { + uint16_t msgSize; + uint8_t msgType; + + uint8_t multiID; + uint16_t msgID; + char msgName[255]; + }; + +}; + +#endif // ULOGPARSER_H -- 2.22.0