QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
LogViewerDataFlashParser.cc
Go to the documentation of this file.
1#include "LogViewerDataFlashParser.h"
2
3#include "APMDataFlashUtility.h"
4
5#include <QtCore/QByteArray>
6#include <QtCore/QCoreApplication>
7#include <QtCore/QFile>
8#include <QtCore/QHash>
9#include <QtCore/QSet>
10#include <QtCore/QVariantMap>
11
12#include <algorithm>
13#include <limits>
14
15namespace {
16
17QString _vehicleTypeFromMessageText(const QString &messageText)
18{
19 const QString text = messageText.toLower();
20 if (text.contains(QStringLiteral("arducopter"))) { return QStringLiteral("ArduCopter"); }
21 if (text.contains(QStringLiteral("arduplane"))) { return QStringLiteral("ArduPlane"); }
22 if (text.contains(QStringLiteral("ardurover"))) { return QStringLiteral("ArduRover"); }
23 if (text.contains(QStringLiteral("ardusub"))) { return QStringLiteral("ArduSub"); }
24 return QString();
25}
26
27QString _ardupilotModeName(const QString &vehicleType, int modeNumber)
28{
29 static const QHash<int, QString> planeModes = {
30 {0,QStringLiteral("Manual")},{1,QStringLiteral("CIRCLE")},{2,QStringLiteral("STABILIZE")},
31 {3,QStringLiteral("TRAINING")},{4,QStringLiteral("ACRO")},{5,QStringLiteral("FBWA")},
32 {6,QStringLiteral("FBWB")},{7,QStringLiteral("CRUISE")},{8,QStringLiteral("AUTOTUNE")},
33 {10,QStringLiteral("Auto")},{11,QStringLiteral("RTL")},{12,QStringLiteral("Loiter")},
34 {13,QStringLiteral("TAKEOFF")},{14,QStringLiteral("AVOID_ADSB")},{15,QStringLiteral("Guided")},
35 {17,QStringLiteral("QSTABILIZE")},{18,QStringLiteral("QHOVER")},{19,QStringLiteral("QLOITER")},
36 {20,QStringLiteral("QLAND")},{21,QStringLiteral("QRTL")},{22,QStringLiteral("QAUTOTUNE")},
37 {23,QStringLiteral("QACRO")},{24,QStringLiteral("THERMAL")},{25,QStringLiteral("Loiter to QLand")},
38 {26,QStringLiteral("AUTOLAND")},
39 };
40 static const QHash<int, QString> copterModes = {
41 {0,QStringLiteral("Stabilize")},{1,QStringLiteral("Acro")},{2,QStringLiteral("AltHold")},
42 {3,QStringLiteral("Auto")},{4,QStringLiteral("Guided")},{5,QStringLiteral("Loiter")},
43 {6,QStringLiteral("RTL")},{7,QStringLiteral("Circle")},{9,QStringLiteral("Land")},
44 {11,QStringLiteral("Drift")},{13,QStringLiteral("Sport")},{14,QStringLiteral("Flip")},
45 {15,QStringLiteral("AutoTune")},{16,QStringLiteral("PosHold")},{17,QStringLiteral("Brake")},
46 {18,QStringLiteral("Throw")},{19,QStringLiteral("Avoid_ADSB")},{20,QStringLiteral("Guided_NoGPS")},
47 {21,QStringLiteral("Smart_RTL")},{22,QStringLiteral("FlowHold")},{23,QStringLiteral("Follow")},
48 {24,QStringLiteral("ZigZag")},{25,QStringLiteral("SystemID")},{26,QStringLiteral("Heli_Autorotate")},
49 {27,QStringLiteral("Auto RTL")},{28,QStringLiteral("Turtle")},
50 };
51 if (vehicleType == QStringLiteral("ArduPlane")) {
52 return planeModes.value(modeNumber, QStringLiteral("Mode %1").arg(modeNumber));
53 }
54 if (vehicleType == QStringLiteral("ArduCopter")) {
55 return copterModes.value(modeNumber, QStringLiteral("Mode %1").arg(modeNumber));
56 }
57 return QStringLiteral("Mode %1").arg(modeNumber);
58}
59
60QString _ardupilotErrDescription(int subsystem, int ecode)
61{
62 QString sub;
63 switch (subsystem) {
64 case 1: sub = QCoreApplication::translate("LogFileParser", "Main"); break;
65 case 2: sub = QCoreApplication::translate("LogFileParser", "Radio"); break;
66 case 3: sub = QCoreApplication::translate("LogFileParser", "Compass"); break;
67 case 4: sub = QCoreApplication::translate("LogFileParser", "Optflow"); break;
68 case 5: sub = QCoreApplication::translate("LogFileParser", "Radio Failsafe"); break;
69 case 6: sub = QCoreApplication::translate("LogFileParser", "Battery Failsafe"); break;
70 case 7: sub = QCoreApplication::translate("LogFileParser", "GPS Failsafe"); break;
71 case 8: sub = QCoreApplication::translate("LogFileParser", "GCS Failsafe"); break;
72 case 9: sub = QCoreApplication::translate("LogFileParser", "Fence Failsafe"); break;
73 case 10: sub = QCoreApplication::translate("LogFileParser", "Flight mode"); break;
74 case 11: sub = QCoreApplication::translate("LogFileParser", "GPS"); break;
75 case 12: sub = QCoreApplication::translate("LogFileParser", "Crash Check"); break;
76 case 13: sub = QCoreApplication::translate("LogFileParser", "Flip"); break;
77 case 14: sub = QCoreApplication::translate("LogFileParser", "Autotune"); break;
78 case 15: sub = QCoreApplication::translate("LogFileParser", "Parachute"); break;
79 case 16: sub = QCoreApplication::translate("LogFileParser", "EKF Check"); break;
80 case 17: sub = QCoreApplication::translate("LogFileParser", "EKF Failsafe"); break;
81 case 18: sub = QCoreApplication::translate("LogFileParser", "Barometer"); break;
82 case 19: sub = QCoreApplication::translate("LogFileParser", "CPU Load Watchdog"); break;
83 case 20: sub = QCoreApplication::translate("LogFileParser", "ADSB Failsafe"); break;
84 case 21: sub = QCoreApplication::translate("LogFileParser", "Terrain Data"); break;
85 case 22: sub = QCoreApplication::translate("LogFileParser", "Navigation"); break;
86 case 23: sub = QCoreApplication::translate("LogFileParser", "Terrain Failsafe"); break;
87 case 24: sub = QCoreApplication::translate("LogFileParser", "EKF Primary"); break;
88 case 25: sub = QCoreApplication::translate("LogFileParser", "Thrust Loss Check"); break;
89 case 26: sub = QCoreApplication::translate("LogFileParser", "Sensor Failsafe"); break;
90 case 27: sub = QCoreApplication::translate("LogFileParser", "Leak Failsafe"); break;
91 case 28: sub = QCoreApplication::translate("LogFileParser", "Pilot Input"); break;
92 case 29: sub = QCoreApplication::translate("LogFileParser", "Vibration Failsafe"); break;
93 case 30: sub = QCoreApplication::translate("LogFileParser", "Internal Error"); break;
94 case 31: sub = QCoreApplication::translate("LogFileParser", "Deadreckon Failsafe"); break;
95 default: sub = QCoreApplication::translate("LogFileParser", "Subsystem %1").arg(subsystem); break;
96 }
97 return QStringLiteral("%1 (%2): Code %3").arg(sub).arg(subsystem).arg(ecode);
98}
99
100QString _ardupilotEventDescription(int eventId)
101{
102 switch (eventId) {
103 case 10: return QCoreApplication::translate("LogFileParser", "Armed");
104 case 11: return QCoreApplication::translate("LogFileParser", "Disarmed");
105 case 15: return QCoreApplication::translate("LogFileParser", "Auto Armed");
106 case 17: return QCoreApplication::translate("LogFileParser", "Land Complete Maybe");
107 case 18: return QCoreApplication::translate("LogFileParser", "Land Complete");
108 case 19: return QCoreApplication::translate("LogFileParser", "Lost GPS");
109 case 21: return QCoreApplication::translate("LogFileParser", "Flip Start");
110 case 22: return QCoreApplication::translate("LogFileParser", "Flip End");
111 case 25: return QCoreApplication::translate("LogFileParser", "Set Home");
112 case 26: return QCoreApplication::translate("LogFileParser", "Simple Mode Enabled");
113 case 27: return QCoreApplication::translate("LogFileParser", "Simple Mode Disabled");
114 case 28: return QCoreApplication::translate("LogFileParser", "Not Landed");
115 case 29: return QCoreApplication::translate("LogFileParser", "Super Simple Mode Enabled");
116 case 30: return QCoreApplication::translate("LogFileParser", "AutoTune Initialised");
117 case 31: return QCoreApplication::translate("LogFileParser", "AutoTune Off");
118 case 32: return QCoreApplication::translate("LogFileParser", "AutoTune Restart");
119 case 33: return QCoreApplication::translate("LogFileParser", "AutoTune Success");
120 case 34: return QCoreApplication::translate("LogFileParser", "AutoTune Failed");
121 case 35: return QCoreApplication::translate("LogFileParser", "AutoTune Reached Limit");
122 case 36: return QCoreApplication::translate("LogFileParser", "AutoTune Pilot Testing");
123 case 37: return QCoreApplication::translate("LogFileParser", "AutoTune Saved Gains");
124 case 38: return QCoreApplication::translate("LogFileParser", "Save Trim");
125 case 39: return QCoreApplication::translate("LogFileParser", "Save Waypoint Add");
126 case 41: return QCoreApplication::translate("LogFileParser", "Fence Enabled");
127 case 42: return QCoreApplication::translate("LogFileParser", "Fence Disabled");
128 case 43: return QCoreApplication::translate("LogFileParser", "Acro Trainer Off");
129 case 44: return QCoreApplication::translate("LogFileParser", "Acro Trainer Leveling");
130 case 45: return QCoreApplication::translate("LogFileParser", "Acro Trainer Limited");
131 case 46: return QCoreApplication::translate("LogFileParser", "Gripper Grab");
132 case 47: return QCoreApplication::translate("LogFileParser", "Gripper Release");
133 case 49: return QCoreApplication::translate("LogFileParser", "Parachute Disabled");
134 case 50: return QCoreApplication::translate("LogFileParser", "Parachute Enabled");
135 case 51: return QCoreApplication::translate("LogFileParser", "Parachute Released");
136 case 52: return QCoreApplication::translate("LogFileParser", "Landing Gear Deployed");
137 case 53: return QCoreApplication::translate("LogFileParser", "Landing Gear Retracted");
138 case 54: return QCoreApplication::translate("LogFileParser", "Motors Emergency Stopped");
139 case 55: return QCoreApplication::translate("LogFileParser", "Motors Emergency Stop Cleared");
140 case 56: return QCoreApplication::translate("LogFileParser", "Motors Interlock Disabled");
141 case 57: return QCoreApplication::translate("LogFileParser", "Motors Interlock Enabled");
142 case 58: return QCoreApplication::translate("LogFileParser", "Rotor Runup Complete");
143 case 59: return QCoreApplication::translate("LogFileParser", "Rotor Speed Below Critical");
144 case 60: return QCoreApplication::translate("LogFileParser", "EKF Altitude Reset");
145 case 61: return QCoreApplication::translate("LogFileParser", "Land Cancelled By Pilot");
146 case 62: return QCoreApplication::translate("LogFileParser", "EKF Yaw Reset");
147 case 63: return QCoreApplication::translate("LogFileParser", "ADSB Avoidance Enabled");
148 case 64: return QCoreApplication::translate("LogFileParser", "ADSB Avoidance Disabled");
149 case 65: return QCoreApplication::translate("LogFileParser", "Proximity Avoidance Enabled");
150 case 66: return QCoreApplication::translate("LogFileParser", "Proximity Avoidance Disabled");
151 case 67: return QCoreApplication::translate("LogFileParser", "GPS Primary Changed");
152 case 71: return QCoreApplication::translate("LogFileParser", "ZigZag Store A");
153 case 72: return QCoreApplication::translate("LogFileParser", "ZigZag Store B");
154 case 73: return QCoreApplication::translate("LogFileParser", "Land Repo Active");
155 case 74: return QCoreApplication::translate("LogFileParser", "Standby Enabled");
156 case 75: return QCoreApplication::translate("LogFileParser", "Standby Disabled");
157 case 76: return QCoreApplication::translate("LogFileParser", "Fence Alt Max Enabled");
158 case 77: return QCoreApplication::translate("LogFileParser", "Fence Alt Max Disabled");
159 case 78: return QCoreApplication::translate("LogFileParser", "Fence Circle Enabled");
160 case 79: return QCoreApplication::translate("LogFileParser", "Fence Circle Disabled");
161 case 80: return QCoreApplication::translate("LogFileParser", "Fence Alt Min Enabled");
162 case 81: return QCoreApplication::translate("LogFileParser", "Fence Alt Min Disabled");
163 case 82: return QCoreApplication::translate("LogFileParser", "Fence Polygon Enabled");
164 case 83: return QCoreApplication::translate("LogFileParser", "Fence Polygon Disabled");
165 case 85: return QCoreApplication::translate("LogFileParser", "EK3 Source Set: Primary");
166 case 86: return QCoreApplication::translate("LogFileParser", "EK3 Source Set: Secondary");
167 case 87: return QCoreApplication::translate("LogFileParser", "EK3 Source Set: Tertiary");
168 case 90: return QCoreApplication::translate("LogFileParser", "Airspeed Primary Changed");
169 case 163: return QCoreApplication::translate("LogFileParser", "Surfaced");
170 case 164: return QCoreApplication::translate("LogFileParser", "Not Surfaced");
171 case 165: return QCoreApplication::translate("LogFileParser", "Bottomed");
172 case 166: return QCoreApplication::translate("LogFileParser", "Not Bottomed");
173 default: return QCoreApplication::translate("LogFileParser", "Event %1").arg(eventId);
174 }
175}
176
177double _extractTimestampSeconds(const QMap<QString, QVariant> &values)
178{
179 if (values.contains(QStringLiteral("TimeUS"))) {
180 return values.value(QStringLiteral("TimeUS")).toDouble() / 1000000.0;
181 }
182 if (values.contains(QStringLiteral("TimeMS"))) {
183 return values.value(QStringLiteral("TimeMS")).toDouble() / 1000.0;
184 }
185 if (values.contains(QStringLiteral("Time"))) {
186 return values.value(QStringLiteral("Time")).toDouble() / 1000.0;
187 }
188 return -1.0;
189}
190
191void _appendEvent(QVariantList &events, double timestampSecs, const QString &type, const QString &description)
192{
193 if ((timestampSecs < 0.0) || description.isEmpty()) {
194 return;
195 }
196 QVariantMap eventRow;
197 eventRow[QStringLiteral("time")] = timestampSecs;
198 eventRow[QStringLiteral("type")] = type;
199 eventRow[QStringLiteral("description")] = description;
200 events.append(eventRow);
201}
202
203} // namespace
204
205namespace DataFlashParser {
206
207LogParseResult parseFile(const QString &filePath)
208{
209 LogParseResult result;
210
211 QFile file(filePath);
212 if (!file.open(QIODevice::ReadOnly)) {
213 result.errorMessage = QCoreApplication::translate("LogFileParser", "Failed to open file");
214 return result;
215 }
216
217 const qint64 fileSize = file.size();
218 if (fileSize <= 0) {
219 result.errorMessage = QCoreApplication::translate("LogFileParser", "File is empty");
220 return result;
221 }
222 if (fileSize > std::numeric_limits<qsizetype>::max()) {
223 result.errorMessage = QCoreApplication::translate("LogFileParser", "File is too large to parse");
224 return result;
225 }
226
227 uchar *const mappedData = file.map(0, fileSize);
228 if (mappedData == nullptr) {
229 result.errorMessage = QCoreApplication::translate("LogFileParser", "Failed to memory-map file");
230 return result;
231 }
232
233 struct ScopedUnmap {
234 QFile &file;
235 uchar *data = nullptr;
236 ~ScopedUnmap() { if (data) { file.unmap(data); } }
237 } scopedUnmap{file, mappedData};
238
239 const char *const raw = reinterpret_cast<const char *>(mappedData);
240
241 // Verify DataFlash magic
242 if (fileSize < 3 ||
243 static_cast<uint8_t>(raw[0]) != 0xA3 ||
244 static_cast<uint8_t>(raw[1]) != 0x95) {
245 result.errorMessage = QCoreApplication::translate("LogFileParser", "File does not appear to be a DataFlash log (invalid header)");
246 return result;
247 }
248
249 const QByteArray bytes = QByteArray::fromRawData(raw, static_cast<qsizetype>(fileSize));
250
251 QMap<uint8_t, APMDataFlashUtility::MessageFormat> formats;
252 if (!APMDataFlashUtility::parseFmtMessages(bytes.constData(), bytes.size(), formats)) {
253 result.errorMessage = QCoreApplication::translate("LogFileParser", "No valid FMT messages were found");
254 return result;
255 }
256
257 QSet<QString> fieldSet;
258 QSet<QString> plottableFieldSet;
259 double minTimestampSecs = -1.0;
260 double maxTimestampSecs = -1.0;
261 bool hasOpenModeSegment = false;
262 double modeSegmentStartSecs = -1.0;
263 QString currentModeName;
264
265 QHash<uint8_t, QHash<QString, QString>> fieldNameByCol;
266 fieldNameByCol.reserve(formats.size());
267 for (auto fmtIt = formats.cbegin(); fmtIt != formats.cend(); ++fmtIt) {
268 const APMDataFlashUtility::MessageFormat &fmt = fmtIt.value();
269 QHash<QString, QString> &perCol = fieldNameByCol[fmtIt.key()];
270 perCol.reserve(fmt.columns.size());
271 for (const QString &col : fmt.columns) {
272 perCol.insert(col, fmt.name + QLatin1Char('.') + col);
273 }
274 }
275
276 static const QString kPARM = QStringLiteral("PARM");
277 static const QString kMSG = QStringLiteral("MSG");
278 static const QString kMODE = QStringLiteral("MODE");
279 static const QString kERR = QStringLiteral("ERR");
280 static const QString kEV = QStringLiteral("EV");
281
282 APMDataFlashUtility::iterateMessages(bytes.constData(), bytes.size(), formats,
283 [&](uint8_t msgType, const char *payload, int, const APMDataFlashUtility::MessageFormat &fmt) {
284 const QMap<QString, QVariant> values = APMDataFlashUtility::parseMessage(payload, fmt);
285 const double timestampSecs = _extractTimestampSeconds(values);
286 if (timestampSecs >= 0.0) {
287 if (minTimestampSecs < 0.0 || timestampSecs < minTimestampSecs) { minTimestampSecs = timestampSecs; }
288 maxTimestampSecs = std::max(maxTimestampSecs, timestampSecs);
289 }
290
291 if (fmt.name == kPARM) {
292 const QString paramName = values.value(QStringLiteral("Name")).toString();
293 const QVariant paramValue = values.contains(QStringLiteral("Value"))
294 ? values.value(QStringLiteral("Value"))
295 : values.value(QStringLiteral("Val"));
296 if (!paramName.isEmpty()) {
297 QVariantMap row;
298 row[QStringLiteral("name")] = paramName;
299 row[QStringLiteral("value")] = paramValue;
300 result.parameters.append(row);
301 }
302 } else if (fmt.name == kMSG) {
303 const QString text = values.value(QStringLiteral("Message")).toString();
304 const QString detected = _vehicleTypeFromMessageText(text);
305 if (result.detectedVehicleType.isEmpty() && !detected.isEmpty()) {
306 result.detectedVehicleType = detected;
307 }
308 if (!text.isEmpty()) {
309 QVariantMap row;
310 row[QStringLiteral("time")] = timestampSecs;
311 row[QStringLiteral("text")] = text;
312 result.messages.append(row);
313 }
314 } else if (fmt.name == kMODE) {
315 QString modeName = values.value(QStringLiteral("Mode")).toString();
316 bool isNumeric = false;
317 const int modeNumber = modeName.toInt(&isNumeric);
318 if (isNumeric) {
319 modeName = _ardupilotModeName(result.detectedVehicleType, modeNumber);
320 } else if (modeName.isEmpty()) {
321 modeName = QCoreApplication::translate("LogFileParser", "Unknown");
322 }
323 _appendEvent(result.events, timestampSecs, QStringLiteral("mode"),
324 QCoreApplication::translate("LogFileParser", "Mode: %1").arg(modeName));
325 if (timestampSecs >= 0.0) {
326 if (hasOpenModeSegment && (timestampSecs > modeSegmentStartSecs)) {
327 QVariantMap segment;
328 segment[QStringLiteral("mode")] = currentModeName;
329 segment[QStringLiteral("start")] = modeSegmentStartSecs;
330 segment[QStringLiteral("end")] = timestampSecs;
331 result.modeSegments.append(segment);
332 }
333 hasOpenModeSegment = true;
334 modeSegmentStartSecs = timestampSecs;
335 currentModeName = modeName;
336 }
337 } else if (fmt.name == kERR) {
338 const int subsystem = values.value(QStringLiteral("Subsys")).toInt();
339 const int ecode = values.value(QStringLiteral("ECode")).toInt();
340 _appendEvent(result.events, timestampSecs, QStringLiteral("error"),
341 _ardupilotErrDescription(subsystem, ecode));
342 } else if (fmt.name == kEV) {
343 const int eventId = values.value(QStringLiteral("Id"), values.value(QStringLiteral("Event"))).toInt();
344 _appendEvent(result.events, timestampSecs, QStringLiteral("event"),
345 _ardupilotEventDescription(eventId));
346 }
347
348 result.sampleCount++;
349
350 const QHash<QString, QString> &perCol = fieldNameByCol[msgType];
351 const bool haveTimestamp = (timestampSecs >= 0.0);
352 for (auto it = values.cbegin(); it != values.cend(); ++it) {
353 const auto nameIt = perCol.constFind(it.key());
354 if (nameIt == perCol.constEnd()) { continue; }
355 const QString &fieldName = nameIt.value();
356 fieldSet.insert(fieldName);
357 if (!haveTimestamp) { continue; }
358 const int typeId = it.value().metaType().id();
359 const bool numeric =
360 (typeId == QMetaType::Int) || (typeId == QMetaType::UInt) ||
361 (typeId == QMetaType::LongLong) || (typeId == QMetaType::ULongLong) ||
362 (typeId == QMetaType::Float) || (typeId == QMetaType::Double);
363 if (numeric) {
364 result.fieldSamples[fieldName].append(QPointF(timestampSecs, it.value().toDouble()));
365 plottableFieldSet.insert(fieldName);
366 }
367 }
368 return true;
369 });
370
371 if (hasOpenModeSegment && (maxTimestampSecs >= modeSegmentStartSecs)) {
372 QVariantMap segment;
373 segment[QStringLiteral("mode")] = currentModeName;
374 segment[QStringLiteral("start")] = modeSegmentStartSecs;
375 segment[QStringLiteral("end")] = maxTimestampSecs;
376 result.modeSegments.append(segment);
377 }
378
379 result.availableFields = fieldSet.values();
380 std::sort(result.availableFields.begin(), result.availableFields.end());
381 result.plottableFields = plottableFieldSet.values();
382 std::sort(result.plottableFields.begin(), result.plottableFields.end());
383 result.minTimestamp = minTimestampSecs;
384 result.maxTimestamp = maxTimestampSecs;
385 result.ok = true;
386 return result;
387}
388
389} // namespace DataFlashParser
APMDataFlashUtility::iterateMessages
int iterateMessages(const char *data, qint64 size, const QMap< uint8_t, MessageFormat > &formats, const MessageCallback &callback)
Definition APMDataFlashUtility.cc:275
APMDataFlashUtility::parseFmtMessages
bool parseFmtMessages(const char *data, qint64 size, QMap< uint8_t, MessageFormat > &formats)
Definition APMDataFlashUtility.cc:228
DataFlashParser::parseFile
LogParseResult parseFile(const QString &filePath)
Definition LogViewerDataFlashParser.cc:207