GeoTagController : add offline ULog geotagging interface

1. PX4Log parser is split off into its own class
2. New ULog parser implemented

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 \

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<int>(coordinate.latitude()));
+    gpsData.readable.extendedData.gpsLat[0] = abs(static_cast<int>(geotag.latitude));
     gpsData.readable.extendedData.gpsLat[1] = 1;
-    gpsData.readable.extendedData.gpsLat[2] = static_cast<int>((fabs(coordinate.latitude()) - floor(fabs(coordinate.latitude()))) * 60.0);
+    gpsData.readable.extendedData.gpsLat[2] = static_cast<int>((fabs(geotag.latitude) - floor(fabs(geotag.latitude))) * 60.0);
     gpsData.readable.extendedData.gpsLat[3] = 1;
-    gpsData.readable.extendedData.gpsLat[4] = static_cast<int>((fabs(coordinate.latitude()) * 60.0 - floor(fabs(coordinate.latitude()) * 60.0)) * 60000.0);
+    gpsData.readable.extendedData.gpsLat[4] = static_cast<int>((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<int>(coordinate.longitude()));
+    gpsData.readable.extendedData.gpsLon[0] = abs(static_cast<int>(geotag.longitude));
     gpsData.readable.extendedData.gpsLon[1] = 1;
-    gpsData.readable.extendedData.gpsLon[2] = static_cast<int>((fabs(coordinate.longitude()) - floor(fabs(coordinate.longitude()))) * 60.0);
+    gpsData.readable.extendedData.gpsLon[2] = static_cast<int>((fabs(geotag.longitude) - floor(fabs(geotag.longitude))) * 60.0);
     gpsData.readable.extendedData.gpsLon[3] = 1;
-    gpsData.readable.extendedData.gpsLon[4] = static_cast<int>((fabs(coordinate.longitude()) * 60.0 - floor(fabs(coordinate.longitude()) * 60.0)) * 60000.0);
+    gpsData.readable.extendedData.gpsLon[4] = static_cast<int>((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';

src/AnalyzeView/ExifParser.h

@@ -4,13 +4,15 @@
 #include <QGeoCoordinate>
 #include <QDebug>
 
+#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

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 <QDebug>
 #include <cfloat>
 
+#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<uint8_t*>(log.mid(log.indexOf(gposHeaderHeader) + 4, 1).data());
-    gposHeaderOffset = static_cast<int>(qFromLittleEndian(*iptr));
-    iptr = reinterpret_cast<uint8_t*>(log.mid(log.indexOf(triggerHeaderHeader) + 4, 1).data());
-    triggerHeaderOffset = static_cast<int>(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<uint64_t*>(log.mid(index + triggerOffsets[0], triggerLengths[0]).data());
-        double timeDouble = static_cast<double>(qFromLittleEndian(*time)) / 1.0e6;
-        uint32_t* seq = reinterpret_cast<uint32_t*>(log.mid(index + triggerOffsets[1], triggerLengths[1]).data());
-        int seqInt = static_cast<int>(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<int32_t*>(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data());
-                double latitude = static_cast<double>(qFromLittleEndian(*lat))/1.0e7;
-                lastCoordinate.setLatitude(latitude);
-                int32_t* lon = reinterpret_cast<int32_t*>(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data());
-                double longitude = static_cast<double>(qFromLittleEndian(*lon))/1.0e7;
-                longitude = fmod(180.0 + longitude, 360.0) - 180.0;
-                lastCoordinate.setLongitude(longitude);
-                float* alt = reinterpret_cast<float*>(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);
     }

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<double>           _tagTime;
-    QList<QGeoCoordinate>   _geoRef;
-    QList<double>           _triggerTime;
+    QList<double>           _imageTime;
+    QList<cameraFeedbackPacket> _triggerList;
     QList<int>              _imageIndices;
     QList<int>              _triggerIndices;
+
 };
 
 /// Controller for GeoTagPage.qml. Supports geotagging images based on logfile camera tags.

src/AnalyzeView/PX4LogParser.cc

+#include "PX4LogParser.h"
+#include <math.h>
+#include <QtEndian>
+#include <QDateTime>
+
+PX4LogParser::PX4LogParser()
+{
+
+}
+
+PX4LogParser::~PX4LogParser()
+{
+
+}
+
+bool PX4LogParser::getTagsFromLog(QByteArray& log, QList<GeoTagWorker::cameraFeedbackPacket>& 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<uint8_t*>(log.mid(log.indexOf(gposHeaderHeader) + 4, 1).data());
+    gposHeaderOffset = static_cast<int>(qFromLittleEndian(*iptr));
+    iptr = reinterpret_cast<uint8_t*>(log.mid(log.indexOf(triggerHeaderHeader) + 4, 1).data());
+    triggerHeaderOffset = static_cast<int>(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<uint64_t*>(log.mid(index + triggerOffsets[0], triggerLengths[0]).data());
+        double timeDouble = static_cast<double>(qFromLittleEndian(*time)) / 1.0e6;
+        uint32_t* seq = reinterpret_cast<uint32_t*>(log.mid(index + triggerOffsets[1], triggerLengths[1]).data());
+        int seqInt = static_cast<int>(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<int32_t*>(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data());
+                feedback.latitude = static_cast<double>(qFromLittleEndian(*lat))/1.0e7;
+                int32_t* lon = reinterpret_cast<int32_t*>(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data());
+                feedback.longitude = static_cast<double>(qFromLittleEndian(*lon))/1.0e7;
+                feedback.longitude = fmod(180.0 + feedback.longitude, 360.0) - 180.0;
+                float* alt = reinterpret_cast<float*>(log.mid(gposIndex + gposOffsets[2], gposLengths[2]).data());
+                feedback.altitude = qFromLittleEndian(*alt);
+                cameraFeedback.append(feedback);
+                break;
+            }
+        }
+    }
+
+    return true;
+}

src/AnalyzeView/PX4LogParser.h

+#ifndef PX4LOGPARSER_H
+#define PX4LOGPARSER_H
+
+#include <QGeoCoordinate>
+#include <QDebug>
+
+#include "GeoTagController.h"
+
+class PX4LogParser
+{
+public:
+    PX4LogParser();
+    ~PX4LogParser();
+    bool getTagsFromLog(QByteArray& log, QList<GeoTagWorker::cameraFeedbackPacket>& cameraFeedback);
+
+private:
+
+};
+
+#endif // PX4LOGPARSER_H

src/AnalyzeView/ULogParser.cc

+ #include "ULogParser.h"
+#include <math.h>
+#include <QDateTime>
+
+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<GeoTagWorker::cameraFeedbackPacket>& 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);