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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL 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.
QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Example implementation of a different protocol than the default MAVLink.
* @author Lorenz Meier <mail@qgroundcontrol.org>
*/
#include "MAVLinkLightProtocol.h"
#include "UASManager.h"
#include "ArduPilotMAV.h"
#include "LinkManager.h"
MAVLinkLightProtocol::MAVLinkLightProtocol() :
MAVLinkProtocol()
{
}
/**
* @param message message to send
*/
void MAVLinkLightProtocol::sendMessage(mavlink_light_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 MAVLinkLightProtocol::sendMessage(LinkInterface* link, mavlink_light_message_t message)
{
// Create buffer
uint8_t buffer[100]; // MAXIMUM PACKET LENGTH, INCLUDING STX BYTES
// Write message into buffer, prepending start sign
//int len = mavlink_msg_to_send_buffer(buffer, &message);
// FIXME TO SEND BUFFER FUNCTION MISSING
Q_UNUSED(message);
int len = 0;
// 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 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 MAVLinkLightProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
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
124
125
126
127
128
129
130
131
132
133
134
135
Q_UNUSED(link);
Q_UNUSED(b);
// 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);
//
// // Run through all bytes
// for (int position = 0; position < bytesToRead; position++)
// {
// mavlink_light_message_t msg;
// // FIXME PARSE
// if (1 == 0/* parsing returned a message */)
// {
//
// int sysid = 0; // system id from message, or always null if only one MAV is supported
// UASInterface* uas = UASManager::instance()->getUASForId(sysid);
//
// // Check and (if necessary) create UAS object
// if (uas == NULL)
// {
// ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists
// // Connect this robot to the UAS object
// // it is IMPORTANT here to use the right object type,
// // else the slot of the parent object is called (and thus the special
// // packets never reach their goal)
// connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), mav, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
// uas = mav;
// // Make UAS aware that this link can be used to communicate with the actual robot
// uas->addLink(link);
// // Now add UAS to "official" list, which makes the whole application aware of it
// UASManager::instance()->addUAS(uas);
// }
//
// // 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, msg);
// }
// }
// receiveMutex.unlock();