diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index bf438801aa3cddc51fad46640c5601057990b01f..8d2ea11d05f78266f1ccc17b69dce2d15f00418c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -340,7 +340,8 @@ HEADERS += \ src/AutoPilotPlugins/APM/APMAirframeLoader.h \ src/QmlControls/QGCImageProvider.h \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.h \ - src/PositionManager/PositionManager.h + src/PositionManager/PositionManager.h \ + src/AnalyzeView/ExifParser.h AndroidBuild { HEADERS += \ @@ -506,7 +507,8 @@ SOURCES += \ src/QmlControls/QGCImageProvider.cc \ src/QtLocationPlugin/QMLControl/QGCMapEngineManager.cc \ src/PositionManager/SimulatedPosition.cc \ - src/PositionManager/PositionManager.cpp + src/PositionManager/PositionManager.cpp \ + src/AnalyzeView/ExifParser.cc DebugBuild { SOURCES += \ diff --git a/src/AnalyzeView/ExifParser.cc b/src/AnalyzeView/ExifParser.cc new file mode 100644 index 0000000000000000000000000000000000000000..4e4087ffa029687a507d7568a57ce7cb53e3c562 --- /dev/null +++ b/src/AnalyzeView/ExifParser.cc @@ -0,0 +1,224 @@ +#include "ExifParser.h" +#include +#include +#include + +ExifParser::ExifParser() +{ + +} + +ExifParser::~ExifParser() +{ + +} + +double ExifParser::readTime(QByteArray& buf) +{ + QByteArray tiffHeader("\x49\x49\x2A", 3); + QByteArray createDateHeader("\x04\x90\x02", 3); + + // find header position + uint32_t tiffHeaderIndex = buf.indexOf(tiffHeader); + + // find creation date header index + uint32_t createDateHeaderIndex = buf.indexOf(createDateHeader); + + // extract size of date-time string, -1 accounting for null-termination + uint32_t* sizeString = reinterpret_cast(buf.mid(createDateHeaderIndex + 4, 4).data()); + uint32_t createDateStringSize = qFromLittleEndian(*sizeString) - 1; + + // extract location of date-time string + uint32_t* dataIndex = reinterpret_cast(buf.mid(createDateHeaderIndex + 8, 4).data()); + uint32_t createDateStringDataIndex = qFromLittleEndian(*dataIndex) + tiffHeaderIndex; + + // read out data of create date-time field + QString createDate = buf.mid(createDateStringDataIndex, createDateStringSize); + + QStringList createDateList = createDate.split(' '); + if (createDateList.count() < 2) { + qWarning() << "Could not decode creation time and date: " << createDateList; + return -1.0; + } + QStringList dateList = createDateList[0].split(':'); + if (dateList.count() < 3) { + qWarning() << "Could not decode creation date: " << dateList; + return -1.0; + } + QStringList timeList = createDateList[1].split(':'); + if (timeList.count() < 3) { + qWarning() << "Could not decode creation time: " << timeList; + return -1.0; + } + QDate date(dateList[0].toInt(), dateList[1].toInt(), dateList[2].toInt()); + QTime time(timeList[0].toInt(), timeList[1].toInt(), timeList[2].toInt()); + QDateTime tagTime(date, time); + return tagTime.toMSecsSinceEpoch()/1000.0; +} + +bool ExifParser::write(QByteArray &buf, QGeoCoordinate coordinate) +{ + QByteArray app1Header("\xff\xe1", 2); + uint32_t app1HeaderInd = buf.indexOf(app1Header); + uint16_t *conversionPointer = reinterpret_cast(buf.mid(app1HeaderInd + 2, 2).data()); + uint16_t app1Size = *conversionPointer; + uint16_t app1SizeEndian = qFromBigEndian(app1Size) + 0xa5; // change wrong endian + QByteArray tiffHeader("\x49\x49\x2A", 3); + uint32_t tiffHeaderInd = buf.indexOf(tiffHeader); + conversionPointer = reinterpret_cast(buf.mid(tiffHeaderInd + 8, 2).data()); + uint16_t numberOfTiffFields = *conversionPointer; + uint32_t nextIfdOffsetInd = tiffHeaderInd + 10 + 12 * (numberOfTiffFields); + conversionPointer = reinterpret_cast(buf.mid(nextIfdOffsetInd, 2).data()); + uint16_t nextIfdOffset = *conversionPointer; + + // Definition of usefull unions and structs + union char2uint32_u { + char c[4]; + uint32_t i; + }; + union char2uint16_u { + char c[2]; + uint16_t i; + }; + // This struct describes a standart field used in exif files + struct field_s { + uint16_t tagID; // Describes which information is added here, e.g. GPS Lat + uint16_t type; // Describes the data type, e.g. string, uint8_t,... + uint32_t size; // Describes the size + uint32_t content; // Either contains the information, or the offset to the exif header where the information is stored (if 32 bits is not enough) + }; + // This struct contains all the fields that we want to add to the image + struct fields_s { + field_s gpsVersion; + field_s gpsLatRef; + field_s gpsLat; + field_s gpsLonRef; + field_s gpsLon; + field_s gpsAltRef; + field_s gpsAlt; + field_s gpsMapDatum; + uint32_t finishedDataField; + }; + // These are the additional information that can not be put into a single uin32_t + struct extended_s { + uint32_t gpsLat[6]; + uint32_t gpsLon[6]; + uint32_t gpsAlt[2]; + char mapDatum[7];// = {0x57,0x47,0x53,0x2D,0x38,0x34,0x00}; + }; + // This struct contains all the information we want to add to the image + struct readable_s { + fields_s fields; + extended_s extendedData; + }; + + // This union is used because for writing the information we have to use a char array, but we still want the information to be available in a more descriptive way + union { + char c[0xa3]; + readable_s readable; + } gpsData; + + + char2uint32_u gpsIFDInd; + gpsIFDInd.i = nextIfdOffset; + + // this will stay constant + QByteArray gpsInfo("\x25\x88\x04\x00\x01\x00\x00\x00", 8); + gpsInfo.append(gpsIFDInd.c[0]); + gpsInfo.append(gpsIFDInd.c[1]); + gpsInfo.append(gpsIFDInd.c[2]); + gpsInfo.append(gpsIFDInd.c[3]); + + // filling values to gpsData + uint32_t gpsDataExtInd = gpsIFDInd.i + 2 + sizeof(fields_s); + + // Filling up the fields with the corresponding values + gpsData.readable.fields.gpsVersion.tagID = 0; + gpsData.readable.fields.gpsVersion.type = 1; + gpsData.readable.fields.gpsVersion.size = 4; + gpsData.readable.fields.gpsVersion.content = 2; + + 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.gpsLat.tagID = 2; + gpsData.readable.fields.gpsLat.type = 5; + gpsData.readable.fields.gpsLat.size = 3; + gpsData.readable.fields.gpsLat.content = gpsDataExtInd; + + 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.gpsLon.tagID = 4; + gpsData.readable.fields.gpsLon.type = 5; + gpsData.readable.fields.gpsLon.size = 3; + gpsData.readable.fields.gpsLon.content = gpsDataExtInd + 6 * 4; + + gpsData.readable.fields.gpsAltRef.tagID = 5; + gpsData.readable.fields.gpsAltRef.type = 2; + gpsData.readable.fields.gpsAltRef.size = 2; + gpsData.readable.fields.gpsAltRef.content = 0x00; + + gpsData.readable.fields.gpsAlt.tagID = 6; + gpsData.readable.fields.gpsAlt.type = 5; + gpsData.readable.fields.gpsAlt.size = 1; + gpsData.readable.fields.gpsAlt.content = gpsDataExtInd + 6 * 4 * 2; + + gpsData.readable.fields.gpsMapDatum.tagID = 18; + gpsData.readable.fields.gpsMapDatum.type = 2; + gpsData.readable.fields.gpsMapDatum.size = 7; + gpsData.readable.fields.gpsMapDatum.content = gpsDataExtInd + 6 * 4 * 2 + 2 * 4; + + 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(coordinate.latitude())); + gpsData.readable.extendedData.gpsLat[1] = 1; + gpsData.readable.extendedData.gpsLat[2] = static_cast((fabs(coordinate.latitude()) - floor(fabs(coordinate.latitude()))) * 60000.0); + gpsData.readable.extendedData.gpsLat[3] = 1000; + gpsData.readable.extendedData.gpsLat[4] = 0; + gpsData.readable.extendedData.gpsLat[5] = 1; + + gpsData.readable.extendedData.gpsLon[0] = abs(static_cast(coordinate.longitude())); + gpsData.readable.extendedData.gpsLon[1] = 1; + gpsData.readable.extendedData.gpsLon[2] = static_cast((fabs(coordinate.longitude()) - floor(fabs(coordinate.longitude()))) * 60000.0); + gpsData.readable.extendedData.gpsLon[3] = 1000; + gpsData.readable.extendedData.gpsLon[4] = 0; + gpsData.readable.extendedData.gpsLon[5] = 1; + + gpsData.readable.extendedData.gpsAlt[0] = coordinate.altitude() * 100; + gpsData.readable.extendedData.gpsAlt[1] = 100; + gpsData.readable.extendedData.mapDatum[0] = 'W'; + gpsData.readable.extendedData.mapDatum[1] = 'G'; + gpsData.readable.extendedData.mapDatum[2] = 'S'; + gpsData.readable.extendedData.mapDatum[3] = '-'; + gpsData.readable.extendedData.mapDatum[4] = '8'; + gpsData.readable.extendedData.mapDatum[5] = '4'; + gpsData.readable.extendedData.mapDatum[6] = 0x00; + + // remove 12 spaces from image description, as otherwise we need to loop through every field and correct the new address values + buf.remove(nextIfdOffsetInd + 4, 12); + // TODO correct size in image description + // insert Gps Info to image file + buf.insert(nextIfdOffsetInd, gpsInfo, 12); + char numberOfFields[2] = {0x08, 0x00}; + // insert number of gps specific fields that we want to add + buf.insert(gpsIFDInd.i + tiffHeaderInd, numberOfFields, 2); + // insert the gps data + buf.insert(gpsIFDInd.i + 2 + tiffHeaderInd, gpsData.c, 0xa3); + + // update the new file size and exif offsets + char2uint16_u converter; + converter.i = qToBigEndian(app1SizeEndian); + buf.replace(app1HeaderInd + 2, 2, converter.c, 2); + converter.i = nextIfdOffset + 12 + 0xa5; + buf.replace(nextIfdOffsetInd + 12, 2, converter.c, 2); + + converter.i = (numberOfTiffFields) + 1; + buf.replace(tiffHeaderInd + 8, 2, converter.c, 2); + return true; +} diff --git a/src/AnalyzeView/ExifParser.h b/src/AnalyzeView/ExifParser.h new file mode 100644 index 0000000000000000000000000000000000000000..9a29da3806d0132373df90e900c235efcba681a6 --- /dev/null +++ b/src/AnalyzeView/ExifParser.h @@ -0,0 +1,16 @@ +#ifndef EXIFPARSER_H +#define EXIFPARSER_H + +#include +#include + +class ExifParser +{ +public: + ExifParser(); + ~ExifParser(); + double readTime(QByteArray& buf); + bool write(QByteArray& data, QGeoCoordinate coordinate); +}; + +#endif // EXIFPARSER_H diff --git a/src/AnalyzeView/GeoTagController.cc b/src/AnalyzeView/GeoTagController.cc index f9314c31eb352b4e863c9f3645cd355a0cacd69c..5527888a587079160b5a1cae62fa560e16c09201 100644 --- a/src/AnalyzeView/GeoTagController.cc +++ b/src/AnalyzeView/GeoTagController.cc @@ -8,7 +8,14 @@ ****************************************************************************/ #include "GeoTagController.h" +#include "ExifParser.h" #include "QGCFileDialog.h" +#include "QGCLoggingCategory.h" +#include +#include +#include +#include +#include GeoTagController::GeoTagController(void) : _progress(0) @@ -43,10 +50,81 @@ void GeoTagController::pickImageDirectory(void) } } +void GeoTagController::pickSaveDirectory(void) +{ + QString dir = QGCFileDialog::getExistingDirectory(NULL, "Select save directory"); + if (!dir.isEmpty()) { + _worker.setSaveDirectory(dir); + emit saveDirectoryChanged(dir); + } +} + void GeoTagController::startTagging(void) { _errorMessage.clear(); emit errorMessageChanged(_errorMessage); + + QDir imageDirectory = QDir(_worker.imageDirectory()); + if(!imageDirectory.exists()) { + _errorMessage = tr("Cannot find the image directory"); + emit errorMessageChanged(_errorMessage); + return; + } + if(_worker.saveDirectory() == "") { + if(!imageDirectory.mkdir(_worker.imageDirectory() + "/TAGGED")) { + QMessageBox msgBox(QMessageBox::Question, + tr("Images have alreay been tagged."), + tr("The images have already been tagged. Do you want to replace the previously tagged images?"), + QMessageBox::Cancel); + msgBox.setWindowModality(Qt::ApplicationModal); + msgBox.addButton(tr("Replace"), QMessageBox::ActionRole); + if (msgBox.exec() == QMessageBox::Cancel) { + _errorMessage = tr("Images have already been tagged"); + emit errorMessageChanged(_errorMessage); + return; + } + QDir oldTaggedFolder = QDir(_worker.imageDirectory() + "/TAGGED"); + oldTaggedFolder.removeRecursively(); + if(!imageDirectory.mkdir(_worker.imageDirectory() + "/TAGGED")) { + _errorMessage = tr("Couldn't replace the previously tagged images"); + emit errorMessageChanged(_errorMessage); + return; + } + } + } else { + QDir saveDirectory = QDir(_worker.saveDirectory()); + if(!saveDirectory.exists()) { + _errorMessage = tr("Cannot find the save directory"); + emit errorMessageChanged(_errorMessage); + return; + } + saveDirectory.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable); + QStringList nameFilters; + nameFilters << "*.jpg" << "*.JPG"; + saveDirectory.setNameFilters(nameFilters); + QStringList imageList = saveDirectory.entryList(); + if(!imageList.isEmpty()) { + QMessageBox msgBox(QMessageBox::Question, + tr("Save folder not empty."), + tr("The save folder already contains images. Do you want to replace them?"), + QMessageBox::Cancel); + msgBox.setWindowModality(Qt::ApplicationModal); + msgBox.addButton(tr("Replace"), QMessageBox::ActionRole); + if (msgBox.exec() == QMessageBox::Cancel) { + _errorMessage = tr("Save folder not empty"); + emit errorMessageChanged(_errorMessage); + return; + } + foreach(QString dirFile, imageList) + { + if(!saveDirectory.remove(dirFile)) { + _errorMessage = tr("Couldn't replace the existing images"); + emit errorMessageChanged(_errorMessage); + return; + } + } + } + } _worker.start(); } @@ -64,6 +142,9 @@ void GeoTagController::_workerError(QString errorMessage) GeoTagWorker::GeoTagWorker(void) : _cancel(false) + , _logFile("") + , _imageDirectory("") + , _saveDirectory("") { } @@ -71,17 +152,237 @@ GeoTagWorker::GeoTagWorker(void) void GeoTagWorker::run(void) { _cancel = false; - emit progressChanged(0); + emit progressChanged(1); + double nSteps = 5; + + // Load Images + _imageList.clear(); + QDir imageDirectory = QDir(_imageDirectory); + imageDirectory.setFilter(QDir::Files | QDir::Readable | QDir::NoSymLinks | QDir::Writable); + imageDirectory.setSorting(QDir::Name); + QStringList nameFilters; + nameFilters << "*.jpg" << "*.JPG"; + imageDirectory.setNameFilters(nameFilters); + _imageList = imageDirectory.entryInfoList(); + if(_imageList.isEmpty()) { + emit error(tr("The image directory doesn't contain images, make sure your images are of the JPG format")); + return; + } + emit progressChanged((100/nSteps)); + + // Parse EXIF + ExifParser exifParser; + _tagTime.clear(); + for (int i = 0; i < _imageList.size(); ++i) { + QFile file(_imageList.at(i).absoluteFilePath()); + if (!file.open(QIODevice::ReadOnly)) { + emit error(tr("Geotagging failed. Couldn't open an image.")); + return; + } + QByteArray imageBuffer = file.readAll(); + file.close(); + + _tagTime.append(exifParser.readTime(imageBuffer)); + + emit progressChanged((100/nSteps) + ((100/nSteps) / _imageList.size())*i); + + if (_cancel) { + qCDebug(GeotaggingLog) << "Tagging cancelled"; + emit error(tr("Tagging cancelled")); + return; + } + } + + // Load PX4 log + _geoRef.clear(); + _triggerTime.clear(); + if (!parsePX4Log()) { + if (_cancel) { + qCDebug(GeotaggingLog) << "Tagging cancelled"; + emit error(tr("Tagging cancelled")); + return; + } else { + qCDebug(GeotaggingLog) << "Log parsing failed"; + emit error(tr("Log parsing failed - tagging cancelled")); + return; + } + } + emit progressChanged(3*(100/nSteps)); + + qCDebug(GeotaggingLog) << "Found " << _geoRef.count() << " trigger logs."; + + if (_cancel) { + qCDebug(GeotaggingLog) << "Tagging cancelled"; + emit error(tr("Tagging cancelled")); + return; + } + + // Filter Trigger + if (!triggerFiltering()) { + qCDebug(GeotaggingLog) << "Geotagging failed in trigger filtering"; + emit error(tr("Geotagging failed in trigger filtering")); + return; + } + emit progressChanged(4*(100/nSteps)); + + if (_cancel) { + qCDebug(GeotaggingLog) << "Tagging cancelled"; + emit error(tr("Tagging cancelled")); + return; + } + + // Tag images + int maxIndex = std::min(_imageIndices.count(), _triggerIndices.count()); + maxIndex = std::min(maxIndex, _imageList.count()); + for(int i = 0; i < maxIndex; i++) { + QFile fileRead(_imageList.at(_imageIndices[i]).absoluteFilePath()); + if (!fileRead.open(QIODevice::ReadOnly)) { + emit error(tr("Geotagging failed. Couldn't open an image.")); + return; + } + QByteArray imageBuffer = fileRead.readAll(); + fileRead.close(); + + if (!exifParser.write(imageBuffer, _geoRef[_triggerIndices[i]])) { + emit error(tr("Geotagging failed. Couldn't write to image.")); + return; + } else { + QFile fileWrite; + if(_saveDirectory == "") { + fileWrite.setFileName(_imageDirectory + "/TAGGED/" + _imageList.at(_imageIndices[i]).fileName()); + } else { + fileWrite.setFileName(_saveDirectory + "/" + _imageList.at(_imageIndices[i]).fileName()); + } + if (!fileWrite.open(QFile::WriteOnly)) { + emit error(tr("Geotagging failed. Couldn't write to an image.")); + return; + } + fileWrite.write(imageBuffer); + fileWrite.close(); + } + emit progressChanged(4*(100/nSteps) + ((100/nSteps) / maxIndex)*i); - for (int i=0; i<10;i++) { if (_cancel) { + qCDebug(GeotaggingLog) << "Tagging cancelled"; emit error(tr("Tagging cancelled")); return; } - emit progressChanged(i*10); - sleep(1); + } + + if (_cancel) { + qCDebug(GeotaggingLog) << "Tagging cancelled"; + emit error(tr("Tagging cancelled")); + return; } emit progressChanged(100); - emit taggingComplete(); +} + +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(log.mid(log.indexOf(gposHeaderHeader) + 4, 1).data()); + gposHeaderOffset = static_cast(qFromLittleEndian(*iptr)); + iptr = reinterpret_cast(log.mid(log.indexOf(triggerHeaderHeader) + 4, 1).data()); + triggerHeaderOffset = static_cast(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(log.mid(index + triggerOffsets[0], triggerLengths[0]).data()); + double timeDouble = static_cast(qFromLittleEndian(*time)) / 1.0e6; + uint32_t* seq = reinterpret_cast(log.mid(index + triggerOffsets[1], triggerLengths[1]).data()); + int seqInt = static_cast(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(log.mid(gposIndex + gposOffsets[0], gposLengths[0]).data()); + double latitude = static_cast(qFromLittleEndian(*lat))/1.0e7; + lastCoordinate.setLatitude(latitude); + int32_t* lon = reinterpret_cast(log.mid(gposIndex + gposOffsets[1], gposLengths[1]).data()); + double longitude = static_cast(qFromLittleEndian(*lon))/1.0e7; + longitude = fmod(180.0 + longitude, 360.0) - 180.0; + lastCoordinate.setLongitude(longitude); + float* alt = reinterpret_cast(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++) { + _imageIndices.append(i); + _triggerIndices.append(i); + } + + return true; } diff --git a/src/AnalyzeView/GeoTagController.h b/src/AnalyzeView/GeoTagController.h index a2820fa35fc697bdc1c2587d0d535c7222bd7a32..a9c67f68cc110cda6c22fd271c1684386d77cd9e 100644 --- a/src/AnalyzeView/GeoTagController.h +++ b/src/AnalyzeView/GeoTagController.h @@ -10,9 +10,16 @@ #ifndef GeoTagController_H #define GeoTagController_H +#include "QmlObjectListModel.h" +#include "Fact.h" +#include "FactMetaData.h" #include #include #include +#include +#include +#include +#include class GeoTagWorker : public QThread { @@ -21,26 +28,38 @@ class GeoTagWorker : public QThread public: GeoTagWorker(void); - QString logFile(void) const { return _logFile; } - QString imageDirectory(void) const { return _imageDirectory; } + void setLogFile (const QString& logFile) { _logFile = logFile; } + void setImageDirectory (const QString& imageDirectory) { _imageDirectory = imageDirectory; } + void setSaveDirectory (const QString& saveDirectory) { _saveDirectory = saveDirectory; } - void setLogFile(const QString& logFile) { _logFile = logFile; } - void setImageDirectory(const QString& imageDirectory) { _imageDirectory = imageDirectory; } + QString logFile (void) const { return _logFile; } + QString imageDirectory (void) const { return _imageDirectory; } + QString saveDirectory (void) const { return _saveDirectory; } - void cancellTagging(void) { _cancel = true; } + void cancelTagging (void) { _cancel = true; } protected: void run(void) final; signals: - void error(QString errorMsg); - void taggingComplete(void); - void progressChanged(double progress); + void error (QString errorMsg); + void taggingComplete (void); + void progressChanged (double progress); private: - bool _cancel; - QString _logFile; - QString _imageDirectory; + bool parsePX4Log(); + bool triggerFiltering(); + + bool _cancel; + QString _logFile; + QString _imageDirectory; + QString _saveDirectory; + QFileInfoList _imageList; + QList _tagTime; + QList _geoRef; + QList _triggerTime; + QList _imageIndices; + QList _triggerIndices; }; /// Controller for GeoTagPage.qml. Supports geotagging images based on logfile camera tags. @@ -54,6 +73,7 @@ public: Q_PROPERTY(QString logFile READ logFile NOTIFY logFileChanged) Q_PROPERTY(QString imageDirectory READ imageDirectory NOTIFY imageDirectoryChanged) + Q_PROPERTY(QString saveDirectory READ saveDirectory NOTIFY saveDirectoryChanged) /// Set to an error message is geotagging fails Q_PROPERTY(QString errorMessage READ errorMessage NOTIFY errorMessageChanged) @@ -66,32 +86,35 @@ public: Q_INVOKABLE void pickLogFile(void); Q_INVOKABLE void pickImageDirectory(void); + Q_INVOKABLE void pickSaveDirectory(void); Q_INVOKABLE void startTagging(void); - Q_INVOKABLE void cancelTagging(void) { _worker.cancellTagging(); } + Q_INVOKABLE void cancelTagging(void) { _worker.cancelTagging(); } - QString logFile (void) const { return _worker.logFile(); } - QString imageDirectory (void) const { return _worker.imageDirectory(); } - double progress (void) const { return _progress; } - bool inProgress (void) const { return _worker.isRunning(); } - QString errorMessage (void) const { return _errorMessage; } + QString logFile (void) const { return _worker.logFile(); } + QString imageDirectory (void) const { return _worker.imageDirectory(); } + QString saveDirectory (void) const { return _worker.saveDirectory(); } + double progress (void) const { return _progress; } + bool inProgress (void) const { return _worker.isRunning(); } + QString errorMessage (void) const { return _errorMessage; } signals: - void logFileChanged(QString logFile); - void imageDirectoryChanged(QString imageDirectory); - void progressChanged(double progress); - void inProgressChanged(void); - void errorMessageChanged(QString errorMessage); + void logFileChanged (QString logFile); + void imageDirectoryChanged (QString imageDirectory); + void saveDirectoryChanged (QString saveDirectory); + void progressChanged (double progress); + void inProgressChanged (void); + void errorMessageChanged (QString errorMessage); private slots: void _workerProgressChanged(double progress); void _workerError(QString errorMsg); private: - QString _errorMessage; - double _progress; - bool _inProgress; + QString _errorMessage; + double _progress; + bool _inProgress; - GeoTagWorker _worker; + GeoTagWorker _worker; }; #endif diff --git a/src/AnalyzeView/GeoTagPage.qml b/src/AnalyzeView/GeoTagPage.qml index a7d84071082f4bb91ade83f6e2bafbecc1aaf425..a4881a7f8bbb36433a770ca87ea600a66b16165b 100644 --- a/src/AnalyzeView/GeoTagPage.qml +++ b/src/AnalyzeView/GeoTagPage.qml @@ -11,7 +11,10 @@ import QtQuick 2.5 import QtQuick.Controls 1.4 import QtQuick.Dialogs 1.2 +import QGroundControl 1.0 import QGroundControl.Palette 1.0 +import QGroundControl.FactSystem 1.0 +import QGroundControl.FactControls 1.0 import QGroundControl.Controls 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controllers 1.0 @@ -19,17 +22,17 @@ import QGroundControl.Controllers 1.0 AnalyzePage { id: geoTagPage pageComponent: pageComponent - pageName: qsTr("GeoTag Images (WIP)") - pageDescription: qsTr("GetTag Images is used to tag a set of images from a survey mission with gps coordinates. You must provide the binary log from the flight as well as the directory which contains the images to tag.") + pageName: qsTr("GeoTag Images") + pageDescription: qsTr("GeoTag Images is used to tag a set of images from a survey mission with gps coordinates. You must provide the binary log from the flight as well as the directory which contains the images to tag.") - property real _margin: ScreenTools.defaultFontPixelWidth + property real _margin: ScreenTools.defaultFontPixelWidth * 2 GeoTagController { id: controller } Component { - id: pageComponent + id: pageComponent Column { id: mainColumn @@ -37,67 +40,104 @@ AnalyzePage { spacing: _margin Row { - spacing: _margin + spacing: ScreenTools.defaultFontPixelWidth * 2 - QGCLabel { - text: "Log file:" + ProgressBar { + id: progressBar + width: qgcView.width -_margin * 5 + maximumValue: 100 + value: controller.progress } - QGCLabel { - text: controller.logFile + BusyIndicator { + running: controller.progress > 0 && controller.progress < 100 && controller.errorMessage === "" + width: progressBar.height + height: progressBar.height } + } + + QGCLabel { + text: controller.errorMessage + font.bold: true + font.pointSize: ScreenTools.largeFontPointSize + color: "red" + } + + // Horizontal spacer line + Rectangle { + height: 1 + width: qgcView.width * 1.0 + color: qgcPal.windowShadeDark + anchors.horizontalCenter: parent.horizontalCenter + } + + Row { + spacing: _margin QGCButton { text: qsTr("Select log file") + width: ScreenTools.defaultFontPixelWidth * 30 onClicked: controller.pickLogFile() + anchors.verticalCenter: parent.verticalCenter + } + + QGCLabel { + text: controller.logFile + anchors.verticalCenter: parent.verticalCenter } } Row { spacing: _margin - QGCLabel { - text: "Image directory:" + QGCButton { + text: qsTr("Select image directory") + width: ScreenTools.defaultFontPixelWidth * 30 + onClicked: controller.pickImageDirectory() + anchors.verticalCenter: parent.verticalCenter } QGCLabel { text: controller.imageDirectory + anchors.verticalCenter: parent.verticalCenter } + } + + Row { + spacing: _margin QGCButton { - text: qsTr("Select image directory") - onClicked: controller.pickImageDirectory() + text: qsTr("(Optionally) Select save directory") + width: ScreenTools.defaultFontPixelWidth * 30 + onClicked: controller.pickSaveDirectory() + anchors.verticalCenter: parent.verticalCenter + } + + QGCLabel { + text: controller.saveDirectory != "" ? controller.saveDirectory : "/TAGGED folder in your image folder" + anchors.verticalCenter: parent.verticalCenter } } - QGCLabel { text: "NYI - Simulated only" } + // Horizontal spacer line + Rectangle { + height: 1 + width: qgcView.width * 1.0 + color: qgcPal.windowShadeDark + anchors.horizontalCenter: parent.horizontalCenter + } QGCButton { text: controller.inProgress ? qsTr("Cancel Tagging") : qsTr("Start Tagging") - + width: ScreenTools.defaultFontPixelWidth * 30 onClicked: { if (controller.inProgress) { controller.cancelTagging() } else { - if (controller.logFile == "" || controller.imageDirectory == "") { - geoTagPage.showMessage(qsTr("Error"), qsTr("You must select a log file and image directory before you can start tagging."), StandardButton.Ok) - return - } controller.startTagging() } } } - - QGCLabel { - text: controller.errorMessage - } - - ProgressBar { - anchors.left: parent.left - anchors.right: parent.right - maximumValue: 100 - value: controller.progress - } } // Column } // Component } // AnalyzePage diff --git a/src/QGCLoggingCategory.cc b/src/QGCLoggingCategory.cc index f83818fb817be0263f5bb9f724cbe71a7d42593c..b2220c18a7c75932643538b350f4d1308c5da290 100644 --- a/src/QGCLoggingCategory.cc +++ b/src/QGCLoggingCategory.cc @@ -20,7 +20,8 @@ QGC_LOGGING_CATEGORY(FirmwareUpgradeLog, "FirmwareUpgradeLog") QGC_LOGGING_CATEGORY(FirmwareUpgradeVerboseLog, "FirmwareUpgradeVerboseLog") QGC_LOGGING_CATEGORY(MissionCommandsLog, "MissionCommandsLog") QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") -QGC_LOGGING_CATEGORY(ParameterManagerLog, "ParameterManagerLog") +QGC_LOGGING_CATEGORY(ParameterManagerLog, "ParameterManagerLog") +QGC_LOGGING_CATEGORY(GeotaggingLog, "GeotaggingLog") QGCLoggingCategoryRegister* _instance = NULL; const char* QGCLoggingCategoryRegister::_filterRulesSettingsGroup = "LoggingFilters"; diff --git a/src/QGCLoggingCategory.h b/src/QGCLoggingCategory.h index 9dad9bbccc14bac4114c9008869a366b4e3d6f7e..13dfcb1996b3f9006c96d0500dc16fce9262df48 100644 --- a/src/QGCLoggingCategory.h +++ b/src/QGCLoggingCategory.h @@ -23,6 +23,7 @@ Q_DECLARE_LOGGING_CATEGORY(FirmwareUpgradeVerboseLog) Q_DECLARE_LOGGING_CATEGORY(MissionCommandsLog) Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerLog) +Q_DECLARE_LOGGING_CATEGORY(GeotaggingLog) /// @def QGC_LOGGING_CATEGORY /// This is a QGC specific replacement for Q_LOGGING_CATEGORY. It will register the category name into a