Newer
Older
Beat Küng
committed
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
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2016 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/>.
======================================================================*/
#include "RTCMMavlink.h"
#include "MultiVehicleManager.h"
#include "MAVLinkProtocol.h"
#include "Vehicle.h"
#include <cstdio>
RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox)
: _toolbox(toolbox)
{
_bandwidthTimer.start();
}
void RTCMMavlink::RTCMDataUpdate(QByteArray message)
{
Q_ASSERT(message.size() <= 110); //mavlink message uses a fixed-size buffer
/* statistics */
_bandwidthByteCounter += message.size();
qint64 elapsed = _bandwidthTimer.elapsed();
if (elapsed > 1000) {
printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f);
_bandwidthTimer.restart();
_bandwidthByteCounter = 0;
}
QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
mavlink_gps_inject_data_t mavlinkRtcmData;
memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_inject_data_t));
mavlinkRtcmData.len = message.size();
memcpy(&mavlinkRtcmData.data, message.data(), message.size());
for (int i = 0; i < vehicles.count(); i++) {
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
mavlink_message_t message;
mavlink_msg_gps_inject_data_encode(mavlinkProtocol->getSystemId(),
mavlinkProtocol->getComponentId(), &message, &mavlinkRtcmData);
vehicle->sendMessage(message);
}
}