Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
/*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
Please see our website at <http://pixhawk.ethz.ch>
(c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
This file is part of the PIXHAWK project
PIXHAWK is free software: you can redistribute it and/or modify
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 is distributed in the hope that it will be useful,
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
along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Implementation of the MAVLink protocol
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <inttypes.h>
#include <iostream>
#include <QDebug>
#include <QTime>
#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "configuration.h"
#include "LinkManager.h"
#include <mavlink.h>
/**
* The default constructor will create a new MAVLink object sending heartbeats at
* the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
*/
MAVLinkProtocol::MAVLinkProtocol() :
heartbeatTimer(new QTimer(this)),
heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
m_heartbeatsEnabled(false)
{
start(QThread::LowPriority);
// Start heartbeat timer, emitting a heartbeat at the configured rate
connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
heartbeatTimer->start(1000/heartbeatRate);
totalReceiveCounter = 0;
totalLossCounter = 0;
for (int i = 0; i < 256; i++)
{
for (int j = 0; j < 256; j++)
{
lastIndex[i][j] = -1;
}
}
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
}
MAVLinkProtocol::~MAVLinkProtocol()
{
}
void MAVLinkProtocol::run()
{
}
/**
* The bytes are copied by calling the LinkInterface::readBytes() method.
* This method parses all incoming bytes and constructs a MAVLink packet.
* It can handle multiple links in parallel, as each link has it's own buffer/
* parsing state machine.
* @param link The interface to read from
* @see LinkInterface
**/
void MAVLinkProtocol::receiveBytes(LinkInterface* link)
{
receiveMutex.lock();
// Prepare buffer
static const int maxlen = 4096 * 100;
static char buffer[maxlen];
qint64 bytesToRead = link->bytesAvailable();
// Get all data at once, let link read the bytes in the buffer array
link->readBytes(buffer, maxlen);
mavlink_message_t message;
mavlink_status_t status;
for (int position = 0; position < bytesToRead; position++)
{
unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)*(buffer + position), &message, &status);
if (decodeState == 1)
{
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
// Check and (if necessary) create UAS object
if (uas == NULL)
{
// ORDER MATTERS HERE!
// The UAS object has first to be created and connected,
// only then the rest of the application can be made aware
// of its existence, as it only then can send and receive
// it's first messages.
// First create new UAS object
// Make UAS aware that this link can be used to communicate with the actual robot
uas->addLink(link);
// Connect this robot to the UAS object
connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
// Now add UAS to "official" list, which makes the whole application aware of it
UASManager::instance()->addUAS(uas);
}
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
// Increase receive counter
totalReceiveCounter++;
qint64 lastLoss = totalLossCounter;
// Update last packet index
if (lastIndex[message.sysid][message.compid] == -1)
{
lastIndex[message.sysid][message.compid] = message.seq;
}
else
{
int safeguard = 0;
while(lastIndex[message.sysid][message.compid]+1 != message.seq && safeguard < 100)
{
lastIndex[message.sysid][message.compid] += 1;
totalLossCounter++;
safeguard++;
}
}
// if (lastIndex.contains(message.sysid))
// {
// QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
// if (lastCompIndex->contains(message.compid))
// while (lastCompIndex->value(message.compid, 0)+1 )
// }
//if ()
if (lastLoss != totalLossCounter)
{
// Calculate new loss ratio
// Receive loss
float receiveLoss = (double)totalLossCounter/(double)(totalReceiveCounter+totalLossCounter);
receiveLoss *= 100.0f;
emit receiveLossChanged(receiveLoss);
}
// The packet is emitted as a whole, as it is only 255 - 261 bytes short
// kind of inefficient, but no issue for a groundstation pc.
// It buys as reentrancy for the whole code over all threads
emit messageReceived(link, message);
}
}
receiveMutex.unlock();
}
/**
* @return The name of this protocol
**/
QString MAVLinkProtocol::getName()
{
return QString(tr("MAVLink protocol"));
}
/** @return System id of this application */
int MAVLinkProtocol::getSystemId()
{
return MG::SYSTEM::ID;
}
/** @return Component id of this application */
int MAVLinkProtocol::getComponentId()
{
return MG::SYSTEM::COMPID;
}
/**
* @param message message to send
*/
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
// Get all links connected to this unit
QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);
// Emit message on all links that are currently connected
QList<LinkInterface*>::iterator i;
for (i = links.begin(); i != links.end(); ++i)
{
sendMessage(*i, message);
}
}
/**
* @param link the link to send the message over
* @param message message to send
*/
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
// Create buffer
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// Write message into buffer, prepending start sign
int len = mavlink_msg_to_send_buffer(buffer, &message);
// If link is connected
if (link->isConnected())
{
// Send the portion of the buffer now occupied by the message
link->writeBytes((const char*)buffer, len);
}
}
/**
* The heartbeat is sent out of order and does not reset the
* periodic heartbeat emission. It will be just sent in addition.
* @return mavlink_message_t heartbeat message sent on serial link
*/
void MAVLinkProtocol::sendHeartbeat()
{
if (m_heartbeatsEnabled)
{
mavlink_message_t beat;
mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU);
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
sendMessage(beat);
}
}
/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
m_heartbeatsEnabled = enabled;
emit heartbeatChanged(enabled);
}
bool MAVLinkProtocol::heartbeatsEnabled(void)
{
return m_heartbeatsEnabled;
}
/**
* The default rate is 1 Hertz.
*
* @param rate heartbeat rate in hertz (times per second)
*/
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
heartbeatRate = rate;
heartbeatTimer->setInterval(1000/heartbeatRate);
}
/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
return heartbeatRate;
}