Don Gagne
8 years ago
committed by
GitHub
10 changed files with 438 additions and 124 deletions
@ -0,0 +1,99 @@
@@ -0,0 +1,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; |
||||
} |
||||
@ -0,0 +1,20 @@
@@ -0,0 +1,20 @@
|
||||
#ifndef PX4LOGPARSER_H |
||||
#define PX4LOGPARSER_H |
||||
|
||||
#include <QGeoCoordinate> |
||||
#include <QDebug> |
||||
|
||||
#include "GeoTagController.h" |
||||
|
||||
class PX4LogParser |
||||
{ |
||||
public: |
||||
PX4LogParser(); |
||||
~PX4LogParser(); |
||||
bool getTagsFromLog(QByteArray& log, QList<GeoTagWorker::cameraFeedbackPacket>& cameraFeedback); |
||||
|
||||
private: |
||||
|
||||
}; |
||||
|
||||
#endif // PX4LOGPARSER_H
|
||||
@ -0,0 +1,183 @@
@@ -0,0 +1,183 @@
|
||||
#include "ULogParser.h" |
||||
#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)); |
||||
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; |
||||
} |
||||
@ -0,0 +1,67 @@
@@ -0,0 +1,67 @@
|
||||
#ifndef ULOGPARSER_H |
||||
#define ULOGPARSER_H |
||||
|
||||
#include <QGeoCoordinate> |
||||
#include <QDebug> |
||||
|
||||
#include "GeoTagController.h" |
||||
|
||||
#define ULOG_FILE_HEADER_LEN 16 |
||||
|
||||
class ULogParser |
||||
{ |
||||
public: |
||||
ULogParser(); |
||||
~ULogParser(); |
||||
bool getTagsFromLog(QByteArray& log, QList<GeoTagWorker::cameraFeedbackPacket>& cameraFeedback); |
||||
|
||||
private: |
||||
|
||||
QMap<QString, int> _cameraCaptureOffsets; // <fieldName, fieldOffset>
|
||||
int _cameraCaptureMsgID; |
||||
|
||||
const char _ULogMagic[8] = {'U', 'L', 'o', 'g', 0x01, 0x12, 0x35}; |
||||
|
||||
int sizeOfType(QString& typeName); |
||||
int sizeOfFullType(QString &typeNameFull); |
||||
QString extractArraySize(QString& typeNameFull, int& arraySize); |
||||
|
||||
bool parseFieldFormat(QString& fields); |
||||
|
||||
enum class ULogMessageType : uint8_t { |
||||
FORMAT = 'F', |
||||
DATA = 'D', |
||||
INFO = 'I', |
||||
PARAMETER = 'P', |
||||
ADD_LOGGED_MSG = 'A', |
||||
REMOVE_LOGGED_MSG = 'R', |
||||
SYNC = 'S', |
||||
DROPOUT = 'O', |
||||
LOGGING = 'L', |
||||
}; |
||||
|
||||
#define ULOG_MSG_HEADER_LEN 3 |
||||
struct ULogMessageHeader { |
||||
uint16_t msgSize; |
||||
uint8_t msgType; |
||||
}; |
||||
|
||||
struct ULogMessageFormat { |
||||
uint16_t msgSize; |
||||
uint8_t msgType; |
||||
|
||||
char format[2096]; |
||||
}; |
||||
|
||||
struct ULogMessageAddLogged { |
||||
uint16_t msgSize; |
||||
uint8_t msgType; |
||||
|
||||
uint8_t multiID; |
||||
uint16_t msgID; |
||||
char msgName[255]; |
||||
}; |
||||
|
||||
}; |
||||
|
||||
#endif // ULOGPARSER_H
|
||||
Loading…
Reference in new issue