ULogParser.cc 6.18 KB
Newer Older
Gus Grubba's avatar
Gus Grubba committed
1
#include "ULogParser.h"
2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
#include <math.h>
#include <QDateTime>

ULogParser::ULogParser()
{

}

ULogParser::~ULogParser()
{

}

int ULogParser::sizeOfType(QString& typeName)
{
Don Gagne's avatar
Don Gagne committed
17
    if (typeName == QLatin1Literal("int8_t") || typeName == QLatin1Literal("uint8_t")) {
18 19
        return 1;

Don Gagne's avatar
Don Gagne committed
20
    } else if (typeName == QLatin1Literal("int16_t") || typeName == QLatin1Literal("uint16_t")) {
21 22
        return 2;

Don Gagne's avatar
Don Gagne committed
23
    } else if (typeName == QLatin1Literal("int32_t") || typeName == QLatin1Literal("uint32_t")) {
24 25
        return 4;

Don Gagne's avatar
Don Gagne committed
26
    } else if (typeName == QLatin1Literal("int64_t") || typeName == QLatin1Literal("uint64_t")) {
27 28
        return 8;

Don Gagne's avatar
Don Gagne committed
29
    } else if (typeName == QLatin1Literal("float")) {
30 31
        return 4;

Don Gagne's avatar
Don Gagne committed
32
    } else if (typeName == QLatin1Literal("double")) {
33 34
        return 8;

Don Gagne's avatar
Don Gagne committed
35
    } else if (typeName == QLatin1Literal("char") || typeName == QLatin1Literal("bool")) {
36 37 38
        return 1;
    }

39
    qWarning() << "Unknown type in ULog : " << typeName;
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
    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);

Don Gagne's avatar
Don Gagne committed
77
            if (!fieldName.contains(QLatin1Literal("_padding"))) {
78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
                _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) {

102 103
        ULogMessageHeader header;
        memset(&header, 0, sizeof(header));
104 105 106 107 108
        memcpy(&header, log.data() + index, ULOG_MSG_HEADER_LEN);

        switch (header.msgType) {
            case (int)ULogMessageType::FORMAT:
            {
109 110
                ULogMessageFormat format_msg;
                memset(&format_msg, 0, sizeof(format_msg));
111 112 113 114 115 116 117
                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);

Don Gagne's avatar
Don Gagne committed
118
                if(messageName == QLatin1Literal("camera_capture")) {
119 120 121 122 123 124 125
                    parseFieldFormat(messageFields);
                }
                break;
            }

            case (int)ULogMessageType::ADD_LOGGED_MSG:
            {
126 127
                ULogMessageAddLogged addLoggedMsg;
                memset(&addLoggedMsg, 0, sizeof(addLoggedMsg));
128 129 130 131
                memcpy(&addLoggedMsg, log.data() + index, ULOG_MSG_HEADER_LEN + header.msgSize);

                QString messageName(addLoggedMsg.msgName);

Don Gagne's avatar
Don Gagne committed
132
                if(messageName.contains(QLatin1Literal("camera_capture"))) {
133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152
                    _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
153 154
                    GeoTagWorker::cameraFeedbackPacket feedback;
                    memset(&feedback, 0, sizeof(feedback));
Don Gagne's avatar
Don Gagne committed
155
                    memcpy(&feedback.timestamp, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("timestamp")), 8);
156
                    feedback.timestamp /= 1.0e6; // to seconds
Don Gagne's avatar
Don Gagne committed
157
                    memcpy(&feedback.timestampUTC, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("timestamp_utc")), 8);
158
                    feedback.timestampUTC /= 1.0e6; // to seconds
Don Gagne's avatar
Don Gagne committed
159 160 161
                    memcpy(&feedback.imageSequence, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("seq")), 4);
                    memcpy(&feedback.latitude, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("lat")), 8);
                    memcpy(&feedback.longitude, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("lon")), 8);
162
                    feedback.longitude = fmod(180.0 + feedback.longitude, 360.0) - 180.0;
Don Gagne's avatar
Don Gagne committed
163 164 165
                    memcpy(&feedback.altitude, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("alt")), 4);
                    memcpy(&feedback.groundDistance, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("ground_distance")), 4);
                    memcpy(&feedback.captureResult, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("result")), 1);
166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183

                    cameraFeedback.append(feedback);

                }

                break;
            }

            default:
                break;
        }

        index += (3 + header.msgSize);

    }

    return true;
}