Skip to content

Q
qgroundcontrol
Commit e8cce68c authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #5133 from mhkabir/pr-trigger-geotag-transitional 

ULog Geotagging Support v2
parents 741e3bbe c24cbd72
Hide whitespace changes
Inline Side-by-side
Showing with 438 additions and 124 deletions
qgroundcontrol.pro
View file @ e8cce68c
......@@ -450,6 +450,8 @@ DebugBuild { PX4FirmwarePlugin { PX4FirmwarePluginFactory { APMFirmwarePlugin {
HEADERS += \
src/AnalyzeView/ExifParser.h \
src/AnalyzeView/ULogParser.h \
src/AnalyzeView/PX4LogParser.h \
src/CmdLineOptParser.h \
src/FirmwarePlugin/PX4/px4_custom_mode.h \
src/FlightDisplay/VideoManager.h \
......@@ -635,6 +637,8 @@ AndroidBuild {
SOURCES += \
src/AnalyzeView/ExifParser.cc \
src/AnalyzeView/ULogParser.cc \
src/AnalyzeView/PX4LogParser.cc \
src/CmdLineOptParser.cc \
src/FlightDisplay/VideoManager.cc \
src/FlightMap/Widgets/ValuesWidgetController.cc \
......
src/AnalyzeView/ExifParser.cc
View file @ e8cce68c
......@@ -56,7 +56,7 @@ double ExifParser::readTime(QByteArray& buf)
return tagTime.toMSecsSinceEpoch()/1000.0;
}
bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate)
bool ExifParser::write(QByteArray& buf, GeoTagWorker::cameraFeedbackPacket& geotag)
{
QByteArray app1Header("\xff\xe1", 2);
uint32_t app1HeaderInd = buf.indexOf(app1Header);
......@@ -141,7 +141,7 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate)
gpsData.readable.fields.gpsLatRef.tagID = 1;
gpsData.readable.fields.gpsLatRef.type = 2;
gpsData.readable.fields.gpsLatRef.size = 2;
gpsData.readable.fields.gpsLatRef.content = coordinate.latitude() > 0 ? 'N' : 'S';
gpsData.readable.fields.gpsLatRef.content = geotag.latitude > 0 ? 'N' : 'S';
gpsData.readable.fields.gpsLat.tagID = 2;
gpsData.readable.fields.gpsLat.type = 5;
......@@ -151,7 +151,7 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate)
gpsData.readable.fields.gpsLonRef.tagID = 3;
gpsData.readable.fields.gpsLonRef.type = 2;
gpsData.readable.fields.gpsLonRef.size = 2;
gpsData.readable.fields.gpsLonRef.content = coordinate.longitude() > 0 ? 'E' : 'W';
gpsData.readable.fields.gpsLonRef.content = geotag.longitude > 0 ? 'E' : 'W';
gpsData.readable.fields.gpsLon.tagID = 4;
gpsData.readable.fields.gpsLon.type = 5;
......@@ -176,21 +176,21 @@ bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate)
gpsData.readable.fields.finishedDataField = 0;
// Filling up the additional information that does not fit into the fields
gpsData.readable.extendedData.gpsLat[0] = abs(static_cast<int>(coordinate.latitude()));
gpsData.readable.extendedData.gpsLat[0] = abs(static_cast<int>(geotag.latitude));
gpsData.readable.extendedData.gpsLat[1] = 1;
gpsData.readable.extendedData.gpsLat[2] = static_cast<int>((fabs(coordinate.latitude()) - floor(fabs(coordinate.latitude()))) * 60.0);
gpsData.readable.extendedData.gpsLat[2] = static_cast<int>((fabs(geotag.latitude) - floor(fabs(geotag.latitude))) * 60.0);
gpsData.readable.extendedData.gpsLat[3] = 1;
gpsData.readable.extendedData.gpsLat[4] = static_cast<int>((fabs(coordinate.latitude()) * 60.0 - floor(fabs(coordinate.latitude()) * 60.0)) * 60000.0);
gpsData.readable.extendedData.gpsLat[4] = static_cast<int>((fabs(geotag.latitude) * 60.0 - floor(fabs(geotag.latitude) * 60.0)) * 60000.0);
gpsData.readable.extendedData.gpsLat[5] = 1000;
gpsData.readable.extendedData.gpsLon[0] = abs(static_cast<int>(coordinate.longitude()));
gpsData.readable.extendedData.gpsLon[0] = abs(static_cast<int>(geotag.longitude));
gpsData.readable.extendedData.gpsLon[1] = 1;
gpsData.readable.extendedData.gpsLon[2] = static_cast<int>((fabs(coordinate.longitude()) - floor(fabs(coordinate.longitude()))) * 60.0);
gpsData.readable.extendedData.gpsLon[2] = static_cast<int>((fabs(geotag.longitude) - floor(fabs(geotag.longitude))) * 60.0);
gpsData.readable.extendedData.gpsLon[3] = 1;
gpsData.readable.extendedData.gpsLon[4] = static_cast<int>((fabs(coordinate.longitude()) * 60.0 - floor(fabs(coordinate.longitude()) * 60.0)) * 60000.0);
gpsData.readable.extendedData.gpsLon[4] = static_cast<int>((fabs(geotag.longitude) * 60.0 - floor(fabs(geotag.longitude) * 60.0)) * 60000.0);
gpsData.readable.extendedData.gpsLon[5] = 1000;
gpsData.readable.extendedData.gpsAlt[0] = coordinate.altitude() * 100;
gpsData.readable.extendedData.gpsAlt[0] = geotag.altitude * 100;
gpsData.readable.extendedData.gpsAlt[1] = 100;
gpsData.readable.extendedData.mapDatum[0] = 'W';
gpsData.readable.extendedData.mapDatum[1] = 'G';
......
src/AnalyzeView/ExifParser.h
View file @ e8cce68c
......@@ -4,13 +4,15 @@
#include <QGeoCoordinate>
#include <QDebug>
#include "GeoTagController.h"
class ExifParser
{
public:
ExifParser();
~ExifParser();
double readTime(QByteArray& buf);
bool write(QByteArray& data, QGeoCoordinate coordinate);
bool write(QByteArray& buf, GeoTagWorker::cameraFeedbackPacket& geotag);
};
#endif // EXIFPARSER_H
src/AnalyzeView/GeoTagController.cc
View file @ e8cce68c
......@@ -8,7 +8,6 @@
****************************************************************************/
#include "GeoTagController.h"
#include "ExifParser.h"
#include "QGCQFileDialog.h"
#include "QGCLoggingCategory.h"
#include "MainWindow.h"
......@@ -18,6 +17,10 @@
#include <QDebug>
#include <cfloat>
#include "ExifParser.h"
#include "ULogParser.h"
#include "PX4LogParser.h"
GeoTagController::GeoTagController(void)
: _progress(0)
, _inProgress(false)
......@@ -35,7 +38,7 @@ GeoTagController::~GeoTagController()
void GeoTagController::pickLogFile(void)
{
QString filename = QGCQFileDialog::getOpenFileName(MainWindow::instance(), "Select log file load", QString(), "PX4 log file (*.px4log);;All Files (*.*)");
QString filename = QGCQFileDialog::getOpenFileName(MainWindow::instance(), "Select log file load", QString(), "ULog file (*.ulg);;PX4 log file (*.px4log);;All Files (*.*)");
if (!filename.isEmpty()) {
_worker.setLogFile(filename);
emit logFileChanged(filename);
......@@ -173,7 +176,7 @@ void GeoTagWorker::run(void)
// Parse EXIF
ExifParser exifParser;
_tagTime.clear();
_imageTime.clear();
for (int i = 0; i < _imageList.size(); ++i) {
QFile file(_imageList.at(i).absoluteFilePath());
if (!file.open(QIODevice::ReadOnly)) {
......@@ -183,7 +186,7 @@ void GeoTagWorker::run(void)
QByteArray imageBuffer = file.readAll();
file.close();
_tagTime.append(exifParser.readTime(imageBuffer));
_imageTime.append(exifParser.readTime(imageBuffer));
emit progressChanged((100/nSteps) + ((100/nSteps) / _imageList.size())*i);
......@@ -194,10 +197,30 @@ void GeoTagWorker::run(void)
}
}
// Load PX4 log
_geoRef.clear();
_triggerTime.clear();
if (!parsePX4Log()) {
// Load log
bool isULog = _logFile.endsWith(".ulg", Qt::CaseSensitive);
QFile file(_logFile);
if (!file.open(QIODevice::ReadOnly)) {
emit error(tr("Geotagging failed. Couldn't open log file."));
return;
}
QByteArray log = file.readAll();
file.close();
// Instantiate appropriate parser
_triggerList.clear();
bool parseComplete = false;
if(isULog) {
ULogParser parser;
parseComplete = parser.getTagsFromLog(log, _triggerList);
} else {
PX4LogParser parser;
parseComplete = parser.getTagsFromLog(log, _triggerList);
}
if (!parseComplete) {
if (_cancel) {
qCDebug(GeotaggingLog) << "Tagging cancelled";
emit error(tr("Tagging cancelled"));
......@@ -210,7 +233,7 @@ void GeoTagWorker::run(void)
}
emit progressChanged(3*(100/nSteps));
qCDebug(GeotaggingLog) << "Found " << _geoRef.count() << " trigger logs.";
qCDebug(GeotaggingLog) << "Found " << _triggerList.count() << " trigger logs.";
if (_cancel) {
qCDebug(GeotaggingLog) << "Tagging cancelled";
......@@ -244,7 +267,7 @@ void GeoTagWorker::run(void)
QByteArray imageBuffer = fileRead.readAll();
fileRead.close();
if (!exifParser.write(imageBuffer, _geoRef[_triggerIndices[i]])) {
if (!exifParser.write(imageBuffer, _triggerList[_triggerIndices[i]])) {
emit error(tr("Geotagging failed. Couldn't write to image."));
return;
} else {
......@@ -279,108 +302,11 @@ void GeoTagWorker::run(void)
emit progressChanged(100);
}
bool GeoTagWorker::parsePX4Log()
{
// 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};
// load log
QFile file(_logFile);
if (!file.open(QIODevice::ReadOnly)) {
qCDebug(GeotaggingLog) << "Could not open log file " << _logFile;
return false;
}
QByteArray log = file.readAll();
file.close();
// 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;
QGeoCoordinate lastCoordinate;
while(index < log.count() - 1) {
if (_cancel) {
return false;
}
// 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;
}
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;
}
_triggerTime.append(timeDouble);
sequence = seqInt;
// second extract position
bool lookForGpos = true;
while (lookForGpos) {
if (_cancel) {
return false;
}
int gposIndex = log.indexOf(gposHeader, index + 1);
if (gposIndex < 0) {
_geoRef.append(lastCoordinate);
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());
double latitude = static_cast<double>(qFromLittleEndian(*lat))/1.0e7;
lastCoordinate.setLatitude(latitude);
int32_t* lon = reinterpret_cast<int32_t*>(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data());
double longitude = static_cast<double>(qFromLittleEndian(*lon))/1.0e7;
longitude = fmod(180.0 + longitude, 360.0) - 180.0;
lastCoordinate.setLongitude(longitude);
float* alt = reinterpret_cast<float*>(log.mid(gposIndex + gposOffsets[2], gposLengths[2]).data());
lastCoordinate.setAltitude(qFromLittleEndian(*alt));
_geoRef.append(lastCoordinate);
break;
}
}
}
return true;
}
bool GeoTagWorker::triggerFiltering()
{
_imageIndices.clear();
_triggerIndices.clear();
for(int i = 0; i < _tagTime.count() && i < _triggerTime.count(); i++) {
for(int i = 0; i < _imageTime.count() && i < _triggerList.count(); i++) {
_imageIndices.append(i);
_triggerIndices.append(i);
}
......
src/AnalyzeView/GeoTagController.h
View file @ e8cce68c
......@@ -38,6 +38,18 @@ public:
void cancelTagging (void) { _cancel = true; }
struct cameraFeedbackPacket {
double timestamp;
double timestampUTC;
uint32_t imageSequence;
double latitude;
double longitude;
float altitude;
float groundDistance;
float attitudeQuaternion[4];
uint8_t captureResult;
};
protected:
void run(void) final;
......@@ -47,7 +59,6 @@ signals:
void progressChanged (double progress);
private:
bool parsePX4Log();
bool triggerFiltering();
bool _cancel;
......@@ -55,11 +66,11 @@ private:
QString _imageDirectory;
QString _saveDirectory;
QFileInfoList _imageList;
QList<double> _tagTime;
QList<QGeoCoordinate> _geoRef;
QList<double> _triggerTime;
QList<double> _imageTime;
QList<cameraFeedbackPacket> _triggerList;
QList<int> _imageIndices;
QList<int> _triggerIndices;
};
/// Controller for GeoTagPage.qml. Supports geotagging images based on logfile camera tags.
......
src/AnalyzeView/PX4LogParser.cc 0 → 100644
View file @ e8cce68c
#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;
}
src/AnalyzeView/PX4LogParser.h 0 → 100644
View file @ e8cce68c
#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
src/AnalyzeView/ULogParser.cc 0 → 100644
View file @ e8cce68c
#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;
}
src/AnalyzeView/ULogParser.h 0 → 100644
View file @ e8cce68c
#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
src/Vehicle/Vehicle.cc
View file @ e8cce68c
......@@ -2406,7 +2406,9 @@ void Vehicle::triggerCamera(void)
MAV_CMD_DO_DIGICAM_CONTROL,
true, // show errors
0.0, 0.0, 0.0, 0.0, // param 1-4 unused
1.0); // trigger camera
1.0, // trigger camera
0.0, // param 6 unused
1.0); // test shot flag
}
const char* VehicleGPSFactGroup::_hdopFactName = "hdop";
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment