Newer
Older
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) {
ULogMessageHeader header;
memset(&header, 0, sizeof(header));
memcpy(&header, log.data() + index, ULOG_MSG_HEADER_LEN);
switch (header.msgType) {
case (int)ULogMessageType::FORMAT:
{
ULogMessageFormat format_msg;
memset(&format_msg, 0, sizeof(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;
memset(&addLoggedMsg, 0, sizeof(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;
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;
}