MAVLinkProtocol.h 11.7 KB
Newer Older
pixhawk's avatar
pixhawk committed
1
2
/*=====================================================================

pixhawk's avatar
pixhawk committed
3
QGroundControl Open Source Ground Control Station
pixhawk's avatar
pixhawk committed
4

pixhawk's avatar
pixhawk committed
5
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
6

pixhawk's avatar
pixhawk committed
7
This file is part of the QGROUNDCONTROL project
pixhawk's avatar
pixhawk committed
8

pixhawk's avatar
pixhawk committed
9
    QGROUNDCONTROL is free software: you can redistribute it and/or modify
pixhawk's avatar
pixhawk committed
10
11
12
13
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

pixhawk's avatar
pixhawk committed
14
    QGROUNDCONTROL is distributed in the hope that it will be useful,
pixhawk's avatar
pixhawk committed
15
16
17
18
19
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
pixhawk's avatar
pixhawk committed
20
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
pixhawk's avatar
pixhawk committed
21
22
23
24
25

======================================================================*/

/**
 * @file
pixhawk's avatar
pixhawk committed
26
27
 *   @brief Definition of class MAVLinkProtocol
 *   @author Lorenz Meier <mail@qgroundcontrol.org>
pixhawk's avatar
pixhawk committed
28
29
30
31
32
33
34
35
36
 */

#ifndef MAVLINKPROTOCOL_H_
#define MAVLINKPROTOCOL_H_

#include <QObject>
#include <QMutex>
#include <QString>
#include <QTimer>
lm's avatar
lm committed
37
#include <QFile>
lm's avatar
lm committed
38
#include <QMap>
pixhawk's avatar
pixhawk committed
39
#include <QByteArray>
40
#include <QLoggingCategory>
Don Gagne's avatar
Don Gagne committed
41

pixhawk's avatar
pixhawk committed
42
#include "LinkInterface.h"
pixhawk's avatar
pixhawk committed
43
#include "QGCMAVLink.h"
44
#include "QGC.h"
Don Gagne's avatar
Don Gagne committed
45
#include "QGCTemporaryFile.h"
46
#include "QGCSingleton.h"
pixhawk's avatar
pixhawk committed
47

48
49
class LinkManager;

50
51
Q_DECLARE_LOGGING_CATEGORY(MAVLinkProtocolLog)

pixhawk's avatar
pixhawk committed
52
/**
pixhawk's avatar
pixhawk committed
53
 * @brief MAVLink micro air vehicle protocol reference implementation.
pixhawk's avatar
pixhawk committed
54
 *
pixhawk's avatar
pixhawk committed
55
56
57
 * MAVLink is a generic communication protocol for micro air vehicles.
 * for more information, please see the official website.
 * @ref http://pixhawk.ethz.ch/software/mavlink/
pixhawk's avatar
pixhawk committed
58
 **/
