ULogParser.cc 6.23 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
    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;
    }

60
    arraySize = typeNameFull.midRef(startPos + 1, endPos - startPos - 1).toInt();
61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76
    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
                _cameraCaptureOffsets.insert(fieldName, offset);
                offset += sizeOfFullType(typeNameFull);
            }
        }

        prevFieldEnd = fieldEnd + 1;
        fieldEnd = fields.indexOf(';', prevFieldEnd);
    }
    return false;
}

89
bool ULogParser::getTagsFromLog(QByteArray& log, QList<GeoTagWorker::cameraFeedbackPacket>& cameraFeedback, QString& errorMessage)
90
{
91 92
    errorMessage.clear();

93 94
    //verify it's an ULog file
    if(!log.contains(_ULogMagic)) {
95
        errorMessage = tr("Could not detect ULog file header magic");
96 97 98 99 100 101 102 103
        return false;
    }

    int index = ULOG_FILE_HEADER_LEN;
    bool geotagFound = false;

    while(index < log.count() - 1) {

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

        switch (header.msgType) {
            case (int)ULogMessageType::FORMAT:
            {
111 112
                ULogMessageFormat format_msg;
                memset(&format_msg, 0, sizeof(format_msg));
113 114 115 116 117 118 119
                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
120
                if(messageName == QLatin1Literal("camera_capture")) {
121 122 123 124 125 126 127
                    parseFieldFormat(messageFields);
                }
                break;
            }

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

                QString messageName(addLoggedMsg.msgName);

Don Gagne's avatar
Don Gagne committed
134
                if(messageName.contains(QLatin1Literal("camera_capture"))) {
135 136 137 138 139 140 141 142 143 144 145 146
                    _cameraCaptureMsgID = addLoggedMsg.msgID;
                    geotagFound = true;
                }

                break;
            }

            case (int)ULogMessageType::DATA:
            {
                uint16_t msgID = -1;
                memcpy(&msgID, log.data() + index + ULOG_MSG_HEADER_LEN, 2);

147
                if (geotagFound && msgID == _cameraCaptureMsgID) {
148 149

                    // Completely dynamic parsing, so that changing/reordering the message format will not break the parser
150 151
                    GeoTagWorker::cameraFeedbackPacket feedback;
                    memset(&feedback, 0, sizeof(feedback));
Don Gagne's avatar
Don Gagne committed
152
                    memcpy(&feedback.timestamp, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("timestamp")), 8);
153
                    feedback.timestamp /= 1.0e6; // to seconds
Don Gagne's avatar
Don Gagne committed
154
                    memcpy(&feedback.timestampUTC, log.data() + index + 5 + _cameraCaptureOffsets.value(QStringLiteral("timestamp_utc")), 8);
155
                    feedback.timestampUTC /= 1.0e6; // to seconds
Don Gagne's avatar
Don Gagne committed
156 157 158
                    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);
159
                    feedback.longitude = fmod(180.0 + feedback.longitude, 360.0) - 180.0;
Don Gagne's avatar
Don Gagne committed
160 161 162
                    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);
163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178

                    cameraFeedback.append(feedback);

                }

                break;
            }

            default:
                break;
        }

        index += (3 + header.msgSize);

    }

179
    if (cameraFeedback.count() == 0) {
180
        errorMessage = tr("Could not detect camera_capture packets in ULog");
181 182 183
        return false;
    }

184 185
    return true;
}