ULogParser.cc 5.82 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 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 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 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
#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) {

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 118 119 120 121 122 123 124 125
                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:
            {
126 127
                ULogMessageAddLogged addLoggedMsg;
                memset(&addLoggedMsg, 0, sizeof(addLoggedMsg));
128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152
                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
153 154
                    GeoTagWorker::cameraFeedbackPacket feedback;
                    memset(&feedback, 0, sizeof(feedback));
155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183
                    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;
}