59
class MAVLinkProtocol : public QGCSingleton
60
{
pixhawk's avatar
pixhawk committed
61
    Q_OBJECT
62
    
63
    DECLARE_QGC_SINGLETON(MAVLinkProtocol, MAVLinkProtocol)
pixhawk's avatar
pixhawk committed
64

65
public:
pixhawk's avatar
pixhawk committed
66
67
    /** @brief Get the human-friendly name of this protocol */
    QString getName();
68
69
70
71
    /** @brief Get the system id of this application */
    int getSystemId();
    /** @brief Get the component id of this application */
    int getComponentId();
pixhawk's avatar
pixhawk committed
72
73
74
    /** @brief The auto heartbeat emission rate in Hertz */
    int getHeartbeatRate();
    /** @brief Get heartbeat state */
75
    bool heartbeatsEnabled() const {
76
        return _heartbeatsEnabled;
77
    }
Don Gagne's avatar
Don Gagne committed
78
    
lm's avatar
lm committed
79
    /** @brief Get protocol version check state */
80
81
82
    bool versionCheckEnabled() const {
        return m_enable_version_check;
    }
83
    /** @brief Get the multiplexing state */
84
85
86
    bool multiplexingEnabled() const {
        return m_multiplexingEnabled;
    }
87
    /** @brief Get the authentication state */
88
89
90
    bool getAuthEnabled() {
        return m_authEnabled;
    }
91
    /** @brief Get the protocol version */
92
93
94
    int getVersion() {
        return MAVLINK_VERSION;
    }
95
    /** @brief Get the auth key */
96
97
98
    QString getAuthKey() {
        return m_authKey;
    }
99
    /** @brief Get state of parameter retransmission */
100
101
102
    bool paramGuardEnabled() {
        return m_paramGuardEnabled;
    }
103
    /** @brief Get parameter read timeout */
104
105
106
    int getParamRetransmissionTimeout() {
        return m_paramRetransmissionTimeout;
    }
107
    /** @brief Get parameter write timeout */
108
109
110
    int getParamRewriteTimeout() {
        return m_paramRewriteTimeout;
    }
111
    /** @brief Get state of action retransmission */
112
113
114
    bool actionGuardEnabled() {
        return m_actionGuardEnabled;
    }
115
    /** @brief Get parameter read timeout */
116
117
118
    int getActionRetransmissionTimeout() {
        return m_actionRetransmissionTimeout;
    }
119
120
121
122
    /**
     * Retrieve a total of all successfully parsed packets for the specified link.
     * @returns -1 if this is not available for this protocol, # of packets otherwise.
     */
123
    qint32 getReceivedPacketCount(const LinkInterface *link) const {
124
        return totalReceiveCounter[link->getMavlinkChannel()];
125
126
127
128
129
    }
    /**
     * Retrieve a total of all parsing errors for the specified link.
     * @returns -1 if this is not available for this protocol, # of errors otherwise.
     */
130
    qint32 getParsingErrorCount(const LinkInterface *link) const {
131
        return totalErrorCounter[link->getMavlinkChannel()];
132
133
134
135
136
    }
    /**
     * Retrieve a total of all dropped packets for the specified link.
     * @returns -1 if this is not available for this protocol, # of packets otherwise.
     */
137
    qint32 getDroppedPacketCount(const LinkInterface *link) const {
138
        return totalLossCounter[link->getMavlinkChannel()];
139
    }
140
141
142
143
    /**
     * Reset the counters for all metadata for this link.
     */
    virtual void resetMetadataForLink(const LinkInterface *link);
Don Gagne's avatar
Don Gagne committed
144
    
145
146
    /// Suspend/Restart logging during replay.
    void suspendLogForReplay(bool suspend);
Lorenz Meier's avatar
Lorenz Meier committed
147

pixhawk's avatar
pixhawk committed
148
149
public slots:
    /** @brief Receive bytes from a communication interface */
150
    void receiveBytes(LinkInterface* link, QByteArray b);
151
152
153
154
    
    void linkConnected(void);
    void linkDisconnected(void);
    
pixhawk's avatar
pixhawk committed
155
156
    /** @brief Send MAVLink message through serial interface */
    void sendMessage(mavlink_message_t message);
Lorenz Meier's avatar
Lorenz Meier committed
157
    /** @brief Send MAVLink message */
pixhawk's avatar
pixhawk committed
158
    void sendMessage(LinkInterface* link, mavlink_message_t message);
Lorenz Meier's avatar
Lorenz Meier committed
159
160
    /** @brief Send MAVLink message with correct system / component ID */
    void sendMessage(LinkInterface* link, mavlink_message_t message, quint8 systemid, quint8 componentid);
pixhawk's avatar
pixhawk committed
161
162
    /** @brief Set the rate at which heartbeats are emitted */
    void setHeartbeatRate(int rate);
163
164
    /** @brief Set the system id of this application */
    void setSystemId(int id);
pixhawk's avatar
pixhawk committed
165
166
167
168

    /** @brief Enable / disable the heartbeat emission */
    void enableHeartbeats(bool enabled);

169
170
171
    /** @brief Enabled/disable packet multiplexing */
    void enableMultiplexing(bool enabled);

172
173
174
    /** @brief Enable / disable parameter retransmission */
    void enableParamGuard(bool enabled);

175
176
177
    /** @brief Enable / disable action retransmission */
    void enableActionGuard(bool enabled);

178
179
180
181
182
183
    /** @brief Set parameter read timeout */
    void setParamRetransmissionTimeout(int ms);

    /** @brief Set parameter write timeout */
    void setParamRewriteTimeout(int ms);

184
185
186
    /** @brief Set parameter read timeout */
    void setActionRetransmissionTimeout(int ms);

lm's avatar
lm committed
187
188
189
    /** @brief Enable / disable version check */
    void enableVersionCheck(bool enabled);

190
191
192
193
    /** @brief Enable / disable authentication */
    void enableAuth(bool enable);

    /** @brief Set authentication token */
194
195
196
    void setAuthKey(QString key) {
        m_authKey = key;
    }
197

pixhawk's avatar
pixhawk committed
198
199
200
    /** @brief Send an extra heartbeat to all connected units */
    void sendHeartbeat();

201
202
203
204
    /** @brief Load protocol settings */
    void loadSettings();
    /** @brief Store protocol settings */
    void storeSettings();
Don Gagne's avatar
Don Gagne committed
205
    
Don Gagne's avatar
Don Gagne committed
206
207
    /// @brief Deletes any log files which are in the temp directory
    static void deleteTempLogFiles(void);
208
209
210
    
    /// Checks for lost log files
    void checkForLostLogFiles(void);
Don Gagne's avatar
Don Gagne committed
211

212
protected:
213
    bool m_multiplexingEnabled; ///< Enable/disable packet multiplexing
214
215
    bool m_authEnabled;        ///< Enable authentication token broadcast
    QString m_authKey;         ///< Authentication key
lm's avatar
lm committed
216
    bool m_enable_version_check; ///< Enable checking of version match of MAV and QGC
217
218
    int m_paramRetransmissionTimeout; ///< Timeout for parameter retransmission
    int m_paramRewriteTimeout;    ///< Timeout for sending re-write request
219
220
221
    bool m_paramGuardEnabled;       ///< Parameter retransmission/rewrite enabled
    bool m_actionGuardEnabled;       ///< Action request retransmission enabled
    int m_actionRetransmissionTimeout; ///< Timeout for parameter retransmission
222
223
224
225
226
227
228
    QMutex receiveMutex;        ///< Mutex to protect receiveBytes function
    int lastIndex[256][256];    ///< Store the last received sequence ID for each system/componenet pair
    int totalReceiveCounter[MAVLINK_COMM_NUM_BUFFERS];    ///< The total number of successfully received messages
    int totalLossCounter[MAVLINK_COMM_NUM_BUFFERS];       ///< Total messages lost during transmission.
    int totalErrorCounter[MAVLINK_COMM_NUM_BUFFERS];      ///< Total count of all parsing errors. Generally <= totalLossCounter.
    int currReceiveCounter[MAVLINK_COMM_NUM_BUFFERS];     ///< Received messages during this sample time window. Used for calculating loss %.
    int currLossCounter[MAVLINK_COMM_NUM_BUFFERS];        ///< Lost messages during this sample time window. Used for calculating loss %.
229
    bool versionMismatchIgnore;
230
    int systemId;
231

pixhawk's avatar
pixhawk committed
232
233
234
235
236
signals:
    /** @brief Message received and directly copied via signal */
    void messageReceived(LinkInterface* link, mavlink_message_t message);
    /** @brief Emitted if heartbeat emission mode is changed */
    void heartbeatChanged(bool heartbeats);
237
238
    /** @brief Emitted if multiplexing is started / stopped */
    void multiplexingChanged(bool enabled);
239
240
241
242
    /** @brief Emitted if authentication support is enabled / disabled */
    void authKeyChanged(QString key);
    /** @brief Authentication changed */
    void authChanged(bool enabled);
lm's avatar
lm committed
243
244
    /** @brief Emitted if version check is enabled / disabled */
    void versionCheckChanged(bool enabled);
245
246
    /** @brief Emitted if a message from the protocol should reach the user */
    void protocolStatusMessage(const QString& title, const QString& message);
247
248
249
250
251
252
253
254
    /** @brief Emitted if a new system ID was set */
    void systemIdChanged(int systemId);
    /** @brief Emitted if param guard status changed */
    void paramGuardChanged(bool enabled);
    /** @brief Emitted if param read timeout changed */
    void paramRetransmissionTimeoutChanged(int ms);
    /** @brief Emitted if param write timeout changed */
    void paramRewriteTimeoutChanged(int ms);
255
256
    /** @brief Emitted if action guard status changed */
    void actionGuardChanged(bool enabled);
dogmaphobic's avatar
dogmaphobic committed
257
    /** @brief Emitted if action request timeout changed */
258
    void actionRetransmissionTimeoutChanged(int ms);
259
260
261
    /** @brief Update the packet loss from one system */
    void receiveLossChanged(int uasId, float loss);

262
263
264
265
266
267
268
269
270
271
272
273
274
    /**
     * @brief Emitted if a new radio status packet received
     *
     * @param rxerrors receive errors
     * @param fixed count of error corrected packets
     * @param rssi local signal strength
     * @param remrssi remote signal strength
     * @param txbuf how full the tx buffer is as a percentage
     * @param noise background noise level
     * @param remnoise remote background noise level
     */
    void radioStatusChanged(LinkInterface* link, unsigned rxerrors, unsigned fixed, unsigned rssi, unsigned remrssi,
    unsigned txbuf, unsigned noise, unsigned remnoise);
Don Gagne's avatar
Don Gagne committed
275
276
277
278
279
    
    /// @brief Emitted when a temporary log file is ready for saving
    void saveTempFlightDataLog(QString tempLogfile);
    
private:
280
281
282
    MAVLinkProtocol(QObject* parent = NULL);
    ~MAVLinkProtocol();

283
    void _linkStatusChanged(LinkInterface* link, bool connected);
Don Gagne's avatar
Don Gagne committed
284
285
286
287
    bool _closeLogFile(void);
    void _startLogging(void);
    void _stopLogging(void);
    
288
289
290
291
    /// List of all links connected to protocol. We keep SharedLinkInterface objects
    /// which are QSharedPointer's in order to maintain reference counts across threads.
    /// This way Link deletion works correctly.
    QList<SharedLinkInterface> _connectedLinks;
Don Gagne's avatar
Don Gagne committed
292
293
294
    
    bool _logSuspendError;      ///< true: Logging suspended due to error
    bool _logSuspendReplay;     ///< true: Logging suspended due to replay
Don Gagne's avatar
Don Gagne committed
295
    bool _logWasArmed;          ///< true: vehicle was armed during logging
Don Gagne's avatar
Don Gagne committed
296
    
Don Gagne's avatar
Don Gagne committed
297
298
299
    QGCTemporaryFile    _tempLogFile;            ///< File to log to
    static const char*  _tempLogFileTemplate;    ///< Template for temporary log file
    static const char*  _logFileExtension;       ///< Extension for log files
Don Gagne's avatar
Don Gagne committed
300
    
301
    LinkManager* _linkMgr;
302
303
304
305
    
    QTimer  _heartbeatTimer;    ///< Timer to emit heartbeats
    int     _heartbeatRate;     ///< Heartbeat rate, controls the timer interval
    bool    _heartbeatsEnabled; ///< Enabled/disable heartbeat emission
pixhawk's avatar
pixhawk committed
306
307
308
};

#endif // MAVLINKPROTOCOL_H_