From 48acdca4f538e64f0677d03bbdfdfc4862026b54 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 28 Mar 2018 19:53:35 -0600 Subject: [PATCH 1/4] KML export: fix kmz export bug --- src/ui/Loghandling/LogAnalysis.cpp | 8 ++++++++ src/ui/Loghandling/LogExporter.cpp | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/ui/Loghandling/LogAnalysis.cpp b/src/ui/Loghandling/LogAnalysis.cpp index 6cc5457e5e..a4012c0c08 100644 --- a/src/ui/Loghandling/LogAnalysis.cpp +++ b/src/ui/Loghandling/LogAnalysis.cpp @@ -1192,6 +1192,14 @@ void LogAnalysis::exportDialogAccepted() QString outputFileName = dialog->selectedFiles().at(0); dialog->close(); + // force kml extension + int dotindex = outputFileName.indexOf("."); + if (dotindex < 0) { + outputFileName.append(".kml"); + } else { + outputFileName = outputFileName.left(dotindex).append(".kml"); + } + if(m_kmlExport) { QLOG_DEBUG() << "iconInterval: " << m_iconInterval; diff --git a/src/ui/Loghandling/LogExporter.cpp b/src/ui/Loghandling/LogExporter.cpp index 4710fc31ef..6316bdf355 100644 --- a/src/ui/Loghandling/LogExporter.cpp +++ b/src/ui/Loghandling/LogExporter.cpp @@ -189,7 +189,7 @@ void KmlLogExporter::writeLine(QString &line) void KmlLogExporter::endExport() { QString generated = m_kmlExporter.finish(true); - QString msg = QString("Successfull exported to %1.").arg(generated); + QString msg = QString("Successfully exported to %1.").arg(generated); QLOG_DEBUG() << msg; QMessageBox::information(0, "Export KML", msg); } From 86831a5e17c809faad7e788a05ccf0604e1c7d21 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 28 Mar 2018 19:54:39 -0600 Subject: [PATCH 2/4] KML export: add RC pattern maneuvers to KML output --- src/output/kmlcreator.cc | 202 +++++++++++++++++++++++++++++---------- src/output/kmlcreator.h | 54 ++++++++--- 2 files changed, 190 insertions(+), 66 deletions(-) diff --git a/src/output/kmlcreator.cc b/src/output/kmlcreator.cc index 0ec894e975..5de3767a24 100644 --- a/src/output/kmlcreator.cc +++ b/src/output/kmlcreator.cc @@ -318,6 +318,7 @@ QString SummaryData::summarize() { } KMLCreator::KMLCreator() : + m_maneuverData(), m_summary(new SummaryData()), m_newXKQ1(false), m_newNKQ1(false), @@ -325,9 +326,11 @@ KMLCreator::KMLCreator() : m_newATT(false), m_mav_type((MAV_TYPE)0) { +// ManeuverData m_maneuverData(); } KMLCreator::KMLCreator(MAV_TYPE mav_type, double iconInterval) : + m_maneuverData(), m_summary(new SummaryData()), m_newXKQ1(false), m_newNKQ1(false), @@ -388,20 +391,23 @@ void KMLCreator::processLine(QString &line) // POS, ATT, AHR2, NKQ1, and XKQ1 messages are all logged at 25Hz (by default). else if(line.indexOf("POS,") == 0 && (gpsOffset > 0)) { Placemark* pm = lastPlacemark(); + Attitude att; if(!pm) { QLOG_WARN() << "No placemark"; } else { // use last read attitude with highest priority: EKF3, EKF2, AHR2, ATT if (m_newXKQ1) { - Attitude att = attFromXKQ1(m_xkq1); + att = attFromXKQ1(m_xkq1); pm->addquat(att); } else if (m_newNKQ1) { - Attitude att = attFromNKQ1(m_nkq1); + att = attFromNKQ1(m_nkq1); pm->addquat(att); } + // not sure why AHR2 isn't checked here if (m_newATT) { + att = m_att; pm->add(m_att); } } @@ -417,6 +423,8 @@ void KMLCreator::processLine(QString &line) GPSRecord gps = gpsFromPOS(pos, gpsOffset, pm->mGPS); if(gps.hasData()) { + // add a pos/att pair to the ManeuverData object + m_maneuverData.add(gps, att); m_summary->add(gps); Placemark* pm = lastPlacemark(); @@ -550,44 +558,42 @@ QString KMLCreator::finish(bool kmz) { writer.writeEndElement(); // PolyStyle writer.writeEndElement(); // Style - writer.writeStartElement("Folder"); - writer.writeTextElement("name", "Flight Path"); - writer.writeTextElement("description", m_summary->summarize()); +// writer.writeStartElement("Folder"); +// writer.writeTextElement("name", "Flight Path"); +// writer.writeTextElement("description", m_summary->summarize()); - /* - * Flight path (complete) - */ - foreach(Placemark *pm, m_placemarks) { - writePathElement(writer, pm); - } +// /* +// * Flight path (complete) +// */ +// foreach(Placemark *pm, m_placemarks) { +// writePathElement(writer, pm); +// } - writer.writeEndElement(); // Folder +// writer.writeEndElement(); // Folder writer.writeStartElement("Folder"); - writer.writeTextElement("name", "Flight Path (segmented)"); + writer.writeTextElement("name", "Flight Path (maneuvers)"); writer.writeTextElement("description", m_summary->summarize()); /* - * Flight path (segmented) + * Flight path (segmented into aerobatic maneuvers) */ - foreach(Placemark *pm, m_placemarks) { - writeLogPlacemarkElement(writer, pm); - } + writeManeuversElement(writer, m_maneuverData); writer.writeEndElement(); // Folder - /* - * Planes element - */ - writer.writeStartElement("Folder"); - writer.writeTextElement("name", "Attitudes"); +// /* +// * Planes element +// */ +// writer.writeStartElement("Folder"); +// writer.writeTextElement("name", "Attitudes"); - int idx = 0; - foreach(Placemark *pm, m_placemarks) { - writePlanePlacemarkElement(writer, pm, idx); - } +// int idx = 0; +// foreach(Placemark *pm, m_placemarks) { +// writePlanePlacemarkElement(writer, pm, idx); +// } - writer.writeEndElement(); // Folder +// writer.writeEndElement(); // Folder /* * Planes element (quaternion) @@ -595,7 +601,7 @@ QString KMLCreator::finish(bool kmz) { writer.writeStartElement("Folder"); writer.writeTextElement("name", "EKFattitudes"); - idx = 0; + int idx = 0; foreach(Placemark *pm, m_placemarks) { writePlanePlacemarkElementQ(writer, pm, idx); } @@ -844,6 +850,7 @@ void KMLCreator::writePlanePlacemarkElementQ(QXmlStreamWriter &writer, Placemark GPSRecord gps = p->mPoints.at(0); float curLat = gps.lat().toFloat(); float curLng = gps.lng().toFloat(); + float curAlt = gps.alt().toFloat(); foreach(GPSRecord c, p->mPoints) { @@ -851,10 +858,16 @@ void KMLCreator::writePlanePlacemarkElementQ(QXmlStreamWriter &writer, Placemark float newLat = c.lat().toFloat(); float newLng = c.lng().toFloat(); - distance = 1000 * distanceBetween(curLat, curLng, newLat, newLng); + float newAlt = c.alt().toFloat(); + + distance = 1000 * distanceBetween(curLat, curLng, newLat, newLng); // meters + float altdiff = newAlt - curAlt; + distance = sqrt(distance*distance + altdiff*altdiff); + if (distance > m_iconInterval) { curLat = newLat; curLng = newLng; + curAlt = newAlt; QString dateTime = utc2KmlTimeStamp(c.getUtc_ms()); writer.writeStartElement("Placemark"); @@ -988,39 +1001,122 @@ void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, writer.writeEndElement(); // Placemark } -void KMLCreator::writeLogPlacemarkElement(QXmlStreamWriter &writer, Placemark *p) { - if(!p || p->mPoints.size()==0) { +void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, + QString& coords, QXmlStreamWriter& writer, QString& title, QString& color) { + + writer.writeStartElement("TimeSpan"); + + writer.writeTextElement("begin", utc2KmlTimeStamp(startUtc)); + writer.writeTextElement("end", utc2KmlTimeStamp(endUtc)); + writer.writeEndElement(); // TimeSpan + + writer.writeTextElement("name", title + ": " + QString::number(seq)); + writer.writeTextElement("description", utc2KmlTimeStamp(startUtc) + "\n" + utc2KmlTimeStamp(endUtc)); + writer.writeTextElement("styleUrl", "#yellowLineGreenPoly"); + + writer.writeStartElement("Style"); + writer.writeStartElement("LineStyle"); + writer.writeTextElement("color", color); + writer.writeTextElement("colorMode", "normal"); + writer.writeTextElement("width", "2"); + writer.writeEndElement(); // LineStyle + writer.writeEndElement(); // Style + + writer.writeStartElement("LineString"); + writer.writeTextElement("altitudeMode", "absolute"); + writer.writeTextElement("coordinates", coords); + writer.writeEndElement(); // LineString + + writer.writeEndElement(); // Placemark +} + +void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &md) { + if(md.mGPS.size()==0) { return; } - // for each 1000 milliseconds of data, create a Placemark representing that segment of the trajectory + // start a new Placemark at the middle of each straight and level (inverted or not) segment + // which is longer than x seconds. Name them maneuver N + + // This needs a 2-pass approach for simplicity: first pass + // construct a list of straight & level start and end indexes + struct SegSpec { + SegSpec(int b, int e, qint64 sutc, qint64 eutc, float d): begin(b), end(e), beginUtc(sutc), endUtc(eutc), duration(d) {} + int begin; + int end; + qint64 beginUtc; + qint64 endUtc; + qint64 duration; + }; + QList slSegments; + int slBegin = 0; + float avgRoll = 0; + float avgPitch = 0; + float d = 0.95; // single-pole IIR low pass filter + float rollThresh = 15.0f; // 15 degrees + float pitchThresh = 15.0f; // 15 degrees + bool sAndL = true; + int listIndex = 0; + foreach(Attitude att, md.mAttitudes) { + avgRoll += (1.0f - d) * (att.roll().toFloat() - avgRoll); + avgPitch += (1.0f - d) * (att.pitch().toFloat() - avgPitch); + bool slcheck = ((fabs(avgRoll) < rollThresh) || (fabs(avgRoll-3.1416f) < rollThresh)) && + (fabs(avgPitch) < pitchThresh); + if (sAndL) { + if (!slcheck) { + // vehicle is no longer straight and level + // check segment duration + qint64 beginUtc = md.mGPS.at(slBegin).getUtc_ms(); + qint64 endUtc = md.mGPS.at(listIndex).getUtc_ms(); + qint64 duration = endUtc - beginUtc; + // if segment is longer than 3 seconds + if (duration > 3000) { + // record this segment + slSegments.append(new SegSpec(slBegin, listIndex, beginUtc, endUtc, duration)); + } + sAndL = false; + } + } else { + if (slcheck) { + // vehicle is now straight and level + slBegin = listIndex; + sAndL = true; + } + } + listIndex++; + } + + // Since sAndL is init'ed to true, the first entry in slBegin will be 0 + // and the first entry in slEnd will occur shortly after takeoff. + // Thereafter, we should see pairs of entries in slBegin and slEnd + // which mark straight & level flight between maneuvers. QString coords("\n"); QString lastCoords; - qint64 startUtc=0, endUtc=0; - int seq=0; - foreach(GPSRecord c, p->mPoints) { - if (startUtc == 0) { - startUtc = c.getUtc_ms(); - // create first Placemark - writer.writeStartElement("Placemark"); + QList LevelSeg; + for (int segNum=0; segNum 0) { + begin = (slSegments.at(segNum)->begin + slSegments.at(segNum)->end) / 2; + } else { + begin = slSegments.at(segNum)->begin; } - if (endUtc >= startUtc+1000) { - // end current Placemark - endLogPlaceMark(seq++, startUtc, endUtc, coords, writer, p); - // leave the last set of coordinates in the buffer so that segments are contiguous - coords.clear(); + qint64 startUtc = md.mGPS.at(begin).getUtc_ms(); //slSegments.at(segNum)->startUtc; + + int end = (slSegments.at(segNum+1)->begin + slSegments.at(segNum+1)->end) / 2; + qint64 endUtc = md.mGPS.at(end).getUtc_ms(); //slSegments.at(segNum+1)->endUtc; + + // start a new Placemark + writer.writeStartElement("Placemark"); + QString title("Maneuver"); + QString color("FF00FF00"); + for (int i=begin; i<=end; i++) { + GPSRecord gpsRec = md.mGPS.at(i); + lastCoords = gpsRec.toStringForKml(); coords += lastCoords + "\n"; - - // start a new Placemark - startUtc = endUtc; - writer.writeStartElement("Placemark"); } - lastCoords = c.toStringForKml(); - coords += lastCoords + "\n"; - endUtc = c.getUtc_ms(); + endLogPlaceMark(segNum++, startUtc, endUtc, coords, writer, title, color); + coords.clear(); } - // end current Placemark - endLogPlaceMark(seq, startUtc, endUtc, coords, writer, p); } } // namespace kml diff --git a/src/output/kmlcreator.h b/src/output/kmlcreator.h index 61fc95916d..3000dee081 100644 --- a/src/output/kmlcreator.h +++ b/src/output/kmlcreator.h @@ -26,16 +26,16 @@ namespace kml { * @brief A GPS record from a log file. */ struct GPSRecord: DataLine { - QString hdop() { return values.value("HDop"); } - QString lat() { return values.value("Lat"); } - QString lng() { return values.value("Lng"); } - QString alt() { return values.value("Alt"); } - QString speed() { return values.value("Spd"); } - QString crs() { return values.value("GCrs"); } - QString vz() { return values.value("VZ"); } + QString hdop() const { return values.value("HDop"); } + QString lat() const { return values.value("Lat"); } + QString lng() const { return values.value("Lng"); } + QString alt() const { return values.value("Alt"); } + QString speed() const { return values.value("Spd"); } + QString crs() const { return values.value("GCrs"); } + QString vz() const { return values.value("VZ"); } // older logs have TimeMS instead of TimeUS; Also GMS->GPSTimeMS and GWk->Week - QString msec() { + QString msec() const { if (values.contains("GMS")) { return values.value("GMS"); } @@ -43,7 +43,7 @@ struct GPSRecord: DataLine { return values.value("GPSTimeMS"); } } - QString week() { + QString week() const { if (values.contains("GWk")) { return values.value("GWk"); } @@ -51,7 +51,7 @@ struct GPSRecord: DataLine { return values.value("Week"); } } - QString timeUS() { + QString timeUS() const { if (values.contains("TimeUS")) { return values.value("TimeUS"); } @@ -66,14 +66,14 @@ struct GPSRecord: DataLine { return status && (week > 0); } - QString toStringForKml() { + QString toStringForKml() const { QString str = QString("%1,%2,%3").arg(lng(), lat(), alt()); return str; } static GPSRecord from(FormatLine& format, QString& line); - qint64 getUtc_ms() { + qint64 getUtc_ms() const { // msec since start of week (max value is 2^29.17, so it just fits in a signed 32 bit int) int32_t week_ms = this->msec().toInt(); // weeks since 6 Jan 1980 @@ -281,6 +281,31 @@ struct CommandedWaypoint: DataLine { virtual ~CommandedWaypoint() {} }; +/** + * @brief A container of data for aerobatic maneuver logs in a KML file. + * First pass through logfile calls processLine which creates a list of GPSRecord + * and associated POS attitude records in this struct. + * and marks + * the beginning and end of all "straight and level" flight segments. + * Each maneuver consists of all records from beginning of S&L segN to + * the end of S&L segN+1. Takeoff and landing are special; from start of + * log to beginning of S&L seg0 and from last S&L seg to end of log, respectively. + * + */ +class ManeuverData { +public: + ManeuverData(){}; + ~ManeuverData(){}; + + void add(GPSRecord &p, Attitude &a) { + this->mGPS.append(p); + this->mAttitudes.append(a); + } + + QList mGPS; + QList mAttitudes; +}; + /** * @brief A container of data for creating Placemarks in a KML file. */ @@ -351,15 +376,18 @@ class KMLCreator { Placemark *lastPlacemark(); void writePathElement(QXmlStreamWriter &writer, Placemark *p); - void writeLogPlacemarkElement(QXmlStreamWriter &, Placemark *); + void writeManeuversElement(QXmlStreamWriter &, ManeuverData &); void writePlanePlacemarkElement(QXmlStreamWriter &, Placemark *, int &); void writePlanePlacemarkElementQ(QXmlStreamWriter &, Placemark *, int &); void writeWaypointsPlacemarkElement(QXmlStreamWriter &); void endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, QString& coords, QXmlStreamWriter& writer, Placemark* p); + void endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, + QString& coords, QXmlStreamWriter& writer, QString& title, QString& color); QString m_filename; QList m_placemarks; + ManeuverData m_maneuverData; QList m_waypoints; QHash m_formatLines; SummaryData* m_summary; From 40d33201e0f822325aad45a2005d942156ad4935 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 31 Mar 2018 14:17:19 -0600 Subject: [PATCH 3/4] KML export: add segmented maneuvers folder facilitating display of RPY, AoA, SSA --- src/output/kmlcreator.cc | 265 ++++++++++++++++++++++++++++----------- src/output/kmlcreator.h | 43 +++++-- 2 files changed, 225 insertions(+), 83 deletions(-) diff --git a/src/output/kmlcreator.cc b/src/output/kmlcreator.cc index 5de3767a24..cf0d64ecdf 100644 --- a/src/output/kmlcreator.cc +++ b/src/output/kmlcreator.cc @@ -153,6 +153,12 @@ Attitude Attitude::from(FormatLine &format, QString &line) { return a; } +AoaSsa AoaSsa::from(FormatLine &format, QString &line) { + AoaSsa a; + a.readFields(format, line); + return a; +} + AHR2 AHR2::from(FormatLine &format, QString &line) { AHR2 a; a.readFields(format, line); @@ -388,7 +394,7 @@ void KMLCreator::processLine(QString &line) } } } - // POS, ATT, AHR2, NKQ1, and XKQ1 messages are all logged at 25Hz (by default). + // POS, ATT, AHR2, AOA, NKQ1, and XKQ1 messages are all logged at 25Hz (by default). else if(line.indexOf("POS,") == 0 && (gpsOffset > 0)) { Placemark* pm = lastPlacemark(); Attitude att; @@ -424,7 +430,8 @@ void KMLCreator::processLine(QString &line) if(gps.hasData()) { // add a pos/att pair to the ManeuverData object - m_maneuverData.add(gps, att); + m_maneuverData.add(gps, att, m_aoa); + m_summary->add(gps); Placemark* pm = lastPlacemark(); @@ -483,6 +490,12 @@ void KMLCreator::processLine(QString &line) } } } + else if((line.indexOf("AOA,") == 0)) { + FormatLine fl = m_formatLines.value("AOA"); + if(fl.hasData()) { + m_aoa = AoaSsa::from(fl, line); + } + } else if(line.indexOf("CMD,") == 0) { FormatLine fl = m_formatLines.value("CMD"); if(fl.hasData()) { @@ -558,19 +571,6 @@ QString KMLCreator::finish(bool kmz) { writer.writeEndElement(); // PolyStyle writer.writeEndElement(); // Style -// writer.writeStartElement("Folder"); -// writer.writeTextElement("name", "Flight Path"); -// writer.writeTextElement("description", m_summary->summarize()); - -// /* -// * Flight path (complete) -// */ -// foreach(Placemark *pm, m_placemarks) { -// writePathElement(writer, pm); -// } - -// writer.writeEndElement(); // Folder - writer.writeStartElement("Folder"); writer.writeTextElement("name", "Flight Path (maneuvers)"); writer.writeTextElement("description", m_summary->summarize()); @@ -582,24 +582,23 @@ QString KMLCreator::finish(bool kmz) { writer.writeEndElement(); // Folder -// /* -// * Planes element -// */ -// writer.writeStartElement("Folder"); -// writer.writeTextElement("name", "Attitudes"); + writer.writeStartElement("Folder"); + writer.writeTextElement("name", "Flight Path (maneuver segments)"); + writer.writeTextElement("description", m_summary->summarize()); -// int idx = 0; -// foreach(Placemark *pm, m_placemarks) { -// writePlanePlacemarkElement(writer, pm, idx); -// } + /* + * Flight path (segmented into aerobatic maneuvers) + */ + writeManeuverSegments(writer, m_maneuverData); -// writer.writeEndElement(); // Folder + writer.writeEndElement(); // Folder /* * Planes element (quaternion) */ writer.writeStartElement("Folder"); writer.writeTextElement("name", "EKFattitudes"); + writer.writeTextElement("description", m_summary->summarize()); int idx = 0; foreach(Placemark *pm, m_placemarks) { @@ -690,6 +689,7 @@ void KMLCreator::writeWaypointsPlacemarkElement(QXmlStreamWriter &writer) { writer.writeStartElement("Placemark"); writer.writeTextElement("name", "Waypoints"); + writer.writeTextElement("visibility", "0"); writer.writeStartElement("Style"); writer.writeStartElement("LineStyle"); @@ -757,6 +757,25 @@ static QString RPYdescription(Attitude &att, GPSRecord &gps) { return s; } +static QString MDdescription(const Attitude &att, const AoaSsa &as, const GPSRecord &gps) { + QString s; + s.append(QString("RPY: %1, %2, %3\n") + .arg(att.roll().toFloat(),6,'f',1) + .arg(att.pitch().toFloat(),6,'f',1) + .arg(att.yaw().toFloat(),6,'f',1)); + + s.append(QString("AOA: %1, SSA: %2\n") + .arg(as.AOA().toFloat(),6,'f',1) + .arg(as.SSA().toFloat(),6,'f',1)); + + s.append(QString("Alt: %1\nSpeed: %2\nCourse: %3\nvZ: %4") + .arg(gps.alt().toFloat(),6,'f',1) + .arg(gps.speed().toFloat(),6,'f',1) + .arg(gps.crs().toFloat(),6,'f',1) + .arg(gps.vz().toFloat(),6,'f',1)); + return s; +} + QString utc2KmlTimeStamp(qint64 utc_msec) { QDateTime time = QDateTime::fromMSecsSinceEpoch(utc_msec); // Qt::ISODateWithMs is not defined? @@ -844,6 +863,10 @@ void KMLCreator::writePlanePlacemarkElementQ(QXmlStreamWriter &writer, Placemark } // generate placemarks for quaternion attitudes + + // Note that you apparently can't have both line and model aspects to a placemark, + // and it seems you can't click-select models like you can with lines + int index = 0; Attitude att; double distance; @@ -851,6 +874,8 @@ void KMLCreator::writePlanePlacemarkElementQ(QXmlStreamWriter &writer, Placemark float curLat = gps.lat().toFloat(); float curLng = gps.lng().toFloat(); float curAlt = gps.alt().toFloat(); +// QString lastCoords = p->mPoints.first().toStringForKml(); +// qint64 startUtc = p->mPoints.first().getUtc_ms(); foreach(GPSRecord c, p->mPoints) { @@ -869,9 +894,20 @@ void KMLCreator::writePlanePlacemarkElementQ(QXmlStreamWriter &writer, Placemark curLng = newLng; curAlt = newAlt; QString dateTime = utc2KmlTimeStamp(c.getUtc_ms()); +// QString curCoords = c.toStringForKml(); +// QString coords = lastCoords + "\n" + curCoords; +// lastCoords = curCoords; +// qint64 endUtc = c.getUtc_ms(); writer.writeStartElement("Placemark"); - writer.writeStartElement("TimeStamp"); +// writer.writeStartElement("TimeSpan"); +// writer.writeTextElement("begin", utc2KmlTimeStamp(startUtc)); +// writer.writeTextElement("end", utc2KmlTimeStamp(endUtc)); +// writer.writeEndElement(); // TimeSpan + +// startUtc = endUtc; + + writer.writeStartElement("TimeStamp"); writer.writeTextElement("when", utc2KmlTimeStamp(c.getUtc_ms())); writer.writeEndElement(); // TimeStamp @@ -879,7 +915,21 @@ void KMLCreator::writePlanePlacemarkElementQ(QXmlStreamWriter &writer, Placemark double ts_sec = (double)timeUS / 1e6; QString timeLabel = dateTime.mid(dateTime.indexOf('T')+1, 12); writer.writeTextElement("name", QString("%1: %2: %3: %4").arg(p->title).arg(idx++).arg(ts_sec,5,'f',3).arg(timeLabel)); - writer.writeTextElement("visibility", "0"); + writer.writeTextElement("visibility", "1"); + +// writer.writeTextElement("styleUrl", "#yellowLineGreenPoly"); +// writer.writeStartElement("Style"); +// writer.writeStartElement("LineStyle"); +// writer.writeTextElement("color", "FF00FF00"); +// writer.writeTextElement("colorMode", "normal"); +// writer.writeTextElement("width", "2"); +// writer.writeEndElement(); // LineStyle +// writer.writeEndElement(); // Style + +// writer.writeStartElement("LineString"); +// writer.writeTextElement("altitudeMode", "absolute"); +// writer.writeTextElement("coordinates", coords); +// writer.writeEndElement(); // LineString writer.writeStartElement("Model"); writer.writeTextElement("altitudeMode", "absolute"); @@ -971,37 +1021,7 @@ void KMLCreator::writePathElement(QXmlStreamWriter &writer, Placemark *p) { writer.writeEndElement(); // Placemark } -// end current Placemark -void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, - QString& coords, QXmlStreamWriter& writer, Placemark* p) { - - writer.writeStartElement("TimeSpan"); - - writer.writeTextElement("begin", utc2KmlTimeStamp(startUtc)); - writer.writeTextElement("end", utc2KmlTimeStamp(endUtc)); - writer.writeEndElement(); // TimeSpan - - writer.writeTextElement("name", p->title + ": " + QString::number(seq)); - writer.writeTextElement("description", utc2KmlTimeStamp(startUtc)); - writer.writeTextElement("styleUrl", "#yellowLineGreenPoly"); - - writer.writeStartElement("Style"); - writer.writeStartElement("LineStyle"); - writer.writeTextElement("color", p->color); - writer.writeTextElement("colorMode", "normal"); - writer.writeTextElement("width", "2"); - writer.writeEndElement(); // LineStyle - writer.writeEndElement(); // Style - - writer.writeStartElement("LineString"); - writer.writeTextElement("altitudeMode", "absolute"); - writer.writeTextElement("coordinates", coords); - writer.writeEndElement(); // LineString - - writer.writeEndElement(); // Placemark -} - -void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, +void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, QString desc, QString& coords, QXmlStreamWriter& writer, QString& title, QString& color) { writer.writeStartElement("TimeSpan"); @@ -1011,7 +1031,7 @@ void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, writer.writeEndElement(); // TimeSpan writer.writeTextElement("name", title + ": " + QString::number(seq)); - writer.writeTextElement("description", utc2KmlTimeStamp(startUtc) + "\n" + utc2KmlTimeStamp(endUtc)); + writer.writeTextElement("description", desc); writer.writeTextElement("styleUrl", "#yellowLineGreenPoly"); writer.writeStartElement("Style"); @@ -1030,7 +1050,7 @@ void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, writer.writeEndElement(); // Placemark } -void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &md) { +void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &md, float p_lp, float sl_dur) { if(md.mGPS.size()==0) { return; } @@ -1038,7 +1058,6 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m // start a new Placemark at the middle of each straight and level (inverted or not) segment // which is longer than x seconds. Name them maneuver N - // This needs a 2-pass approach for simplicity: first pass // construct a list of straight & level start and end indexes struct SegSpec { SegSpec(int b, int e, qint64 sutc, qint64 eutc, float d): begin(b), end(e), beginUtc(sutc), endUtc(eutc), duration(d) {} @@ -1052,7 +1071,7 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m int slBegin = 0; float avgRoll = 0; float avgPitch = 0; - float d = 0.95; // single-pole IIR low pass filter + float d = p_lp; // single-pole IIR low pass filter float rollThresh = 15.0f; // 15 degrees float pitchThresh = 15.0f; // 15 degrees bool sAndL = true; @@ -1070,7 +1089,7 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m qint64 endUtc = md.mGPS.at(listIndex).getUtc_ms(); qint64 duration = endUtc - beginUtc; // if segment is longer than 3 seconds - if (duration > 3000) { + if (duration > sl_dur) { // record this segment slSegments.append(new SegSpec(slBegin, listIndex, beginUtc, endUtc, duration)); } @@ -1086,6 +1105,9 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m listIndex++; } + QString title("Maneuver"); + QString color("FF00FF00"); + // Since sAndL is init'ed to true, the first entry in slBegin will be 0 // and the first entry in slEnd will occur shortly after takeoff. // Thereafter, we should see pairs of entries in slBegin and slEnd @@ -1093,30 +1115,133 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m QString coords("\n"); QString lastCoords; QList LevelSeg; - for (int segNum=0; segNum 0) { begin = (slSegments.at(segNum)->begin + slSegments.at(segNum)->end) / 2; } else { begin = slSegments.at(segNum)->begin; } - qint64 startUtc = md.mGPS.at(begin).getUtc_ms(); //slSegments.at(segNum)->startUtc; + qint64 startUtc = md.mGPS.at(begin).getUtc_ms(); int end = (slSegments.at(segNum+1)->begin + slSegments.at(segNum+1)->end) / 2; - qint64 endUtc = md.mGPS.at(end).getUtc_ms(); //slSegments.at(segNum+1)->endUtc; + qint64 endUtc = md.mGPS.at(end).getUtc_ms(); - // start a new Placemark writer.writeStartElement("Placemark"); - QString title("Maneuver"); - QString color("FF00FF00"); for (int i=begin; i<=end; i++) { GPSRecord gpsRec = md.mGPS.at(i); lastCoords = gpsRec.toStringForKml(); coords += lastCoords + "\n"; } - endLogPlaceMark(segNum++, startUtc, endUtc, coords, writer, title, color); + QString desc = utc2KmlTimeStamp(startUtc) + "\n" + utc2KmlTimeStamp(endUtc); + endLogPlaceMark(segNum, startUtc, endUtc, desc, coords, writer, title, color); coords.clear(); } } +void KMLCreator::writeManeuverSegments(QXmlStreamWriter &writer, ManeuverData &md, float p_lp, float sl_dur) { + if(md.mGPS.size()==0) { + return; + } + + // start a new Placemark at the middle of each straight and level (inverted or not) segment + // which is longer than x seconds. Name them maneuver N + + // construct a list of straight & level start and end indexes + struct SegSpec { + SegSpec(int b, int e, qint64 sutc, qint64 eutc, float d): begin(b), end(e), beginUtc(sutc), endUtc(eutc), duration(d) {} + int begin; + int end; + qint64 beginUtc; + qint64 endUtc; + qint64 duration; + }; + QList slSegments; + int slBegin = 0; + float avgRoll = 0; + float avgPitch = 0; + float d = p_lp; // single-pole IIR low pass filter + float rollThresh = 15.0f; // 15 degrees + float pitchThresh = 15.0f; // 15 degrees + bool sAndL = true; + int listIndex = 0; + foreach(Attitude att, md.mAttitudes) { + avgRoll += (1.0f - d) * (att.roll().toFloat() - avgRoll); + avgPitch += (1.0f - d) * (att.pitch().toFloat() - avgPitch); + bool slcheck = ((fabs(avgRoll) < rollThresh) || (fabs(avgRoll-3.1416f) < rollThresh)) && + (fabs(avgPitch) < pitchThresh); + if (sAndL) { + if (!slcheck) { + // vehicle is no longer straight and level + // check segment duration + qint64 beginUtc = md.mGPS.at(slBegin).getUtc_ms(); + qint64 endUtc = md.mGPS.at(listIndex).getUtc_ms(); + qint64 duration = endUtc - beginUtc; + // if segment is longer than 3 seconds + if (duration > sl_dur) { + // record this segment + slSegments.append(new SegSpec(slBegin, listIndex, beginUtc, endUtc, duration)); + } + sAndL = false; + } + } else { + if (slcheck) { + // vehicle is now straight and level + slBegin = listIndex; + sAndL = true; + } + } + listIndex++; + } + + // make a folder containing a placemark for each small segment of the flightpath + // each segment will be selectable with a description including RPY angles, AOA, SSA etc. + QString title("Maneuver_segments"); + QString color("FF00FF00"); + + // Since sAndL is init'ed to true, the first entry in slBegin will be 0 + // and the first entry in slEnd will occur shortly after takeoff. + // Thereafter, we should see pairs of entries in slBegin and slEnd + // which mark straight & level flight between maneuvers. + QString coords("\n"); + QString lastCoords; + QList LevelSeg; + for (int segNum=0; segNum 0) { + begin = (slSegments.at(segNum)->begin + slSegments.at(segNum)->end) / 2; + } else { + begin = slSegments.at(segNum)->begin; + } + qint64 startUtc = md.mGPS.at(begin).getUtc_ms(); + + int end = (slSegments.at(segNum+1)->begin + slSegments.at(segNum+1)->end) / 2; + qint64 endUtc = md.mGPS.at(end).getUtc_ms(); + + // start a new Folder + writer.writeStartElement("Folder"); + writer.writeTextElement("name", title + ": " + QString::number(segNum)); + writer.writeTextElement("description", utc2KmlTimeStamp(startUtc) + "\n" + utc2KmlTimeStamp(endUtc)); + + int i = begin; + int nrecs = 5; + while (i <= end-nrecs) { + startUtc = md.mGPS.at(i).getUtc_ms(); + writer.writeStartElement("Placemark"); + int jmax = std::min(i + nrecs, end); + for (int j=i; j 0); @@ -173,6 +173,22 @@ struct Attitude: DataLine { virtual ~Attitude() {} }; +/** + * @brief An AOA record from a log file. + */ +struct AoaSsa: DataLine { + QString AOA() const { return values.value("AOA"); } + QString SSA() const { return values.value("SSA"); } + + virtual bool hasData() { + return (values.value("AOA").length() > 0); + } + + static AoaSsa from(FormatLine& format, QString& line); + + virtual ~AoaSsa() {} +}; + /** * @brief An AHR2 record from a log file. */ @@ -297,13 +313,15 @@ class ManeuverData { ManeuverData(){}; ~ManeuverData(){}; - void add(GPSRecord &p, Attitude &a) { + void add(GPSRecord &p, Attitude &a, AoaSsa &as) { this->mGPS.append(p); this->mAttitudes.append(a); + this->mAS.append(as); } QList mGPS; QList mAttitudes; + QList mAS; }; /** @@ -376,14 +394,12 @@ class KMLCreator { Placemark *lastPlacemark(); void writePathElement(QXmlStreamWriter &writer, Placemark *p); - void writeManeuversElement(QXmlStreamWriter &, ManeuverData &); + void writeManeuversElement(QXmlStreamWriter &, ManeuverData &, float p_lp=0.95f, float sl_dur=3.0f); + void writeManeuverSegments(QXmlStreamWriter &, ManeuverData &, float p_lp=0.95f, float sl_dur=3.0f); void writePlanePlacemarkElement(QXmlStreamWriter &, Placemark *, int &); void writePlanePlacemarkElementQ(QXmlStreamWriter &, Placemark *, int &); void writeWaypointsPlacemarkElement(QXmlStreamWriter &); - void endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, - QString& coords, QXmlStreamWriter& writer, Placemark* p); - void endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, - QString& coords, QXmlStreamWriter& writer, QString& title, QString& color); + void endLogPlaceMark(int, qint64, qint64, QString, QString&, QXmlStreamWriter&, QString&, QString&); QString m_filename; QList m_placemarks; @@ -397,6 +413,7 @@ class KMLCreator { NKQ1 m_nkq1; AHR2 m_ahr2; Attitude m_att; + AoaSsa m_aoa; bool m_newXKQ1; bool m_newNKQ1; From 0e6d579fdf8795acd145debce680f7bd940402d9 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 18 Apr 2018 20:34:43 -0600 Subject: [PATCH 4/4] KML maneuvers export: refactor and partially fix truncation bug --- src/output/kmlcreator.cc | 98 +++++++++++++--------------------------- src/output/kmlcreator.h | 11 +++++ 2 files changed, 42 insertions(+), 67 deletions(-) diff --git a/src/output/kmlcreator.cc b/src/output/kmlcreator.cc index cf0d64ecdf..1a77367ac9 100644 --- a/src/output/kmlcreator.cc +++ b/src/output/kmlcreator.cc @@ -1050,23 +1050,8 @@ void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, QStrin writer.writeEndElement(); // Placemark } -void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &md, float p_lp, float sl_dur) { - if(md.mGPS.size()==0) { - return; - } - - // start a new Placemark at the middle of each straight and level (inverted or not) segment - // which is longer than x seconds. Name them maneuver N - - // construct a list of straight & level start and end indexes - struct SegSpec { - SegSpec(int b, int e, qint64 sutc, qint64 eutc, float d): begin(b), end(e), beginUtc(sutc), endUtc(eutc), duration(d) {} - int begin; - int end; - qint64 beginUtc; - qint64 endUtc; - qint64 duration; - }; +QList KMLCreator::seg_maneuvers(float sl_dur, float p_lp, ManeuverData &md) +{ QList slSegments; int slBegin = 0; float avgRoll = 0; @@ -1076,6 +1061,9 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m float pitchThresh = 15.0f; // 15 degrees bool sAndL = true; int listIndex = 0; + qint64 beginUtc; + qint64 endUtc; + qint64 duration; foreach(Attitude att, md.mAttitudes) { avgRoll += (1.0f - d) * (att.roll().toFloat() - avgRoll); avgPitch += (1.0f - d) * (att.pitch().toFloat() - avgPitch); @@ -1085,25 +1073,45 @@ void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &m if (!slcheck) { // vehicle is no longer straight and level // check segment duration - qint64 beginUtc = md.mGPS.at(slBegin).getUtc_ms(); - qint64 endUtc = md.mGPS.at(listIndex).getUtc_ms(); - qint64 duration = endUtc - beginUtc; + beginUtc = md.mGPS.at(slBegin).getUtc_ms(); + endUtc = md.mGPS.at(listIndex).getUtc_ms(); + duration = endUtc - beginUtc; // if segment is longer than 3 seconds if (duration > sl_dur) { - // record this segment + // record this segment and start a new one slSegments.append(new SegSpec(slBegin, listIndex, beginUtc, endUtc, duration)); } sAndL = false; } } else { if (slcheck) { - // vehicle is now straight and level + // vehicle is now straight and level; remember index into mAttitudes and mGPS slBegin = listIndex; sAndL = true; } } listIndex++; } + // record the last segment + listIndex--; + beginUtc = md.mGPS.at(slBegin).getUtc_ms(); + endUtc = md.mGPS.at(listIndex).getUtc_ms(); + duration = endUtc - beginUtc; + slSegments.append(new SegSpec(slBegin, listIndex, beginUtc, endUtc, duration)); + + return slSegments; +} + +void KMLCreator::writeManeuversElement(QXmlStreamWriter &writer, ManeuverData &md, float p_lp, float sl_dur) { + if(md.mGPS.size()==0) { + return; + } + + // start a new Placemark at the middle of each straight and level (inverted or not) segment + // which is longer than x seconds. Name them maneuver N + + // construct a list of straight & level start and end indexes + QList slSegments = seg_maneuvers(sl_dur, p_lp, md); QString title("Maneuver"); QString color("FF00FF00"); @@ -1148,51 +1156,7 @@ void KMLCreator::writeManeuverSegments(QXmlStreamWriter &writer, ManeuverData &m // which is longer than x seconds. Name them maneuver N // construct a list of straight & level start and end indexes - struct SegSpec { - SegSpec(int b, int e, qint64 sutc, qint64 eutc, float d): begin(b), end(e), beginUtc(sutc), endUtc(eutc), duration(d) {} - int begin; - int end; - qint64 beginUtc; - qint64 endUtc; - qint64 duration; - }; - QList slSegments; - int slBegin = 0; - float avgRoll = 0; - float avgPitch = 0; - float d = p_lp; // single-pole IIR low pass filter - float rollThresh = 15.0f; // 15 degrees - float pitchThresh = 15.0f; // 15 degrees - bool sAndL = true; - int listIndex = 0; - foreach(Attitude att, md.mAttitudes) { - avgRoll += (1.0f - d) * (att.roll().toFloat() - avgRoll); - avgPitch += (1.0f - d) * (att.pitch().toFloat() - avgPitch); - bool slcheck = ((fabs(avgRoll) < rollThresh) || (fabs(avgRoll-3.1416f) < rollThresh)) && - (fabs(avgPitch) < pitchThresh); - if (sAndL) { - if (!slcheck) { - // vehicle is no longer straight and level - // check segment duration - qint64 beginUtc = md.mGPS.at(slBegin).getUtc_ms(); - qint64 endUtc = md.mGPS.at(listIndex).getUtc_ms(); - qint64 duration = endUtc - beginUtc; - // if segment is longer than 3 seconds - if (duration > sl_dur) { - // record this segment - slSegments.append(new SegSpec(slBegin, listIndex, beginUtc, endUtc, duration)); - } - sAndL = false; - } - } else { - if (slcheck) { - // vehicle is now straight and level - slBegin = listIndex; - sAndL = true; - } - } - listIndex++; - } + QList slSegments = seg_maneuvers(sl_dur, p_lp, md); // make a folder containing a placemark for each small segment of the flightpath // each segment will be selectable with a description including RPY angles, AOA, SSA etc. diff --git a/src/output/kmlcreator.h b/src/output/kmlcreator.h index 00462835c4..0a25fa4837 100644 --- a/src/output/kmlcreator.h +++ b/src/output/kmlcreator.h @@ -324,6 +324,15 @@ class ManeuverData { QList mAS; }; +struct SegSpec { + SegSpec(int b, int e, qint64 sutc, qint64 eutc, float d): begin(b), end(e), beginUtc(sutc), endUtc(eutc), duration(d) {} + int begin; + int end; + qint64 beginUtc; + qint64 endUtc; + qint64 duration; +}; + /** * @brief A container of data for creating Placemarks in a KML file. */ @@ -422,6 +431,8 @@ class KMLCreator { MAV_TYPE m_mav_type; double m_iconInterval; + + QList seg_maneuvers(float sl_dur, float p_lp, ManeuverData &md); }; } // namespace kml