1
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
#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;
memset(&feedback, 0, sizeof(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;
}