QGroundControl
Ground Control Station for MAVLink Drones
Loading...
Searching...
No Matches
APMDataFlashUtility.h
Go to the documentation of this file.
1#pragma once
2
3#include <QtCore/QByteArray>
4#include <QtCore/QMap>
5#include <QtCore/QString>
6#include <QtCore/QStringList>
7#include <QtCore/QVariant>
8
9#include <cstdint>
10#include <functional>
11
12namespace APMDataFlashUtility
13{
14
15// ============================================================================
16// Constants
17// ============================================================================
18
19constexpr uint8_t kHeaderByte1 = 0xA3;
20constexpr uint8_t kHeaderByte2 = 0x95;
21constexpr uint8_t kFmtMessageType = 128;
22constexpr int kFmtPayloadSize = 86;
23
24// ============================================================================
25// Message Format Structure
26// ============================================================================
27
29 uint8_t type = 0;
30 uint8_t length = 0;
31 QString name;
32 QString format;
33 QStringList columns;
34};
35
36// ============================================================================
37// Format Character Functions
38// ============================================================================
39
43int formatCharSize(char c);
44
48int calculatePayloadSize(const QString &format);
49
50// ============================================================================
51// Value Parsing
52// ============================================================================
53
63QVariant parseValue(const char *data, char formatChar);
64
69QMap<QString, QVariant> parseMessage(const char *data, const MessageFormat &fmt);
70
71// ============================================================================
72// Header and Message Detection
73// ============================================================================
74
79bool isValidHeader(const char *data, qint64 size);
80
86qint64 findNextHeader(const char *data, qint64 size, qint64 offset);
87
88// ============================================================================
89// FMT Message Parsing
90// ============================================================================
91
95MessageFormat parseFmtPayload(const char *data);
96
102bool parseFmtMessages(const char *data, qint64 size, QMap<uint8_t, MessageFormat> &formats);
103
104// ============================================================================
105// Message Iteration
106// ============================================================================
107
114using MessageCallback = std::function<bool(uint8_t msgType, const char *payload,
115 int payloadSize, const MessageFormat &fmt)>;
116
123int iterateMessages(const char *data, qint64 size,
124 const QMap<uint8_t, MessageFormat> &formats,
125 const MessageCallback &callback);
126
127// ============================================================================
128// Half-Precision Float Conversion
129// ============================================================================
130
134float halfToFloat(uint16_t bits);
135
136} // namespace APMDataFlashUtility
float halfToFloat(uint16_t bits)
std::function< bool(uint8_t msgType, const char *payload, int payloadSize, const MessageFormat &fmt)> MessageCallback
QMap< QString, QVariant > parseMessage(const char *data, const MessageFormat &fmt)
constexpr uint8_t kHeaderByte2
constexpr uint8_t kFmtMessageType
int iterateMessages(const char *data, qint64 size, const QMap< uint8_t, MessageFormat > &formats, const MessageCallback &callback)
constexpr uint8_t kHeaderByte1
qint64 findNextHeader(const char *data, qint64 size, qint64 offset)
bool isValidHeader(const char *data, qint64 size)
int calculatePayloadSize(const QString &format)
bool parseFmtMessages(const char *data, qint64 size, QMap< uint8_t, MessageFormat > &formats)
constexpr int kFmtPayloadSize
QVariant parseValue(const char *data, char formatChar)
MessageFormat parseFmtPayload(const char *data)