diff --git a/src/output/kmlcreator.cc b/src/output/kmlcreator.cc index 0ec894e975..1a77367ac9 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); @@ -318,6 +324,7 @@ QString SummaryData::summarize() { } KMLCreator::KMLCreator() : + m_maneuverData(), m_summary(new SummaryData()), m_newXKQ1(false), m_newNKQ1(false), @@ -325,9 +332,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), @@ -385,23 +394,26 @@ 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; 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 +429,9 @@ 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_aoa); + m_summary->add(gps); Placemark* pm = lastPlacemark(); @@ -475,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()) { @@ -551,41 +572,24 @@ QString KMLCreator::finish(bool kmz) { writer.writeEndElement(); // Style writer.writeStartElement("Folder"); - writer.writeTextElement("name", "Flight Path"); + writer.writeTextElement("name", "Flight Path (maneuvers)"); writer.writeTextElement("description", m_summary->summarize()); /* - * Flight path (complete) + * Flight path (segmented into aerobatic maneuvers) */ - foreach(Placemark *pm, m_placemarks) { - writePathElement(writer, pm); - } + writeManeuversElement(writer, m_maneuverData); writer.writeEndElement(); // Folder writer.writeStartElement("Folder"); - writer.writeTextElement("name", "Flight Path (segmented)"); + writer.writeTextElement("name", "Flight Path (maneuver segments)"); writer.writeTextElement("description", m_summary->summarize()); /* - * Flight path (segmented) + * Flight path (segmented into aerobatic maneuvers) */ - foreach(Placemark *pm, m_placemarks) { - writeLogPlacemarkElement(writer, pm); - } - - writer.writeEndElement(); // Folder - - /* - * Planes element - */ - writer.writeStartElement("Folder"); - writer.writeTextElement("name", "Attitudes"); - - int idx = 0; - foreach(Placemark *pm, m_placemarks) { - writePlanePlacemarkElement(writer, pm, idx); - } + writeManeuverSegments(writer, m_maneuverData); writer.writeEndElement(); // Folder @@ -594,8 +598,9 @@ QString KMLCreator::finish(bool kmz) { */ writer.writeStartElement("Folder"); writer.writeTextElement("name", "EKFattitudes"); + writer.writeTextElement("description", m_summary->summarize()); - idx = 0; + int idx = 0; foreach(Placemark *pm, m_placemarks) { writePlanePlacemarkElementQ(writer, pm, idx); } @@ -684,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"); @@ -751,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? @@ -838,12 +863,19 @@ 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; GPSRecord gps = p->mPoints.at(0); 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) { @@ -851,14 +883,31 @@ 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()); +// 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 @@ -866,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"); @@ -958,9 +1021,8 @@ 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) { +void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, QString desc, + QString& coords, QXmlStreamWriter& writer, QString& title, QString& color) { writer.writeStartElement("TimeSpan"); @@ -968,13 +1030,13 @@ void KMLCreator::endLogPlaceMark(int seq, qint64 startUtc, qint64 endUtc, writer.writeTextElement("end", utc2KmlTimeStamp(endUtc)); writer.writeEndElement(); // TimeSpan - writer.writeTextElement("name", p->title + ": " + QString::number(seq)); - writer.writeTextElement("description", utc2KmlTimeStamp(startUtc)); + writer.writeTextElement("name", title + ": " + QString::number(seq)); + writer.writeTextElement("description", desc); writer.writeTextElement("styleUrl", "#yellowLineGreenPoly"); writer.writeStartElement("Style"); writer.writeStartElement("LineStyle"); - writer.writeTextElement("color", p->color); + writer.writeTextElement("color", color); writer.writeTextElement("colorMode", "normal"); writer.writeTextElement("width", "2"); writer.writeEndElement(); // LineStyle @@ -988,39 +1050,162 @@ 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) { +QList KMLCreator::seg_maneuvers(float sl_dur, float p_lp, ManeuverData &md) +{ + 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; + 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); + 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 + 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 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; 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; } - // 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 + + // 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"); + + // 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(); + + int end = (slSegments.at(segNum+1)->begin + slSegments.at(segNum+1)->end) / 2; + qint64 endUtc = md.mGPS.at(end).getUtc_ms(); + + writer.writeStartElement("Placemark"); + for (int i=begin; i<=end; i++) { + GPSRecord gpsRec = md.mGPS.at(i); + lastCoords = gpsRec.toStringForKml(); coords += lastCoords + "\n"; + } + 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 - // start a new Placemark - startUtc = endUtc; + // construct a list of straight & level start and end indexes + 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. + 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; jGPSTimeMS 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 @@ -156,13 +156,13 @@ struct POSRecord: DataLine { * @brief An ATT record from a log file. */ struct Attitude: DataLine { - QString rollIn() { return values.value("RollIn").isEmpty() ? values.value("DesRoll") : values.value("RollIn"); } - QString roll() { return values.value("Roll"); } - QString pitchIn() { return values.value("PitchIn").isEmpty() ? values.value("DesPitch") : values.value("PitchIn"); } - QString pitch() { return values.value("Pitch"); } - QString yawIn() { return values.value("YawIn").isEmpty() ? values.value("DesYaw") : values.value("YawIn"); } - QString yaw() { return values.value("Yaw"); } - QString navYaw() { return values.value("NavYaw"); } + QString rollIn() const { return values.value("RollIn").isEmpty() ? values.value("DesRoll") : values.value("RollIn"); } + QString roll() const { return values.value("Roll"); } + QString pitchIn() const { return values.value("PitchIn").isEmpty() ? values.value("DesPitch") : values.value("PitchIn"); } + QString pitch() const { return values.value("Pitch"); } + QString yawIn() const { return values.value("YawIn").isEmpty() ? values.value("DesYaw") : values.value("YawIn"); } + QString yaw() const { return values.value("Yaw"); } + QString navYaw() const { return values.value("NavYaw"); } virtual bool hasData() { return (values.value("Roll").length() > 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. */ @@ -281,6 +297,42 @@ 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, AoaSsa &as) { + this->mGPS.append(p); + this->mAttitudes.append(a); + this->mAS.append(as); + } + + QList mGPS; + QList mAttitudes; + 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. */ @@ -351,15 +403,16 @@ class KMLCreator { Placemark *lastPlacemark(); void writePathElement(QXmlStreamWriter &writer, Placemark *p); - void writeLogPlacemarkElement(QXmlStreamWriter &, Placemark *); + 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, qint64, qint64, QString, QString&, QXmlStreamWriter&, QString&, QString&); QString m_filename; QList m_placemarks; + ManeuverData m_maneuverData; QList m_waypoints; QHash m_formatLines; SummaryData* m_summary; @@ -369,6 +422,7 @@ class KMLCreator { NKQ1 m_nkq1; AHR2 m_ahr2; Attitude m_att; + AoaSsa m_aoa; bool m_newXKQ1; bool m_newNKQ1; @@ -377,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 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); }